LEGO-LOAM论文与代码阅读

LeGO-LOAM 是对LOAM的一个改进。正如论文题目中Lightweight and Ground-optimized, 它的特点在于相比LOAM更加轻量，加了地面优化。尽管它在实际应用中出现较少，但其中的一些思想都是值得学习的。本文主要关注它相对于LOAM改进的地方，以及存在的不足之处。

LeGO-LOAM主要解决的问题：原始LOAM对计算资源的需求大，在运动剧烈时不够鲁棒，特征不够稳定。

整体框架

系统流程图如下：

与LOAM是基本一致的，只是前面加了点云分割的步骤。但每一步实现的细节与LOAM也有差别，接下来我们具体看这些差别。

对LOAM的改进

点云分割（提取地面，对非地面点云聚类）

首先把一帧点云投影成深度图，深度图的像素为1800*16。深度图每个位置的值是该点到lidar的距离。

Velodyne16 lidar，垂直方向是16线，水平方向是360/0.2=1800.

论文里只是简单提到使用了一个按列评估的地面提取，来看一下代码里是怎么做的。

for (size_t j = 0; j < _horizontal_scans; ++j) {
  for (size_t i = 0; i < _ground_scan_index; ++i) {           // 仅遍历方向朝下的scan
    size_t lowerInd = j + (i)*_horizontal_scans;
    size_t upperInd = j + (i + 1) * _horizontal_scans; //相邻两个laser在同一列的点，也就是深度图的同一列的点

    if (_full_cloud->points[lowerInd].intensity == -1 ||
        _full_cloud->points[upperInd].intensity == -1) {
      // no info to check, invalid points
      _ground_mat(i, j) = -1;
      continue;
    }

    float dX =
        _full_cloud->points[upperInd].x - _full_cloud->points[lowerInd].x;
    float dY =
        _full_cloud->points[upperInd].y - _full_cloud->points[lowerInd].y;
    float dZ =
        _full_cloud->points[upperInd].z - _full_cloud->points[lowerInd].z;

    float vertical_angle = std::atan2(dZ , sqrt(dX * dX + dY * dY + dZ * dZ));

    // TODO: review this change, 判断前后两点的角度变化在10度内

    if ( (vertical_angle - _sensor_mount_angle) <= 10 * DEG_TO_RAD) {
      _ground_mat(i, j) = 1;
      _ground_mat(i + 1, j) = 1;
    }
  }
}

1. 假设激光雷达在一定高度水平安装，只有下面几条laser可能扫描到地面点。因此只遍历这几条线。
2. 取相邻两线在同一水平位置的点（也就是论文所说的按列评估），如下A,B两点

3. 如果两个点连线的垂直角度与lidar安装角度之差在10度以内，就认为是地面点。

这种判断方式过于简单，如果是低于激光雷达的其他平面也会被认为是地面。而且假设了雷达水平安装。但如果用RANSAC之类的提地面方法，耗时就太长了？

然后对非地面点云做聚类。这里论文也没有详细介绍，大致思想是从深度图上一个点出发，找上下左右四个点，如果点之间距离足够接近就聚为一类。直到所有点都有类别。看一下代码（这里是从一个点出发搜索完一类的过程）：

