本节目标:利用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,以及距当前帧的距离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效果还不错,,这是不是差一个初始位姿的旋转?