一起做激光SLAM：ICP匹配用于闭环检测
一起做激光SLAM：ICP匹配用于闭环检测
编辑丨古月居
目标
利用ICP进行闭环检测，完成闭环。
预期效果：通过闭环检测完成起止闭环，下图为加入闭环前后。
rosbag数据:
https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A
提取码: mewi
程序：https://gitee.com/eminbogen/one_liom
问题分析
帧-地图匹配时效性问题
由于上次实验是保存了整个地图，所以在有较好降采样时，匹配时间后期也会达到70ms一帧，这并不十分实时。
闭环问题
实际轨迹是走了类似8字的轨迹，起止点重合。
局部地图构建
构建一个局部地图就是在当前帧位置上找比较近点，具体做法是找位置比较近的帧，我们把每次后端计算的帧的位置保存为一个point（XYZ格式），多帧就可以保存为一个pointcloud，当获得一个新帧时可以根据里程计的结果大体估计当前位置，利用KD树找最近的点，程序中我们找200个临近帧，将他们的plane点构建局部地图。
同时，将帧的序号记录，如果有比当前帧的序号少200以上的历史帧（比如8的中心位置，会遇到历史帧），就记录最近的历史帧（KD树输出按距离从近到远排序），历史帧用在后面闭环检测。
//把所有过去的pose送入KD树pcl::KdTreeFLANN<PointType> kdtreePose;kdtreePose.setInputCloud(laserCloudMap_Pose);//当前位置PointType pointpose;pointpose.x=t_w_curr.x();pointpose.y=t_w_curr.y();pointpose.z=t_w_curr.z();//KD树求解200个最近的pose，获取第几帧pointSearchInd，以及距当前帧的距离pointSearchSqDisstd::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;kdtreePose.nearestKSearch(pointpose, 200, pointSearchInd, pointSearchSqDis);//history_close_num为临近帧为历史帧（不在当前的前200帧的数量），hisory_close_flag是最近历史帧已有的标志，只有当前帧存在历史帧才会标志为true，不再进行判断//history_close_Ind_temp为暂时保存的最近历史帧为第几帧，在满足历史帧在临近帧中有50个时，赋值给history_close_Indint history_close_num=0;bool hisory_close_flag=false;long history_close_Ind_temp=0;history_close_Ind=0;for(int i=0;i<200;i++){ //对于局部地图需要获取的plane点，要从有着全部plane点的laserCloudMap里取，从map_point_begin取到map_point_end //pointSearchInd[i]是第i近的帧是SLAM过程中的帧数，laserCloudMap_Ind[pointSearchInd[i]-1]是该帧起始的点的位置， //laserCloudMap_Ind[pointSearchInd[i]]是该帧结束的点的位置 long map_point_begin=0; long map_point_end=0; if(pointSearchInd[i]!=0) map_point_begin = laserCloudMap_Ind[pointSearchInd[i]-1]; map_point_end = laserCloudMap_Ind[pointSearchInd[i]]; //从起始点录取到结束点 for(long j=map_point_begin;j<map_point_end;j++) { laserCloud_temp_Map->points.push_back(laserCloudMap->points[j]); } //如果临近帧有历史帧，那么保存最近帧的位置，将最近帧已有标志打true，历史帧数量加1 if(pointSearchInd[i]<temp_laserCloudMap_Ind-200&&pointSearchSqDis[i]<10) { if(!hisory_close_flag) history_close_Ind_temp=pointSearchInd[i]; hisory_close_flag = true; history_close_num++; }}printf("history_close_num is %d\n",history_close_num);//如果历史帧超过50，暂存变实际，后面会用history_close_Ind。if(history_close_num>50) history_close_Ind = history_close_Ind_temp;//降采样局部地图点downSizeFilterMap.setInputCloud(laserCloud_temp_Map);downSizeFilterMap.filter(*laserCloud_temp_Map);这样每次后端帧-地图匹配的点就很少，可以达到12ms-15ms一帧，速度很快，效果很好。
闭环检测
ICP基础学习
我在gitee里的test_icp里有三个程序，有对应的数据，使用记得改路径。icp_main用于两个点云之间icp获取icp得分，变换矩阵，四元数q和位移t，并将要矫正的laser点云，目标的map点云，矫正后的laser点云输出为pcd，一个简单的效果如下，蓝色是原始laser，绿色是map，红色是转换后的laser。
icp_score是一个批量计算icp分数的程序。在实验最合适的icp方式时使用，由于gitee有文件限制，所以只能对一部分点云进行实验，会因为缺文件报红，但不影响使用。score is 0 10 8是指所有文件中得分为0.3以下，0.6以下，1.0以下分别为多少个。分数越低匹配越好。
ndt_main是一个ndt实验程序。不过应对本实验的数据效果不好，从已有实验看，map点数10000左右，效果较好，点数较多icp效果会更好，但ndt速度下降且准确度下降。下图是点数较少时ndt效果，中间蓝色laser明显右移为红色laser与map贴合。
这是点较多时，ndt匹配不好的情况，可以看出ndt前后没有 有效变化。
icp原理可以看：
https://blog.csdn.net/u010696366/article/details/8941938
一个简单的ICP写法如下，align时启动icp迭代，并将矫正输出cloud_fina1，icp.getFinalTransformation()为变换矩阵，可以使用pcl::transformPointCloud变换laser点云到cloud_fina2,其与cloud_fina1一样。
//构建icppcl::IterativeClosestPoint<PointType, PointType> icp;icp.setMaxCorrespondenceDistance(100);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-8);icp.setEuclideanFitnessEpsilon(1e-8);icp.setRANSACIterations(0); //将A 和 B喂入 icp；；fina1是帧点云转换后的结果 icp.setInputSource(cloud_laser);icp.setInputTarget(cloud_map);icp.align(*cloud_fina1); //如果点云B 只是由 A进行一些简单的刚体变换得来的，icp.hasConverged()值1，如果存在形变则不为1std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;//输出刚体变换矩阵信息,并用该变换对输入帧点云进行转换获得fina2std::cout << icp.getFinalTransformation() << std::endl;pcl::transformPointCloud(*cloud_laser, *cloud_fina2, icp.getFinalTransformation());ICP效果实验
这里实验了各种论文中出现的plane-plane_ICP,plane-cloud_ICP,cloud-cloud_ICP，去地面的手段，plane是平面点，cloud是一帧的全部点。
基本可以确定点的数量增加会对icp有好处，使用一帧全部点和大量历史帧进行ICP效果优于只使用plane点，地面点对于匹配有很大作用，可能有着地面点的cloud点会更有充分的结构信息，便于ICP。红色部分是看起来很好，但实际上匹配效果有误匹配的感觉的一组数据。
将ICP用于闭环
前边后端匹配使用的plane点较少（每帧不到400点），我们保存全部帧的plane点在内存里，即使10W帧的点也就只有300MB左右，放内存可以承担。但每帧的全部点都存内存不现实，所以首先我们要每隔一定帧数将全部点保存为pcd，节约内存。
//这里累积20帧点进行一次局部地图保存pcdstatic int local_map=0;local_map++;for(int i=0;i<int(laserCloud->points.size());i++){ PointType pointseed; TransformToMap(&laserCloud->points[i],&pointseed); pointseed.r = 255; pointseed.g = 255; pointseed.b = 255; laserCloud_local_map->points.push_back(pointseed);}if(local_map==20){ local_map=0; std::string str;static int pcd_num=0; std::stringstream ss; ss << pcd_num; ss >> str; pcd_num++; std::string pcd_path = "/home/eminbogen/data/one_liom_local/"; downSizeFilterMap.setInputCloud(laserCloud_local_map); downSizeFilterMap.filter(*laserCloud_local_map); printf("local num is %d\n",int(laserCloud_local_map->points.size())); std::string map_save_path=pcd_path+str+".pcd"; pcl::io::savePCDFileBinary(map_save_path, *laserCloud_local_map ); laserCloud_local_map->clear();}当我们在前面局部地图获取最近历史帧序号后，就可以根据序号找临近全部点的局部地图。history_close_Ind为最近历史帧的序号，除20是和上面每20帧保存局部地图一致。
//对于临近的7个局部地图进行读取for(int i=-3;i<=3;i++){ int pcd_read=history_close_Ind/20+i; if(pcd_read<0) continue; std::stringstream ss_read;std::string str_read; ss_read << pcd_read; ss_read >> str_read; std::string pcd_read_path = "/home/eminbogen/data/one_liom_local/"; std::string map_read_path=pcd_read_path+str_read+".pcd"; pcl::PointCloud<PointType>::Ptr laserCloud_local_read_map(new pcl::PointCloud<PointType>()); //读了临近局部地图就累加在laserCloud_map_out里 if (pcl::io::loadPCDFile<PointType>(map_read_path, *laserCloud_local_read_map )== -1) { PCL_ERROR("Couldn't read file local_map_pcd\n"); } for(int j=0;j<int(laserCloud_local_read_map->size());j++) { PointType pointseed; pointseed.x=laserCloud_local_read_map->points[j].x-center.x; pointseed.y=laserCloud_local_read_map->points[j].y-center.y; pointseed.z=laserCloud_local_read_map->points[j].z-center.z; laserCloud_map_out->push_back(pointseed); }}之后就是搭建ICP计算ICP得分icp.getFitnessScore()。如果匹配比较好，那么采用计算的旋转矩阵icp.getFinalTransformation()，计算四元数q和位移t_vector，用于位姿变化实现闭环。
//构建icppcl::IterativeClosestPoint<PointType, PointType> icp;pcl::PointCloud<PointType>::Ptr cloud_fina(new pcl::PointCloud<PointType>);icp.setMaxCorrespondenceDistance(100);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-8);icp.setEuclideanFitnessEpsilon(1e-8);icp.setRANSACIterations(0);icp.setInputSource(laserCloud_now_out);icp.setInputTarget(laserCloud_map_out);//下面这句是必须的icp.align(*cloud_fina); //得分低于0.6可以进行闭环float icp_score = icp.getFitnessScore();if(icp_score<0.6){ //获取变换矩阵，求四元数q和位移t_vector Eigen::Matrix4f correctionLidarFrame; correctionLidarFrame = icp.getFinalTransformation(); Eigen::Matrix3d r_matrix = Eigen::Matrix3d::Identity(); for(int i=0;i<3;i++) { for(int j=0;j<3;j++) { r_matrix(i,j) = double(correctionLidarFrame(i,j)); } } Eigen::Vector3d t_icp(double(correctionLidarFrame(0,3)),double(correctionLidarFrame(1,3)),double(correctionLidarFrame(2,3))); Eigen::Quaterniond q_icp; q_icp = Eigen::Quaterniond ( r_matrix ); //位姿变换 t_w_curr.x() = t_w_curr.x()-center.x; t_w_curr.y() = t_w_curr.y()-center.y; t_w_curr.z() = t_w_curr.z()-center.z; t_w_curr = t_w_curr + q_w_curr * t_icp; q_w_curr = q_w_curr*q_icp ; t_w_curr.x() = t_w_curr.x()+center.x; t_w_curr.y() = t_w_curr.y()+center.y; t_w_curr.z() = t_w_curr.z()+center.z; q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;} 这里使用的局部地图还有位姿变换的修改是为了将icp的坐标系转换到0.0附近而不是距原点非常远。
因为对于icp来说，距原点非常远的就意味着可能出现icp认为先产生较小仰角，在向下平移来匹配的情况（比如0.1度，但因为远离原点，导致你看到的所有点云上移好几米），这样会产生巨大的累积误差。
for(int j=0;j<int(laserCloud_local_read_map->size());j++) { PointType pointseed; pointseed.x=laserCloud_local_read_map->points[j].x-center.x; pointseed.y=laserCloud_local_read_map->points[j].y-center.y; pointseed.z=laserCloud_local_read_map->points[j].z-center.z; laserCloud_map_out->push_back(pointseed); } //位姿变换 t_w_curr.x() = t_w_curr.x()-center.x; t_w_curr.y() = t_w_curr.y()-center.y; t_w_curr.z() = t_w_curr.z()-center.z; t_w_curr = t_w_curr + q_w_curr * t_icp; q_w_curr = q_w_curr*q_icp ; t_w_curr.x() = t_w_curr.x()+center.x; t_w_curr.y() = t_w_curr.y()+center.y; t_w_curr.z() = t_w_curr.z()+center.z;不足
最大的问题就是这样的ICP做闭环检测是一种硬闭环，只在出现闭环时修正了位置，并没有修正前面累积误差的问题，需要采用非线性优化完善效果。
跑了一下Kitti效果还不错，这是不是差一个初始位姿的旋转？
版权声明：本文为CSDN博主「Eminbogen」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
本文仅做学术分享，如有侵权，请联系删文。