livox_loam第二部分详解2.1

二、位姿匹配,建图

1、laser_mapping.cpp

1、主函数构造,构建laserMapping节点,调用Laser_mapping类中的laser_mapping函数,和process函数

2、laser_mapping.hpp

包含:

1.struct Data_pair

struct Data_pair
{
    sensor_msgs::PointCloud2ConstPtr m_pc_corner;
    sensor_msgs::PointCloud2ConstPtr m_pc_full;
    sensor_msgs::PointCloud2ConstPtr m_pc_plane;
    bool m_has_pc_corner = 0;
    bool m_has_pc_full = 0;
    bool m_has_pc_plane = 0;
//-------------------------------后面回调函数会调用------------------------------------
    void add_pc_corner(sensor_msgs::PointCloud2ConstPtr ros_pc)
    {
        m_pc_corner = ros_pc;
        m_has_pc_corner = true;
    }
​
    void add_pc_plane(sensor_msgs::PointCloud2ConstPtr ros_pc)
    {
        m_pc_plane = ros_pc;
        m_has_pc_plane = true;
    }
​
    void add_pc_full(sensor_msgs::PointCloud2ConstPtr ros_pc)
    {
        m_pc_full = ros_pc;
        m_has_pc_full = true;
    }
​
    bool is_completed()
    {
        return (m_has_pc_corner & m_has_pc_full & m_has_pc_plane);
    }
};

2、class Point_cloud_registration

3、class Laser_mapping

1、进入laser_mapping函数

1)定义了一些点云指针变量并对

// 初始化点云类型
m_laser_cloud_corner_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
m_laser_cloud_surf_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
m_laser_cloud_surround = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
​
m_laser_cloud_corner_from_map = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
m_laser_cloud_surf_from_map = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
​
m_laser_cloud_corner_from_map_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
m_laser_cloud_surf_from_map_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
​
m_laser_cloud_full_res = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>());
​
//初始化ros参数
init_parameters(m_ros_node_handle);

2)接收来自上一节点的角点、面点和所有点,调用回调函数

// livox_corners 每帧的corners, plannes, corners + plannes
m_sub_laser_cloud_corner_last = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_corners", 10000, &Laser_mapping::laserCloudCornerLastHandler, this);
m_sub_laser_cloud_surf_last = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_surface", 10000, &Laser_mapping::laserCloudSurfLastHandler, this);
m_sub_laser_cloud_full_res = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_full", 10000, &Laser_mapping::laserCloudFullResHandler, this);

回调函数laserCloudCornerLastHandler,开启多线程

    // 把三个回调函数中的laser_data合并起来,压入到 m_queue_avail_data
    void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
    {
        std::unique_lock<std::mutex> lock(m_mutex_buf);
        Data_pair *data_pair = get_data_pair(laserCloudCornerLast2->header.stamp.toSec()); // NOTE pair the corner data with other feature in same timestamp注:将角数据与相同时间戳的其他特征配对  
        data_pair->add_pc_corner(laserCloudCornerLast2);                                   // TODO if two corner cloud came within 1 sec,and both not processed yet, the first is overwrited??
        if (data_pair->is_completed())
        {
            m_queue_avail_data.push(data_pair);//把数据对压入队列中
        }
    }

主要部分是三个回调函数:get_data_pair比较时间戳,找到匹配的时间戳后输出数据对data_pair -->用到最一开始的struct Data_pair -->将数据对压入队列m_queue_avail_data中(-->process进程中处理数据)

队列m_queue_avail_data同时保存了

m_pc_corner

m_pc_plane

m_pc_full

3)发布消息

 // 每隔100帧发布当前帧周围1000m范围内降采样后的points
 m_pub_laser_cloud_surround = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 10000);
​
 // 每一帧corners在map下的points,pose graph 更新前的地图
 m_pub_last_corner_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/features_corners", 10000);
// 每一帧plannes在map下的points,pose graph 更新前的地图
 m_pub_last_surface_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/features_surface", 10000);
​
 m_pub_match_corner_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/match_pc_corners", 10000);
// local map corners
m_pub_match_surface_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/match_pc_surface", 10000);
// local map plannes
m_pub_pc_aft_loop = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/pc_aft_loop_closure", 10000);
// 每有一次闭环用pose graph更新后的poses,对地图的各个小部分进行更新,也就是重新计算在map下的points
 m_pub_debug_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/pc_debug", 10000);
// topic上的数据为空
​
// 没用到
 m_pub_laser_cloud_map = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 10000);
m_pub_laser_cloud_full_res = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 10000);
// 每一帧corners + plannes 在map下的points, pose graph 更新前的地图
m_pub_odom_aft_mapped = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 10000);
// 与local map 匹配得到的每帧的位姿
// 没用到
 m_pub_odom_aft_mapped_hight_frec = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 10000);
// 每隔10帧发布全部与local map 匹配得到的每帧的位姿
m_pub_laser_aft_mapped_path = m_ros_node_handle.advertise<nav_msgs::Path>("/aft_mapped_path", 10000);
 m_pub_laser_aft_loopclosure_path = m_ros_node_handle.advertise<nav_msgs::Path>("/aft_loopclosure_path", 10000);
