一起做激光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::KdTreeFLANNkdtreePose;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::vectorpointSearchInd;std::vectorpointSearchSqDis;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;jpoints.push_back(laserCloudMap->points[j]);     }    //如果临近帧有历史帧，那么保存最近帧的位置，将最近帧已有标志打true，历史帧数量加1    if(pointSearchInd[i]