编辑丨古月居

目标

利用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版权协议，转载请附上原文出处链接及本声明。

本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)
3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目+双目+鱼眼）

13.重磅！四旋翼飞行器：算法与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~