// 每有一次闭环就会计算一次pose graph, 然后把计算后的位姿发布到该topic

2、进入process函数

1)m_service_pub_surround_pts,发布周围点

m_service_pub_surround_pts = new std::future<void>(std::async(std::launch::async, &Laser_mapping::service_pub_surround_pts, this));

a) 这里调用了service_pub_surround_pts函数,每隔100帧发布当前帧周围1000m范围内降采样后的points

//创建滤波对象,格子大小为0.5
pcl::VoxelGrid<PointType> down_sample_filter_surface;
down_sample_filter_surface.setLeafSize(m_surround_pointcloud_resolution, m_surround_pointcloud_resolution, m_surround_pointcloud_resolution);
//定义变量
pcl::PointCloud<PointType> pc_temp;
sensor_msgs::PointCloud2 ros_laser_cloud_surround;//转换成ROS形式发布周围点
//
std::this_thread::sleep_for(std::chrono::nanoseconds(10));//10纳秒,0.000,01 ms
​
pcl::PointCloud<PointType>::Ptr laser_cloud_surround(new pcl::PointCloud<PointType>()); // 当前帧周围的map cells(corners + plannes)
laser_cloud_surround->reserve(1e8);//预先设定容量到指定值
​
std::this_thread::sleep_for(std::chrono::milliseconds(1000));   ////睡眠1000毫秒(1秒)
int last_update_index = 0;

注:引用了std::this_thread::sleep_for函数是C11的休眠函数,表示当前线程休眠一段时间,休眠期间不与其他线程竞争CPU,根据线程需求,等待若干时间。

b) 进入while循环

 while (1)
        {
            while (m_current_frame_index - last_update_index < 100) // 每100帧发布一次
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(1500));//休眠1.5s
            }
​
            last_update_index = m_current_frame_index;//100帧之后开始发布,当前帧索引为最新更新
            pcl::PointCloud<PointType> pc_temp;
            laser_cloud_surround->clear();
            if (m_pt_cell_map_full.get_cells_size() == 0)//判断是否有单元格
                continue;
            //寻找半径范围内的单元格
            std::vector<Points_cloud_map<float>::Mapping_cell_ptr> cell_vec = m_pt_cell_map_full.find_cells_in_radius(m_t_w_curr, 1000.0);
            // 获得当前帧周围的map cells(corners + plannes)
            for (size_t i = 0; i < cell_vec.size(); i++)
            {   //如果需要体素滤波
                if (m_down_sample_replace)
                {//对第i个单元格的点进行体素滤波并存放到pc_temp当中
                    down_sample_filter_surface.setInputCloud(cell_vec[i]->get_pointcloud().makeShared());
                    down_sample_filter_surface.filter(pc_temp);
                    *laser_cloud_surround += pc_temp;
                }
                else
                {
                    *laser_cloud_surround += cell_vec[i]->get_pointcloud();
                }
            }
            if (laser_cloud_surround->points.size())//发布周围点
            {
                down_sample_filter_surface.setInputCloud(laser_cloud_surround);
                down_sample_filter_surface.filter(*laser_cloud_surround);
                pcl::toROSMsg(*laser_cloud_surround, ros_laser_cloud_surround);
                ros_laser_cloud_surround.header.stamp = ros::Time::now();
                ros_laser_cloud_surround.header.frame_id = "camera_init";
                m_pub_laser_cloud_surround.publish(ros_laser_cloud_surround);
            }
        }
    }

1.m_current_frame_index为当前帧索引

2.m_pt_cell_map_full为全部帧在map的下标

2) 这里没有开启回环检测 m_loop_closure_if_enable=0

 if (m_loop_closure_if_enable)
{
    m_service_loop_detection = new std::future<void>(std::async(std::launch::async, &Laser_mapping::service_loop_detection, this));
}

3)进入while循环

a) 如果队列为空,休眠0.001s

 while (m_queue_avail_data.empty())
 {
      sleep(0.0001);
 }

b) 判断队列中的数据是否超过最大限制,如果超过。依次删除

 while (m_queue_avail_data.size() >= (unsigned int)m_max_buffer_size) // 如果队列中数据超过限制,依次删除队列中的第一个元素直到数量达到要求
{
     ROS_WARN("Drop lidar frame in mapping for real time performance !!!");
     (*m_logger_common.get_ostream()) << "Drop lidar frame in mapping for real time performance !!!" << endl;
      m_queue_avail_data.pop();
}

c) 从队列的第一项开始处理

// 从队列的最开始处理,将队列中的第一项赋值给current_data_pair,后续将对current_data_pair进行处理,同时删除队列中的这个数据以便于下次循环的进行
Data_pair *current_data_pair = m_queue_avail_data.front(); 
m_queue_avail_data.pop();
m_mutex_buf.unlock();
//时间戳
m_time_pc_corner_past = current_data_pair->m_pc_corner->header.stamp.toSec();

d) 给队列的第一项赋予时间戳 first_time_stamp

if (first_time_stamp < 0)
{
     first_time_stamp = m_time_pc_corner_past;
}
(*m_logger_common.get_ostream()) << "Messgage time stamp = " << m_time_pc_corner_past - first_time_stamp << endl;

e) 将current_data_pair中的ROS消息转换为点云信息

