本节目标：利用ICP进行闭环检测，完成闭环。 预期效果：通过闭环检测完成起止闭环，下图为加入闭环前后。
rosbag数据: https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A 提取码: mewi 程序：https://gitee.com/eminbogen/one_liom
目录
问题分析
帧-地图匹配时效性问题
闭环问题
局部地图构建
ICP基础学习
ICP效果实验
将ICP用于闭环
不足
问题分析
帧-地图匹配时效性问题
由于上次实验是保存了整个地图，所以在有较好降采样时，匹配时间后期也会达到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，以及距当前帧的距离pointSearchSqDis
std::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_Ind
int 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一样。
//构建icp
pcl::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，如果存在形变则不为1
std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;
//输出刚体变换矩阵信息,并用该变换对输入帧点云进行转换获得fina2
std::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帧点进行一次局部地图保存pcd
static 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，用于位姿变化实现闭环。
//构建icp
pcl::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效果还不错，，这是不是差一个初始位姿的旋转？
