一起做激光SLAM[四]常见SLAM技巧使用效果对比，后端_人工智能

本节目标：搭建一套700行代码的激光SLAM。通过对ALOAM进行修改实验，确定对激光SLAM最核心的技巧，并接上节里程计，完成后端，构建较大场景(轨迹约2km)地图。预期效果：
一起做激光SLAM[四]常见SLAM技巧使用效果对比，后端 rosbag数据: https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A 提取码: mewi 程序：https://gitee.com/eminbogen/one_liom

实际地图与ALOAM效果

ALOAM修改实验

棱匹配与曲率排序

帧对地图匹配

棱面边界去除

棱点筛除

地面点分离

后端构建

讨论

时耗

平整度

不足

实际地图与ALOAM效果

因为先试了一下LOAM跟丢了，所以用完整走完的ALOAM来进行实验。蓝色为里程计结果，绿色为后端优化后的效果，差距非常大。第三张图是跟丢的LOAM。
一起做激光SLAM[四]常见SLAM技巧使用效果对比，后端

ALOAM修改实验

棱匹配与曲率排序

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

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;
		}
	    }
	}
    }
}

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

帧对地图匹配

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

棱面边界去除

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

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++;
	}
    }
}