m_mutex_querypointcloud.lock(); 不懂

m_mutex_querypointcloud.unlock();

//与多线程进程读取Laser_mapping::process_new_scan中的点相关
m_mutex_querypointcloud.lock(); 
m_laser_cloud_corner_last->clear();
//将current_data_pair中的ROS消息转换为点云信息
pcl::fromROSMsg(*(current_data_pair->m_pc_corner), *m_laser_cloud_corner_last);
m_laser_cloud_surf_last->clear();
pcl::fromROSMsg(*(current_data_pair->m_pc_plane), *m_laser_cloud_surf_last);
​
m_laser_cloud_full_res->clear();
pcl::fromROSMsg(*(current_data_pair->m_pc_full), *m_laser_cloud_full_res);
m_mutex_querypointcloud.unlock();
​
delete current_data_pair;//转换后删除当前数据

f) 维护最大线程池[线程的部分还不太懂]

std::liststd::future<int > m_thread_pool; m_maximum_parallel_thread = 2;

Common_tools::maintain_maximum_thread_pool<std::future<int> *>(m_thread_pool, m_maximum_parallel_thread);

调用了common_tool中的maintain_maximum_thread_pool函数

g) 开启process_new_scan线程

std::future<int> *thd = new std::future<int>(std::async(std::launch::async, &Laser_mapping::process_new_scan, this));

3、进入process_new_scan函数

1、记录本帧数据中的最大最小时间戳,这里的intensity存放的是时间

float min_t, max_t;                         // 本帧数据中最小,最大时间戳
find_min_max_intensity(current_laser_cloud_full.makeShared(), min_t, max_t); // 点的intensity存放的是时间戳,找到当前帧的最大最小时间戳,是为了后面调用refine_blur消除畸变

2、时间戳的转换,帧间转换

m_time_odom = m_last_time_stamp;  //m_last_time_stamp的初始为0
//这里的m_last_time_stamp是在上一帧处理时的最大时间戳,也是当前帧的最小时间戳
m_minimum_pt_time_stamp = m_last_time_stamp;
m_maximum_pt_time_stamp = max_t;
//这里将max_t赋值给m_last_time_stamp是为了进行下一帧处理时充当下一帧的最小时间戳
m_last_time_stamp = max_t;

3、调用Point_cloud_registration类,调用init_pointcloud_registration函数【不太懂】

local map与当前帧匹配前,设置注册参数

用定义好的类创建一个对象 ,类的使用 pc_reg就是我们使用的类

Point_cloud_registration pc_reg; ////用定义好的类创建一个对象 pc_reg
init_pointcloud_registration(pc_reg); // local map与当前帧匹配前,set registration params
m_current_frame_index++;

4、对数据进行滤波降采样

//对数据进行滤波降采样并存储在下方
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
//对数据进行滤波降采样
if (m_if_input_downsample_mode)//=1
{
            down_sample_filter_corner.setInputCloud(current_laser_cloud_corner_last.makeShared());
down_sample_filter_corner.filter(*laserCloudCornerStack);
           down_sample_filter_surface.setInputCloud(current_laser_cloud_surf_last.makeShared());
down_sample_filter_surface.filter(*laserCloudSurfStack);
}
else
{
    *laserCloudCornerStack = current_laser_cloud_corner_last; // NOTE 调取上一轮的边角点
    *laserCloudSurfStack = current_laser_cloud_surf_last;
}
​
int laser_corner_pt_num = laserCloudCornerStack->points.size();
int laser_surface_pt_num = laserCloudSurfStack->points.size();

5、保存数据:当前帧所有点云和时间戳

//保存数据
if (m_if_save_to_pcd_files && PCD_SAVE_RAW)
{
    m_pcl_tools_raw.save_to_pcd_files("raw", current_laser_cloud_full, m_current_frame_index);   
}

6、位姿,在3中对世界坐标系和map坐标系的位姿进行定义,包括旋转和平移

m_q_w_last = m_q_w_curr; // NOTE 保存当前位姿
m_t_w_last = m_t_w_curr;

7、建立面点,角点的地图指针和kdtree

pcl::PointCloud<PointType>::Ptr laser_cloud_corner_from_map(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laser_cloud_surf_from_map(new pcl::PointCloud<PointType>());
int reg_res = 0;
//历史地图数据=============================这里互斥锁的设置???
m_mutex_buff_for_matching_corner.lock();
*laser_cloud_corner_from_map = *m_laser_cloud_corner_from_map_last; // local map corners
kdtree_corner_from_map = m_kdtree_corner_from_map_last;  // local_map_kdtree_corners
m_mutex_buff_for_matching_surface.unlock();
​
m_mutex_buff_for_matching_surface.lock();
*laser_cloud_surf_from_map = *m_laser_cloud_surf_from_map_last; // local map plannes
kdtree_surf_from_map = m_kdtree_surf_from_map_last;             // local_map_kdtree_plannes
m_mutex_buff_for_matching_corner.unlock();

8、调用了上述reg_res类中的find_out_incremental_transfrom函数,输入的是角地图,面地图,及其kdtree,当前帧的角点和面点,这里是匹配点查询和位姿优化

reg_res = pc_reg.find_out_incremental_transfrom(laser_cloud_corner_from_map, laser_cloud_surf_from_map, // local map
kdtree_corner_from_map, kdtree_surf_from_map,           // local map kdtree
laserCloudCornerStack, laserCloudSurfStack);            // 当前帧corners, plannes
​

1)位姿(旋转和平移)增量