void labelComponents(int row, int col){
    // use std::queue std::vector std::deque will slow the program down greatly
    float d1, d2, alpha, angle;
    int fromIndX, fromIndY, thisIndX, thisIndY; 
    bool lineCountFlag[N_SCAN] = {false};

    queueIndX[0] = row;
    queueIndY[0] = col;
    int queueSize = 1;
    int queueStartInd = 0;
    int queueEndInd = 1;

    allPushedIndX[0] = row;
    allPushedIndY[0] = col;
    int allPushedIndSize = 1;
    
    while(queueSize > 0){
        // Pop point
        fromIndX = queueIndX[queueStartInd];
        fromIndY = queueIndY[queueStartInd];
        --queueSize;
        ++queueStartInd;
        // 标记要pop的点的label
        labelMat.at<int>(fromIndX, fromIndY) = labelCount;
        // Loop through all the neighboring grids of popped grid
        for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
            // new index
            thisIndX = fromIndX + (*iter).first;
            thisIndY = fromIndY + (*iter).second;
            // index should be within the boundary
            if (thisIndX < 0 || thisIndX >= N_SCAN)
                continue;
            // at range image margin (left or right side)
            if (thisIndY < 0)
                thisIndY = Horizon_SCAN - 1;
            if (thisIndY >= Horizon_SCAN)
                thisIndY = 0;
            // prevent infinite loop (caused by put already examined point back)
            if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                continue;

            d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                            rangeMat.at<float>(thisIndX, thisIndY));
            d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                            rangeMat.at<float>(thisIndX, thisIndY));

            if ((*iter).first == 0)
                alpha = segmentAlphaX;
            else
                alpha = segmentAlphaY;

            angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

            if (angle > segmentTheta){

                queueIndX[queueEndInd] = thisIndX;
                queueIndY[queueEndInd] = thisIndY;
                ++queueSize;
                ++queueEndInd;

                labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                lineCountFlag[thisIndX] = true;

                allPushedIndX[allPushedIndSize] = thisIndX;
                allPushedIndY[allPushedIndSize] = thisIndY;
                ++allPushedIndSize;
            }
        }
    }

    // 点数少于30的类被认为无效
    bool feasibleSegment = false;
    if (allPushedIndSize >= 30)
        feasibleSegment = true;
    else if (allPushedIndSize >= segmentValidPointNum){
        int lineCount = 0;
        for (size_t i = 0; i < N_SCAN; ++i)
            if (lineCountFlag[i] == true)
                ++lineCount;
        if (lineCount >= segmentValidLineNum)
            feasibleSegment = true;            
    }
    // segment is valid, mark these points
    if (feasibleSegment == true){
        ++labelCount;
    }else{ // segment is invalid, mark these points
        for (size_t i = 0; i < allPushedIndSize; ++i){
            labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
        }
    }
}

• 聚类时相当于一种广度优先搜索。由于queue,vector结构会导致速度严重变慢，这里用静态数组queueIndX，queueIndY记录，然后用queueStartInd做标记
• 点数少于30的类被认为无效。这里是为了去除像树叶等不稳定的点，防止被提取为特征点
• 同一类原理：其中tang的角度越大，表明相邻的两个point越接近在一个平面上，故可认为是同一类

特征提取

在特征提取阶段，LeGO-LOAM计算曲率的方式从LOAM的计算点的欧式距离改为只用深度：

\[c=\frac{1}{|S|\cdot \|r_i\|} \| \sum _ {j \in s, j\neq i}( r_ {j} -r_ {i}) \| \]

edge点只从非地面点中提取，在设置阈值c后，可以得到边缘点集\(\mathbb{F}_e\)，这些点中c值最大的n(n=2)个点构成\(F_e\)。平面点从地面和非地面点中提取，在设置阈值后得到点集\(\mathbb{F}_p\)，这些点中c值最小的m（m=4）个点构成\(F_p\)

特征匹配

在特征匹配时，我们对这一帧的\(F^t_e,F^t_p\)，在上一帧\(\mathbb{F}^{t-1}_e,\mathbb{F}^{t-1}_p\)中找匹配点，而不是在所有点中找。同时平面点只在地面点类别中找，边缘点只在非地面点类别中找。计算残差的方式和LOAM类似，也是点到线的距离和点到面的距离

帧间位姿估计：两步LM优化

先用平面特征估计\([roll,pitch,z]\)，再用边缘特征估计另外三个维度。因为平面主要以地面为主，而地面就约束了这三个维度。这种优化方法由于分解了问题，减少了每次优化时的变量维度，减少了迭代的次数。且在地面提取比较准确时，可以较好地估计前3个维度，提高算法的鲁棒性。

但也就导致算法非常依赖准确的地面提取，在斜坡，地面不平，lidar安装角度不水平时效果都会变差，且分布优化应该会降低准确率。

建图

建图主要是把当前帧的特征\(F^t_e,F^t_p\)和局部地图做匹配。此外，LeGO-LOAM使用每一个关键帧特征点云单独存储的方式，加入了回环检测，如果当前特征集与之前的特征集通过ICP得到匹配，就把这个约束加入位姿图优化。

LeGo-LOAM的局限性

1. 需要lidar水平安装或已知lidar安装的角度
2. 需要有平坦的地面
3. LeGO-LOAM仅保存特征点，因此点云地图很稀疏。