二、位姿匹配,建图
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