Eigen::Map<Eigen::Quaterniond> q_w_incre = Eigen::Map<Eigen::Quaterniond>(m_para_buffer_incremental);
Eigen::Map<Eigen::Vector3d> t_w_incre = Eigen::Map<Eigen::Vector3d>(m_para_buffer_incremental + 4);
        // <1>: 用上一帧end到当前帧end TF 作为初值
        // <2>: incre_q = I, incre_t = T, T:curr_finished_keyframe_center - history_finished_keyframe_center
        // <2>: curr_q = I,  curr_t = T   在scene_alignment.hpp中设置的初值
        // <2>: last_q = I,  last_t = 0

2)变量定义赋值

m_kdtree_corner_from_map = kdtree_corner_from_map;
m_kdtree_surf_from_map = kdtree_surf_from_map;
​
pcl::PointCloud<PointType> laser_cloud_corner_from_map = *in_laser_cloud_corner_from_map;
pcl::PointCloud<PointType> laser_cloud_surf_from_map = *in_laser_cloud_surf_from_map;
​
int laserCloudCornerFromMapNum = laser_cloud_corner_from_map.points.size();
int laserCloudSurfFromMapNum = laser_cloud_surf_from_map.points.size();
int laser_corner_pt_num = laserCloudCornerStack->points.size();
int laser_surface_pt_num = laserCloudSurfStack->points.size();
​
int surf_avail_num = 0;
int corner_avail_num = 0;
float minimize_cost = summary.final_cost;
PointType pointOri, pointSel;
int corner_rejection_num = 0;
int surface_rejecetion_num = 0;
int if_undistore_in_matching = 1;

3)位姿转换,消除畸变

最开始的前50帧数据,poses默认m_q_w_curr = I, m_t_w_curr = 0

之后每帧的位姿都是通过与local map 匹配得到

if (laserCloudCornerFromMapNum > CORNER_MIN_MAP_NUM && laserCloudSurfFromMapNum > SURFACE_MIN_MAP_NUM && m_current_frame_index > m_mapping_init_accumulate_frames)
​

a)求解位姿,最大迭代次数20

m_timer->tic("Build kdtree");
​
*(m_logger_timer->get_ostream()) << m_timer->toc_string("Build kdtree") << std::endl;
Eigen::Quaterniond q_last_optimize(1.f, 0.f, 0.f, 0.f);
Eigen::Vector3d t_last_optimize(0.f, 0.f, 0.f);
int iterCount = 0;
​
std::vector<int> m_point_search_Idx;
std::vector<float> m_point_search_sq_dis;
//最大迭代次数20
for (iterCount = 0; iterCount < m_para_icp_max_iterations; iterCount++) // 计算位姿时, yaml: 15; loop closure, yaml: 5
 {

b)定义相关变量,和求解的ceres模块

m_point_search_Idx.clear();
m_point_search_sq_dis.clear();
corner_avail_num = 0;
surf_avail_num = 0;
corner_rejection_num = 0;
surface_rejecetion_num = 0;
​
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);//核函数
ceres::LocalParameterization *q_parameterization = new      ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::ResidualBlockId block_id;
ceres::Problem problem(problem_options);
std::vector<ceres::ResidualBlockId> residual_block_ids;
​
problem.AddParameterBlock(m_para_buffer_incremental, 4, q_parameterization);//四维
// 为问题添加一个大小适当的参数块。具有相同参数的重复调用将被忽略。使用相同的双指针但大小不同的重复调用将导致未定义行为。
problem.AddParameterBlock(m_para_buffer_incremental + 4, 3);

c1) 角点 转换到地图坐标系下 并做运动补偿 pointOri转换前的点,pointSel转换后的点

for (int i = 0; i < laser_corner_pt_num; i++) // curr帧corners
{
    pointOri = laserCloudCornerStack->points[i];
                    if ((!std::isfinite(pointOri.x)) ||
                        (!std::isfinite(pointOri.y)) ||
                        (!std::isfinite(pointOri.z)))
                        continue;
​
    pointAssociateToMap(&pointOri, &pointSel, refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), if_undistore_in_matching);
// 用上一帧时刻的位姿作为初值,把当前帧下的点转化到map下

这里调用了pointAssociateToMap函数;将点转换到map下

m_if_motion_deblur =0 为分段处理;=1为线性插值 这里为0 论文C In-frame motion compensation

