一起做激光SLAM：ICP匹配用于闭环检测

一起做激光SLAM：ICP匹配用于闭环检测

大家好，孙乐来为大家解答一起做激光SLAM：ICP匹配用于闭环检测的相关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版权协议，转载请附上原文出处链接及本声明。

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

本文到此结束，希望对大家有所帮助。

特别声明：本文来自网络或网友投稿，部分图片和文字来源于网络收集整理，仅供学习交流，版权归原作者所有，转载的目的在于传递更多信息及用于网络分享，并不代表本站赞同其观点和对其真实性负责，也不构成任何其他建议。如果您发现网站上有侵犯您的知识产权的作品，请与我们取得联系，我们会及时修改或删除；邮箱：1091218940@qq.com

本页标题：一起做激光SLAM：ICP匹配用于闭环检测

本页链接：http://cn.dfyuhang.com/icp/743.html