一起做激光SLAM[四]常见SLAM技巧使用效果对比,后端

177
0
2021年1月28日 09时16分

 

本节目标:搭建一套700行代码的激光SLAM。通过对ALOAM进行修改实验,确定对激光SLAM最核心的技巧,并接上节里程计,完成后端,构建较大场景(轨迹约2km)地图。

 

预期效果:

 

微信图片_20210125155059

 

rosbag数据: https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A 提取码: mewi

 

程序:https://gitee.com/eminbogen/one_liom

 

目录

 

实际地图与ALOAM效果

ALOAM修改实验

棱匹配与曲率排序

帧对地图匹配

棱面边界去除

棱点筛除

地面点分离

后端构建

讨论

时耗

平整度

不足


 

实际地图与ALOAM效果

 

因为先试了一下LOAM跟丢了,所以用完整走完的ALOAM来进行实验。蓝色为里程计结果,绿色为后端优化后的效果,差距非常大。第三张图是跟丢的LOAM。

 

微信图片_20210125155114

 

微信图片_20210125155121

 

微信图片_20210125155123

 

ALOAM修改实验

 

棱匹配与曲率排序

 

棱匹配,是希望图像中曲率较大的点匹配到对应的棱上,与点面匹配对应。在下图中c可以当做我们的当前帧的点,ab为前一帧棱上点,作公式如下,即CA×CB/AB,由于叉乘为|CA|*|CB|*sin\theta,所以在AB点固定时角度越接近0度或180度,两向量越小,分子就越小,可以将d作为损失量进行优化。实验后来发现这个没啥用。

 

微信图片_20210125155203

 

微信图片_20210125155206

 

程序:

 