void pointAssociateToMap(PointType const *const pi, PointType *const po,
                             double interpolate_s = 1.0, int if_undistore = 0)
    {
        Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
        Eigen::Vector3d point_w;
        if (m_if_motion_deblur == 0 || if_undistore == 0 || interpolate_s == 1.0)
        {
            point_w = m_q_w_curr * point_curr + m_t_w_curr;
        }
    //进入else
        else
        {
            if (interpolate_s > 1.0 || interpolate_s < 0.0)
            {
                screen_printf("Input interpolate_s = %.5f\r\n", interpolate_s);
            }
​
            if (1) // Using rodrigues for fast compute.
            {
                //https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
                Eigen::Vector3d interpolate_T = m_t_w_incre * (interpolate_s * BLUR_SCALE);
                double interpolate_R_theta = m_interpolatation_theta * interpolate_s;
                Eigen::Matrix<double, 3, 3> interpolate_R_mat;
​
                interpolate_R_mat = Eigen::Matrix3d::Identity() + sin(interpolate_R_theta) * m_interpolatation_omega_hat + (1 - cos(interpolate_R_theta)) * m_interpolatation_omega_hat_sq2;
                point_w = m_q_w_last * (interpolate_R_mat * point_curr + interpolate_T) + m_t_w_last;
            }
            else
            {
                Eigen::Quaterniond interpolate_q = m_q_I.slerp(interpolate_s * BLUR_SCALE, m_q_w_incre);
                Eigen::Vector3d interpolate_T = m_t_w_incre * (interpolate_s * BLUR_SCALE);
                // 先转换到上一帧end下
                point_w = m_q_w_last * (interpolate_q * point_curr + interpolate_T) + m_t_w_last;
                // 再转换到map下
            }
        }
​
        po->x = point_w.x();
        po->y = point_w.y();
        po->z = point_w.z();
        po->intensity = pi->intensity;
    }
​

c2) kdtree 确定线点 找出里当前角点周围最近的5个点,最远的点距离当前点不能超过50m,协方差矩阵∑的特征值的最大值是第二大特征值的三倍,则认为该点在线上;这要还要根据论文做进一步分析

pointAssociateToMap(&pointOri, &pointSel, refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), if_undistore_in_matching);
m_kdtree_surf_from_map.nearestKSearch(pointSel, plane_search_num, m_point_search_Idx, m_point_search_sq_dis);
​
if (m_point_search_sq_dis[plane_search_num - 1] < m_maximum_dis_plane_for_match) // 50m
{
     std::vector<Eigen::Vector3d> nearCorners;
     Eigen::Vector3d center(0, 0, 0);
     if (IF_PLANE_FEATURE_CHECK)
     {
        for (int j = 0; j < plane_search_num; j++)
        {
          Eigen::Vector3 dtmp(laser_cloud_corner_from_map.points[m_point_search_Idx[j]].x,
                                                    laser_cloud_corner_from_map.points[m_point_search_Idx[j]].y,
                                                    laser_cloud_corner_from_map.points[m_point_search_Idx[j]].z);
          center = center + tmp;
          nearCorners.push_back(tmp);
       }
​
        center = center / (float)(plane_search_num);  // 计算这五个点的均值
        Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
         //计算各点的残差,构造协方差矩阵
        for (int j = 0; j < plane_search_num; j++)
         {
             Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
             covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
         }
​
         Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
        if ((saes.eigenvalues()[2] > 3 * saes.eigenvalues()[0]) &&
         (saes.eigenvalues()[2] < 10 * saes.eigenvalues()[1]))
         {
              planeValid = true;
         }
         else
         {
             planeValid = false;
         }
}

c3) // 对这5个points组成的集合计算协方差,特征值分解,最大特征值是否是次大特征的3倍以上,

// 若不是,忽略掉该点,拒绝points num++;

// 若是,计算残差

 Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (line_is_avail) // 1
 {
     if (ICP_LINE) // 1, //loop clousre(scene_alignment.hpp)时:0, 不用corners计算loop closure
     {
     ceres::CostFunction *cost_function;
    auto pt_1 = pcl_pt_to_eigend(laser_cloud_corner_from_map.points[m_point_search_Idx[0]]); // j point
   auto pt_2 = pcl_pt_to_eigend(laser_cloud_corner_from_map.points[m_point_search_Idx[1]]); // l point
          if ((pt_1 - pt_2).norm() < 0.0001)
               continue;
​
          if (m_if_motion_deblur) // 线性插值
          {
              cost_function = ceres_icp_point2line_mb<double>::Create(curr_point,
 pt_1,  pt_2,refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp) * 1.0,Eigen::Matrix<double, 4, 1>(m_q_w_last.w(), m_q_w_last.x(), m_q_w_last.y(), m_q_w_last.z()),m_t_w_last); //pointOri.intensity );
          }
          else
          { // 分段
           cost_function = ceres_icp_point2line<double>::Create(curr_point,pt_1,pt_2,Eigen::Matrix<double, 4, 1>(m_q_w_last.w(), m_q_w_last.x(), m_q_w_last.y(), m_q_w_last.z()),m_t_w_last);
          }
​
          // 直线到点的距离向量,由垂足指向点
           block_id = problem.AddResidualBlock(cost_function, loss_function, m_para_buffer_incremental, m_para_buffer_incremental + 4);
              residual_block_ids.push_back(block_id);
              corner_avail_num++;
 }
}
else
{ // 忽略掉该point
        corner_rejection_num++;
}
}
} // end curr帧corners

c4)同样的,对面点进行约束 ,最终求得的是位姿的旋转和平移的增量

d) 根据残差项的大小去除20%的离群点 这一部分在学习到ceres_slove时再细看

