一起做激光SLAM：常见SLAM技巧使用效果对比，后端

本节目标

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

预期效果：

rosbag数据: 

https://pan.baidu.com/s/1o-noUxgVCdFkaIH21zPq0A

提取码: mewi

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

实际地图与ALOAM效果

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

ALOAM修改实验

棱匹配与曲率排序

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

 程序：

Eigen::Matrix nu = (lpc - lpa).cross(lpc - lpb);
Eigen::Matrix 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