Eigen::Matrix<T, 3, 1> nu = (lpc - lpa).cross(lpc - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
residual[0] = nu.x() / de.norm();
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();

 

曲率排序是在选取特征点对于16线,每线分为6区域,每区域内按曲率高低录取面点和棱点。实验后来发现这个很有用。

 

程序:

 

//利用cloudScanEndInd将每条线分成六块,每块使用sort排序,并从最平缓的4个点录入laserCloudplane,录入后对周围点作不选取的标志
for(int i=0;i<16;i++)
{
    //每条线起止
    int start_num = cloudScanEndInd[i];
    int end_num = cloudScanEndInd[i+1];
    //分六块
    for(int j=0;j<6;j++)
    {
	int start_num_temp = start_num+ ((end_num-start_num)/6)*j;
	int end_num_temp = start_num+ ((end_num-start_num)/6)*(j+1);
	//块内排序cloudSortInd会从1,2,3,4这种顺序变成乱序,乱序后指代的点曲率从小到大排列
	std::sort (cloudSortInd + start_num_temp, cloudSortInd + end_num_temp, comp);
	//计数4个
	int plane_num=0;
	//k要在区间内,点要小于5个
	for(int k=start_num_temp;k<end_num_temp&&plane_num<5;k++)
	{
	    //根据cloudSortInd取点序号
	    long ind = cloudSortInd[k];
	    //可选点+曲率小就要
	    if(laserCloudall->points[ind].r==1&&cloudcurv[ind]<0.1)
	    {
		plane_num++;
		laserCloudall->points[ind].g=plane_num;
		laserCloudplane->push_back(laserCloudall->points[ind]);
		//临近点变成不可选
		for(int m=1;ind+m<laser_num&&m<=5;m++)
		{
		    laserCloudall->points[ind+m].r=2;
		}
		for(int m=1;ind-m>0&&m<=5;m++)
		{
		    laserCloudall->points[ind-m].r=2;
		}
	    }
	}
    }
}

 

这里第一张图为不进行点排序筛选的效果,从左到右,从上到下依次是上次数据集采用棱+面优化结果,上次数据集只采用面优化结果,本次数据集棱+面优化结果,本次数据集只面优化结果。

 

微信图片_20210125155244

 

这里第二张图为进行点排序筛选的效果,从左到右,从上到下依次是上次数据集采用棱+面优化结果,上次数据集只采用面优化结果,本次数据集棱+面优化结果,本次数据集只面优化结果。 对比来看,有无棱没有多少差距,但本次数据集来看,有点的排序筛选的形状畸变更小,在后续加入后端检测后,这一操作有无会产生重大影响。

 

微信图片_20210125155258

 

帧对地图匹配

 

对于ALOAM,每帧点云是与地图中一定范围内的点匹配,这与一帧点云和前一定数量的帧形成的地图进行匹配是不同的,ALOAM因为这一设定拥有了一种类似闭环检测的能力。如下图中,绿色为我写的里程计结果,蓝色为后端优化后结果。第一张是新帧和前200帧形成的地图匹配,后一张为新帧和全局地图匹配,效果差距非常大。这里的匹配是寻找当前帧位姿变换后地图内的临近面点,之后进行点面优化,所以当里程计运算累积误差较大时,匹配也匹配不上实际平面,所以,这和icp求解位姿变换解决闭环检测问题不同,也就是还需要闭环检测。

 

微信图片_20210125155313

 

棱面边界去除

 

这是指程序中如果检测到面点,那么会把周围点去除不进行点筛选,除非附近点曲率较大可能为棱点。

 

程序:

 

for (int l = 1; l <= 5; l++)
{ 
    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
    {
	break;
    }
    cloudNeighborPicked[ind + l] = 1;
}

 

因为ALOAM是先取棱后取面,实际面之间剩下的棱点比较少,其次边缘区域全去除不是坏事,因为曲率如果是0.06,那么这里会没有去除,在之后面点提取中可能把它提进来,这种面之间的棱点提进来我个人觉得不太好,相当于你收集了一个曲面,不利于后续结果,实验发现删了这个操作精度会有效提升。

 

棱点筛除

 

提取棱点有两个作用,一是你确实有了棱点,二是棱点周围的点你都会去除,不会算入面点中,实际效果没啥用。

 

地面点分离

 

这个是参考LEGO和HDL来的,将地面点删除后统计面点,后来发现地面如果比较平(如这个数据集),其实对结果有不错的影响,毕竟地面的面也是面。你的面从四周变成前后左右下五个方向,对结果是有益的。


 

后端构建

 

后端的坐标系有三个,里程计传递过来的相对里程计原点的坐标系q_wodom_curr,t_wodom_curr,后端坐标系q_w_curr,t_w_curr,后端相对里程计的坐标系q_wmap_wodom,t_wmap_wodom,整个系统接收里程计,使用后端相对里程计的坐标系计算后端坐标系。在odometry.cpp里将当前帧变成前一帧坐标系下(局部坐标系),然后在map.cpp里转换到后端坐标系与后端坐标系下的全部地图匹配,以点面匹配的方式,修改后端坐标系q_w_curr,t_w_curr。

 

损失函数:

 

//点面损失函数,输入的是当前帧的某点_point_o_,目标平面的中心点_point_a_,目标平面的法线_norn_,常规求ao向量在法向量上的投影
struct CURVE_PLANE_COST
{
  CURVE_PLANE_COST(Eigen::Vector3d _point_o_, Eigen::Vector3d _point_a_,Eigen::Vector3d _norn_):
		     point_o_(_point_o_),point_a_(_point_a_),norn_(_norn_){}
  template <typename T>
  bool operator()(const T* q,const T* t,T* residual)const
  {
    Eigen::Matrix<T, 3, 1> p_o_curr{T(point_o_.x()), T(point_o_.y()), T(point_o_.z())};
    Eigen::Matrix<T, 3, 1> p_a_last{T(point_a_.x()), T(point_a_.y()), T(point_a_.z())};
    Eigen::Matrix<T, 3, 1> p_norm{T(norn_.x()), T(norn_.y()), T(norn_.z())};
    Eigen::Quaternion<T> rot_q{q[3], q[0], q[1], q[2]};
    Eigen::Matrix<T, 3, 1> rot_t{t[0], t[1], t[2]};
    Eigen::Matrix<T, 3, 1> p_o_last;
    p_o_last=rot_q * p_o_curr + rot_t;
    residual[0]=((p_o_last - p_a_last).dot(p_norm));
    return true;
  }
  const Eigen::Vector3d point_o_,point_a_,norn_;
};

 

对于点面匹配的选点,为遍历当前帧的所有点,使用KD树寻找全局地图下的最近的五个点,并求出五个点的法向量,并根据法向量norn与(五个点和五点中心的向量 )的投影大小确定五点是否成面。成面则进行优化。

 

for(int i=0;i<plane_num;i++)
{
    //将当前帧的点转换到世界坐标系,与世界坐标系内的点找五个最近的点
    PointType pointseed;std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;
    TransformToMap(&laserCloudPlane->points[i],&pointseed);
    kdtreePlane.nearestKSearch(pointseed, 5, pointSearchInd, pointSearchSqDis);
    
    //如果五个点里最远的那个也不超过2m,
    if (pointSearchSqDis[4] < 2.0)
    {
	//找五个点的中心点center,并计算五点形成平面的法向量norm
	std::vector<Eigen::Vector3d> nearCorners;
	Eigen::Vector3d center(0, 0, 0);
	for (int j = 0; j < 5; j++)
	{
		Eigen::Vector3d tmp(laserCloudMap->points[pointSearchInd[j]].x,
				    laserCloudMap->points[pointSearchInd[j]].y,
				    laserCloudMap->points[pointSearchInd[j]].z);
		center = center + tmp;
		nearCorners.push_back(tmp);
	}
	center = center / 5.0;
	Eigen::Matrix<double, 5, 3> matA0;
	Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
	for (int j = 0; j < 5; j++)
	{
		matA0(j, 0) = laserCloudMap->points[pointSearchInd[j]].x;
		matA0(j, 1) = laserCloudMap->points[pointSearchInd[j]].y;
		matA0(j, 2) = laserCloudMap->points[pointSearchInd[j]].z;
		//printf(" pts %f %f %f \n", matA0(j, 0), matA0(j, 1), matA0(j, 2));
	}
	// find the norm of plane
	//可以根据这个学习一下https://www.cnblogs.com/wangxiaoyong/p/8977343.html
	Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
	norm.normalize();
 
	//将五个点和中心点形成向量,向量与法向量求点乘,如果大于0.1那么后面就不把这组点放入优化了
	bool planeValid = true;
	for (int j = 0; j < 5; j++)
	{
		Eigen::Vector3d vector_temp(laserCloudMap->points[pointSearchInd[j]].x-center.x(),
					    laserCloudMap->points[pointSearchInd[j]].y-center.y(),
					    laserCloudMap->points[pointSearchInd[j]].z-center.z());
	  
		if (fabs(norm(0) * vector_temp.x() +norm(1) * vector_temp.y() +norm(2) * vector_temp.z()) > 0.1)
		{
			planeValid = false;
			break;
		}
	}
	
	//当前点curr_point,放入优化
	Eigen::Vector3d curr_point(pointseed.x, pointseed.y, pointseed.z);
	if (planeValid)
	{
		Eigen::Vector3d curr_point_o(laserCloudPlane->points[i].x,laserCloudPlane->points[i].y,laserCloudPlane->points[i].z);
		problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_PLANE_COST,1,4,3>
					(new CURVE_PLANE_COST(curr_point_o,center,norm)),loss_function,parameters,parameters+4);
		res_num++;
	}
    }
}

 

讨论

 

时耗

 

后端在后面点多时约70ms,此时前端点处理大概6ms,里程计大约13ms。这种帧对全图的匹配耗时巨大,应该使用当前帧匹配前一定数量的帧(匹配前200帧大概只要几毫秒,这和0.4的降采样有关),之后引入闭环检测,计划使用LIO_SAM的简单位姿欧拉距离求临近帧再icp的方式解决,第五节见。

 

平整度

 

可以看出后端有效地让地面平整了,约束了累积误差。

 

微信图片_20210125155355

 

不足

 

虽然是帧对全地图匹配,但因为没有真正的闭环,在最后会因为累积误差匹配不上闭环(一图),中间地带虽然有效匹配上了,但任然存在点云分层问题(二图),实际轨迹也并不是在中间地区来回路径一致,需要闭环检测。

 

微信图片_20210125155410

 

微信图片_20210125155413

 

发表评论

后才能评论