for (size_t ii = 0; ii < 1; ii++)
                {
                    options.linear_solver_type = slover_type;
                    options.max_num_iterations = m_para_cere_max_iterations;
                    options.max_num_iterations = m_para_cere_prerun_times; // 先让ceres迭代2次,根据结果的残差项大小,大约删除掉20%的outliers
                    options.minimizer_progress_to_stdout = false;
                    options.check_gradients = false; // TODO true时, 总是 Gradient error detected. Terminating solver
                    options.gradient_check_relative_precision = 1e-10;
​
                    set_ceres_solver_bound(problem, m_para_buffer_incremental); // 设置参数 m_t_w_incre最小值,最大值[-2,2]
                    ceres::Solve(options, &problem, &summary);
​
                    residual_block_ids_temp.clear();
                    ceres::Problem::EvaluateOptions eval_options;
                    eval_options.residual_blocks = residual_block_ids;
                    double total_cost = 0.0;
                    std::vector<double> residuals; // 残差是:点到直线的距离向量,点到面的距离向量
                    problem.Evaluate(eval_options, &total_cost, &residuals, nullptr, nullptr);
​
                    double m_inliner_ratio_threshold = compute_inlier_residual_threshold(residuals, m_inlier_ratio); // 内点所占比例大致为80%
                    m_inlier_threshold = std::max(m_inliner_dis, m_inliner_ratio_threshold);                         // 计算位姿时, max(0.02,  ); 闭环匹配时: max(0.2, )
                    for (unsigned int i = 0; i < residual_block_ids.size(); i++)
                    {
                        if ((fabs(residuals[3 * i + 0]) + fabs(residuals[3 * i + 1]) + fabs(residuals[3 * i + 2])) > m_inlier_threshold) // std::min( 1.0, 10 * avr_cost )
                        {
                            problem.RemoveResidualBlock(residual_block_ids[i]);
                        }
                        else
                        {
                            residual_block_ids_temp.push_back(residual_block_ids[i]);
                        }
                    }
​
                    residual_block_ids = residual_block_ids_temp;
                }

e) 得到当前帧的位姿

options.linear_solver_type = slover_type;
options.max_num_iterations = m_para_cere_max_iterations;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false; // TODO true时, 总是 Gradient error detected. Terminating solver
options.gradient_check_relative_precision = 1e-10;
​
set_ceres_solver_bound(problem, m_para_buffer_incremental);
ceres::Solve(options, &problem, &summary);
if (m_if_motion_deblur) // 线性插值; loop closure时为0
{
    compute_interpolatation_rodrigue(q_w_incre, m_interpolatation_omega, m_interpolatation_theta, m_interpolatation_omega_hat);
    m_interpolatation_omega_hat_sq2 = m_interpolatation_omega_hat * m_interpolatation_omega_hat;
}
​
  m_t_w_curr = m_q_w_last * t_w_incre + m_t_w_last; // loop closure时,last=(I,0)
  m_q_w_curr = m_q_w_last * q_w_incre;              // loop closure时, curr = incre
​
m_angular_diff = (float)m_q_w_curr.angularDistance(m_q_w_last) * 57.3; // delta角度
m_t_diff = (m_t_w_curr - m_t_w_last).norm();                           // delata平移
minimize_cost = summary.final_cost;
if (q_last_optimize.angularDistance(q_w_incre) < 57.3 * m_minimum_icp_R_diff &&
                    (t_last_optimize - t_w_incre).norm() < m_minimum_icp_T_diff)
{
     screen_out << "----- Terminate, iteration times  = " << iterCount << "-----" << endl;
      break;
}
else
{
     q_last_optimize = q_w_incre;
     t_last_optimize = t_w_incre;
}
} // end icp 计算位姿
​
screen_printf("===== corner factor num %d , surf factor num %d=====\n", corner_avail_num, surf_avail_num);
if (laser_corner_pt_num != 0 && laser_surface_pt_num != 0)
 {
                m_logger_common->printf("Corner  total num %d |  use %d | rate = %d \% \r\n", laser_corner_pt_num, corner_avail_num, (corner_avail_num)*100 / laser_corner_pt_num);
                m_logger_common->printf("Surface total num %d |  use %d | rate = %d \% \r\n", laser_surface_pt_num, surf_avail_num, (surf_avail_num)*100 / laser_surface_pt_num);
}
            
            *(m_logger_timer->get_ostream()) << m_timer->toc_string("Pose optimization") << std::endl;
            if (g_export_full_count < 5)
            {
                *(m_logger_common->get_ostream()) << summary.FullReport() << endl;
                g_export_full_count++;
            }
            else
            {
                *(m_logger_common->get_ostream()) << summary.BriefReport() << endl;
            }
​
            *(m_logger_common->get_ostream()) << "Last R:" << m_q_w_last.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * 57.3 << " ,T = " << m_t_w_last.transpose() << endl;
            *(m_logger_common->get_ostream()) << "Curr R:" << m_q_w_curr.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * 57.3 << " ,T = " << m_t_w_curr.transpose() << endl;
            *(m_logger_common->get_ostream()) << "Iteration time: " << iterCount << endl;
​
            m_logger_common->printf("Motion blur = %d | ", m_if_motion_deblur);
            m_logger_common->printf("Cost = %.5f| inlier_thr = %.2f |blk_size = %d | corner_num = %d | surf_num = %d | angle dis = %.2f | T dis = %.2f \r\n",
                                    minimize_cost, m_inlier_threshold, summary.num_residual_blocks, corner_avail_num, surf_avail_num, m_angular_diff, m_t_diff);
​
            m_inlier_threshold = m_inlier_threshold * summary.final_cost / summary.initial_cost; //
            // 计算位姿结束; 计算最终的匹配, used in loop closure(scene_alignment.hpp)
​
            // 计算位姿时:     delta_angle > 4度  或  最终的cost > 2.0,  认为本次计算失败,当前帧的位姿置为上一帧的位姿
            // loop closure时:  没有通过函数的返回值是0 or 1 来判断是否对齐, 而是通过m_inlier_threshold来判断
            if (m_angular_diff > m_para_max_angular_rate || minimize_cost > m_max_final_cost)
            {
                *(m_logger_common->get_ostream()) << "**** Reject update **** " << endl;
                *(m_logger_common->get_ostream()) << summary.FullReport() << endl;
                for (int i = 0; i < 7; i++)
                {
                    m_para_buffer_RT[i] = m_para_buffer_RT_last[i]; // 其他地方没有使用这两个变量,没有用到
                }
​
                m_last_time_stamp = m_minimum_pt_time_stamp;
                m_q_w_curr = m_q_w_last;
                m_t_w_curr = m_t_w_last;
​
                return 0;
            }
​
            m_final_opt_summary = summary;
        }
        else
        {
            screen_out << "time Map corner and surf num are not enough" << std::endl;
        }
​
        return 1;
    }

4)保存当前帧经消除畸变后的角点和面点

PointType pointOri, pointSel;
//下边两个存储的是经过消除畸变的corners和plannes
pcl::PointCloud<PointType>::Ptr pc_new_feature_corners(new pcl::PointCloud<PointType>()); // 当前帧经消除畸变后的corners在map下坐标
pcl::PointCloud<PointType>::Ptr pc_new_feature_surface(new pcl::PointCloud<PointType>()); // 当前帧经消除畸变后的plannes在map下坐标
for (int i = 0; i < laser_corner_pt_num; i++)
{
    pc_reg.pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel, refine_blur(laserCloudCornerStack->points[i].intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), g_if_undistore);
    pc_new_feature_corners->push_back(pointSel); // 上一行refine_blur 是利用时间戳消除畸变,返回一个时间/位置的比例.pointAssociateToMap这个函数把消除畸变并且转换到地图坐标系下的点记录到&pointSel里.
}
​
for (int i = 0; i < laser_surface_pt_num; i++)
{
    pc_reg.pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel, refine_blur(laserCloudSurfStack->points[i].intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), g_if_undistore);
    pc_new_feature_surface->push_back(pointSel); // 平面点除畸变,写入地图坐标系
}

5) 对角点和面点进行滤波

 //进行滤波
down_sample_filter_corner.setInputCloud(pc_new_feature_corners);
down_sample_filter_corner.filter(*pc_new_feature_corners);
down_sample_filter_surface.setInputCloud(pc_new_feature_surface);
down_sample_filter_surface.filter(*pc_new_feature_surface);
//r_diff 旋转角度距离     t_diff平移距离
double r_diff = m_q_w_curr.angularDistance(m_last_his_add_q) * 57.3; // 换成度数
double t_diff = (m_t_w_curr - m_last_his_add_t).norm();              // 世界坐标减去上次增量后归一化
pc_reg.pointcloudAssociateToMap(current_laser_cloud_full, current_laser_cloud_full, g_if_undistore); // 全部点云转换
// 转换到map坐标系下

上面保存的数据在pointAssociateToMap下利用时间戳消除畸变并转换到地图坐标系下,存储到pc_new_feature_corners\pc_new_feature_surface中 current_laser_cloud_full通过pointcloudAssociateToMap将全部点云消除畸变并转换坐标系后重新储存在current_laser_cloud_full中

6)保留400帧作为历史地图,并对超过和不足的进行处理

 //历史关键帧不足m_maximum_history_size则添加,超过m_maximum_history_size则删除
        m_mutex_mapping.lock();
        
        if (m_laser_cloud_corner_history.size() < (size_t)m_maximum_history_size || // 400帧
            (t_diff > history_add_t_step) ||                                        // 0
            (r_diff > history_add_angle_step * 57.3))                               // 0
        {
            m_last_his_add_q = m_q_w_curr; // 存档当前迭代位姿
            m_last_his_add_t = m_t_w_curr;
​
            m_laser_cloud_corner_history.push_back(*pc_new_feature_corners); // 存档历史点云
            m_laser_cloud_surface_history.push_back(*pc_new_feature_surface);
            m_mutex_dump_full_history.lock();
            m_laser_cloud_full_history.push_back(current_laser_cloud_full);
            m_his_reg_error.push_back(pc_reg.m_inlier_threshold); // 每一帧点云与local map匹配计算出位姿后的cost,the smaller the more certain about calculated pose.
            m_mutex_dump_full_history.unlock();
        }
        else
        {
            screen_printf("==== Reject add history, T_norm = %.2f, R_norm = %.2f ====\r\n", t_diff, r_diff);
        }
​
        screen_out << "m_pt_cell_map_corners.size() = " << m_pt_cell_map_corners.get_cells_size() << endl;
        screen_out << "m_pt_cell_map_planes.size() = " << m_pt_cell_map_planes.get_cells_size() << endl;
​
        if (m_laser_cloud_corner_history.size() > (size_t)m_maximum_history_size) // 400帧初始化是100
        {
            (m_laser_cloud_corner_history.front()).clear();
            m_laser_cloud_corner_history.pop_front();//直接pop_front不行吗
        }
​
        if (m_laser_cloud_surface_history.size() > (size_t)m_maximum_history_size) // 400帧
        {
            (m_laser_cloud_surface_history.front()).clear();
            m_laser_cloud_surface_history.pop_front();
        }
​
        if (m_laser_cloud_full_history.size() > (size_t)m_maximum_history_size) // 400帧
        {
            m_mutex_dump_full_history.lock();
            m_laser_cloud_full_history.front().clear();
            m_laser_cloud_full_history.pop_front();
            m_his_reg_error.pop_front();
            m_mutex_dump_full_history.unlock();
        }
        // m_laser_cloud_corner_history, m_laser_cloud_surface_history, m_laser_cloud_full_history
        // 算上当前帧在内,保留400帧 history frame

7)将处理好的点云数据转为eigen形式并添加到m_pt_cell_map_corners/planes中

  m_if_mapping_updated_corner = true;
        m_if_mapping_updated_surface = true;
        //将处理好的点云数据转为eigen形式并添加到m_pt_cell_map_corners/planes中
        m_pt_cell_map_corners.append_cloud(PCL_TOOLS::pcl_pts_to_eigen_pts<float, PointType>(pc_new_feature_corners));
        m_pt_cell_map_planes.append_cloud(PCL_TOOLS::pcl_pts_to_eigen_pts<float, PointType>(pc_new_feature_surface));
        // 把每一帧corners在map下点加入到m_pt_cell_map_corners
        // 把每一帧plannes在map下点加入到m_pt_cell_map_planes
​
        *(m_logger_common.get_ostream()) << "New added regtime " << point_cloud_current_timestamp << endl;
        if ((m_lastest_pc_reg_time < point_cloud_current_timestamp) || (point_cloud_current_timestamp < 10.0))
        {
            m_q_w_curr = pc_reg.m_q_w_curr; // 如线程得到最新位姿则更新位姿
            m_t_w_curr = pc_reg.m_t_w_curr;
            m_lastest_pc_reg_time = point_cloud_current_timestamp;
        }
        else
        {
            *(m_logger_common.get_ostream()) << "***** older update, reject update pose *****" << endl;
        }

8) 回环检测,这部分先不看

9)角点,面点的调试 0

10)发布局部地图数据 0

11) 发布当前位姿 和 当前轨迹

nav_msgs::Odometry odomAftMapped;
        odomAftMapped.header.frame_id = "camera_init";
        odomAftMapped.child_frame_id = "aft_mapped";
        odomAftMapped.header.stamp = ros::Time().fromSec(time_odom);
​
        odomAftMapped.pose.pose.orientation.x = m_q_w_curr.x();
        odomAftMapped.pose.pose.orientation.y = m_q_w_curr.y();
        odomAftMapped.pose.pose.orientation.z = m_q_w_curr.z();
        odomAftMapped.pose.pose.orientation.w = m_q_w_curr.w();
​
        odomAftMapped.pose.pose.position.x = m_t_w_curr.x();
        odomAftMapped.pose.pose.position.y = m_t_w_curr.y();
        odomAftMapped.pose.pose.position.z = m_t_w_curr.z();
​
        m_pub_odom_aft_mapped.publish(odomAftMapped); // name: Odometry aft_mapped_to_init 高频里程计
​
        geometry_msgs::PoseStamped pose_aft_mapped;
        pose_aft_mapped.header = odomAftMapped.header;
        pose_aft_mapped.pose = odomAftMapped.pose.pose;
        m_laser_after_mapped_path.header.stamp = odomAftMapped.header.stamp;
        m_laser_after_mapped_path.header.frame_id = "camera_init";
​
        if (m_current_frame_index % 10 == 0) // 每隔10帧发布一次位姿
        {
            m_laser_after_mapped_path.poses.push_back(pose_aft_mapped);
            m_pub_laser_aft_mapped_path.publish(m_laser_after_mapped_path);
        }

12)发布tf 当前帧到地图坐标的转换关系

static tf::TransformBroadcaster br;
        tf::Transform transform;
        tf::Quaternion q;
        transform.setOrigin(tf::Vector3(m_t_w_curr(0), m_t_w_curr(1), m_t_w_curr(2)));
​
        q.setW(m_q_w_curr.w());
        q.setX(m_q_w_curr.x());
        q.setY(m_q_w_curr.y());
        q.setZ(m_q_w_curr.z());
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped"));

4、返回process函数,不断循环

m_logger_timer.get_ostream 这里没懂

三 没懂得地方

1、线程std::future

2、上锁 m_mutex_buf.lock();

3、数据得存储 (*m_logger_common.get_ostream())

4、分段处理

正在学习的可以一起沟通交流,qq1078087141

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值