十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码

 专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取_goldqiu的博客-CSDN博客

五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计_goldqiu的博客-CSDN博客

六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）_goldqiu的博客-CSDN博客

七.激光SLAM框架学习之A-LOAM框架---速腾Robosense-16线雷达室内建图_goldqiu的博客-CSDN博客

八.激光SLAM框架学习之LeGO-LOAM框架---框架介绍和运行演示_goldqiu的博客-CSDN博客

九.激光SLAM框架学习之LeGO-LOAM框架---速腾Robosense-16线雷达室外建图和其他框架对比、录包和保存数据_goldqiu的博客-CSDN博客

算法原理和改进

LeGO-LOAM相对LOAM的改进：

1.地面分离方法：

LeGO-LOAM中前端改进中很重要的⼀点就是充分利用了地面点，提供了对地面点的提取

如上图，相邻的两个扫描线束的同⼀列打在地⾯上如AB点所示（指的是同个水平角度下），他们的垂直高度差：

水平距离差：

计算垂直高度差和水平高度差的角度：

理想情况下应该接近0,但激光雷达安装无法做到绝对水平且地面也不是绝对水平，因此这个角度会略微大于0,考虑到作者在草坪之类的场景下运动，因此这个值被设置成10度。这个地面分离算法比较简单，我们也可以结合激光雷达安装高度等其他先验信息进行优化。

下面是代码讲解：

void groundRemoval(){
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;

        for (size_t j = 0; j < Horizon_SCAN; ++j){ //水平角
            // groundScanInd 是在 utility.h 文件中声明的线数，groundScanInd=7，提取地面点时朝下的线束才有效。
            for (size_t i = 0; i < groundScanInd; ++i){ //垂直角

                lowerInd = j + ( i )*Horizon_SCAN; //取出同一个水平角（scan）下相邻垂直角（线）的两个点的索引
                upperInd = j + (i+1)*Horizon_SCAN;

                // 如果nanPoint.intensity = -1 则是空点nanPoint或无效点
                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    groundMat.at(i,j) = -1;
                    continue;
                }
                // 由上下两线之间点的XYZ位置得到两线之间的俯仰角(夹角)
                // 如果俯仰角在10度以内，则判定(i,j)为地面点,groundMat[i][j]=1
                // 否则，则不是地面点，进行后续操作
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;  

                if (abs(angle - sensorMountAngle) <= 10){
                    groundMat.at(i,j) = 1;
                    groundMat.at(i+1,j) = 1;
                }
            }
        }
        // 找到所有点中的地面点或者距离为FLT_MAX(rangeMat的初始值)的点，并将他们标记为-1
        // rangeMat[i][j]==FLT_MAX代表的含义是无效点，比如打到天空上的点
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                if (groundMat.at(i,j) == 1 || rangeMat.at(i,j) == FLT_MAX){
                    labelMat.at(i,j) = -1;
                }
            }
        }
        // 把点放到groundCloud队列中去，把地面点发布出来
        if (pubGroundCloud.getNumSubscribers() != 0){
            for (size_t i = 0; i <= groundScanInd; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    if (groundMat.at(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                }
            }
        }
    }

2.广度优先遍历算法BFS和基于此的点云聚类的外点剔除

广度优先遍历（BFS）和深度优先遍历（DFS）一样，是经典的图遍历算法。

LeGO-LOAM框架用的是广度优先遍历算法做点云聚类，首先从某个节点出发，一层一层地遍历，下⼀层必须等到上⼀层节点全部遍历完成之后才会开始遍历。

如上面这个无向图中：如果我们从A节点开始遍历，那么首先访问和A节点相邻的节点，就是S、B、D，然后再访问和S、B、D相邻的其他节点，就是C。因此，遍历的顺序是A->S->B->D->C；如果我们从S开始遍历，顺序就是S->A->B->C->D；可以看到，不同的起始点对应的遍历顺序是不同的。

通常我们使用BFS遍历图结构的时候，会借助⼀个队列std::queue来帮助我们实现，其基本步骤如下：

1. 将起始顶点A加入队列。

2. 访问A，同时把A标记为已访问，A出列，将节点(相邻点)入队列。

3. 循环从队列中访问节点(相邻点)，标记为已访问，并出列，将子节点(相邻点)入队列。

其中： 如果添加的子节点中有已访问的，则不加入队列。如果队列为空，则访问结束。

BFS算法适用于图数据结构，为了将BFS算法运用到单帧lidar点云，首先需要将其建成一个图模型，⼀个简单有效的办法就是将其投影到⼀个平面图上，以速腾rs-16为例，我们将其投影到一个16×1800大小的图上（这里16是垂直的16根线，1800是因为水平分辨率是0.2度，一个扫描周期有1800个点）。如图：

对于任何⼀个栅格点，其上下左右四个相邻点视为图结构中的邻接节点，要注意的是，左右边界的点和边界另⼀侧也构成邻接，因为水平方向是同⼀个扫描周期，具有物理意义上的连续性，但垂直方面没有。我们可以从任意一个点开始执行BFS搜索，直到遍历完所有近邻点，聚类点数过少的就认为是outlier，可以被剔除。

具体实现：

1. 遍历每个点，如果该点已经被处理过了就不处理。

2. 如果没有被处理就说明这是⼀个新的聚类，然后执行BFS的步骤。

3.将队列里的首元素弹出，然后将该元素近邻塞入队列末尾（这里没有使用std::queue，使用的是普通数组，用了双指针，如下图）

4.分别判断近邻点和自身距离是否足够近，这里没有用两点之间的距离来算。

而是用两点夹角的补角来衡量，angle越大则认为两点越可能是同一个聚类物体上的点，则打上同样的label。

5.循环以上操作。

代码如下：

void cloudSegmentation(){
        for (size_t i = 0; i < N_SCAN; ++i)
            for (size_t j = 0; j < Horizon_SCAN; ++j)
                // 如果labelMat[i][j]=0,表示没有对该点进行过分类，则需要对该点进行聚类
                if (labelMat.at(i,j) == 0)
                    labelComponents(i, j);  //BFS算法聚类

        int sizeOfSegCloud = 0;
        for (size_t i = 0; i < N_SCAN; ++i) {
            
            // segMsg.startRingIndex[i]、segMsg.endRingIndex[i] 表示第i线的点云起始序列和终止序列
            // 以开始线后的第6线为开始，以结束线前的第6线为结束
            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

            for (size_t j = 0; j < Horizon_SCAN; ++j) {
                // 找到可用的特征点或者地面点(不选择labelMat[i][j]=0的点)
                if (labelMat.at(i,j) > 0 || groundMat.at(i,j) == 1){
                    // labelMat数值为999999表示这个点是因为聚类数量不够30而被舍弃的点，需要舍弃的点直接continue跳过本次循环，
                    // 当列数为5的倍数，并且行数较大（大于7），将它保存进outlierCloud（外点）中
                    if (labelMat.at(i,j) == 999999){
                        if (i > groundScanInd && j % 5 == 0){
                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                            continue;
                        }else{
                            continue;
                        }
                    }
                    
                    // 如果是地面点,对于列数不为5的倍数的，直接跳过不处理
                    if (groundMat.at(i,j) == 1){
                        if (j%5!=0 && j>5 && j(i,j) == 1);
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at(i,j);
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    ++sizeOfSegCloud;
                }
            }

            // 以结束线前的第5线为结束
            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }

        // 如果有节点订阅SegmentedCloudPure,就把点云数据保存到segmentedCloudPure中去
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
            for (size_t i = 0; i < N_SCAN; ++i){
                for (size_t j = 0; j < Horizon_SCAN; ++j){
                    // 需要选择不是地面点(labelMat[i][j]!=-1)和没被舍弃的点，就是聚类后的点
                    if (labelMat.at(i,j) > 0 && labelMat.at(i,j) != 999999){
                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        segmentedCloudPure->points.back().intensity = labelMat.at(i,j);
                    }
                }
            }
        }
    }

下面是基于BFS的点云聚类函数：

void labelComponents(int row, int col){
        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;
        
        // 标准的BFS，作用是以(row，col)为中心向外面扩散，判断(row,col)是否是这个平面中一点
        while(queueSize > 0){
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // labelCount的初始值为1，后面会递增
            labelMat.at(fromIndX, fromIndY) = labelCount;

            // neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
            // 遍历点[fromIndX,fromIndY]边上的四个邻点
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){

                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;

                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;

                // 由于雷达水平角度360度相连的特性是个环状的图片，左右连通
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;

                // 如果点[thisIndX,thisIndY]已经标记过
                // labelMat中，-1代表无效点，0代表未进行标记过，其余为其他的标记
                // 如果当前的邻点已经标记过，则跳过该点。
                // 如果labelMat已经标记为正整数，则已经聚类完成，不需要再次对该点聚类
                if (labelMat.at(thisIndX, thisIndY) != 0)
                    continue;

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

                // alpha代表角度分辨率，X方向上角度分辨率是segmentAlphaX(rad)，Y方向上角度分辨率是segmentAlphaY(rad)
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                // 通过下面的公式计算这两点之间是否有平面特征，是否是同一类
                // atan2(y,x)的值越大，d1，d2之间的差距越小,越接近，越平坦
                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

                if (angle > segmentTheta){
                    // segmentTheta=1.0472<==>60度
                    // 如果算出角度大于60度，则假设这是个平面
                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    ++queueSize;
                    ++queueEndInd;

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

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

        bool feasibleSegment = false;

        // 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        else if (allPushedIndSize >= segmentValidPointNum){
            // 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数，这种情况典型例子是电线杆
            int lineCount = 0;
            for (size_t i = 0; i < N_SCAN; ++i)
                if (lineCountFlag[i] == true)
                    ++lineCount;

            // 竖直方向上超过3个也将它标记为有效聚类
            if (lineCount >= segmentValidLineNum)
                feasibleSegment = true;            
        }

        if (feasibleSegment == true){
            ++labelCount;
        }else{
            for (size_t i = 0; i < allPushedIndSize; ++i){
                // 标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个，且竖直方向少于3
                labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

3.两步优化的帧间里程计

和A-LOAM一样，通过前后两帧点云来估计两帧之间的运动，累加得到前端里程记的输出，和A-LOAM构建线面约束同时优化六自由度帧间位姿不同，LeGO-LOAM的前端分成两个步骤，每个步骤估计三自由度的变量:

第一步 利用地面点优化

地面点更符合面特征的性质，因此地面点的优化问题就使用点到面的约束来构建，但地面点之间的约束对x，y和yaw这三个自由度是不能观的，就是当这三个自由度的值发生变化时，点到面的残差不会发生显著变化，所以，地面点之间的优化只会对pitch，roll以及z进行约束和优化。（很显然，pitch，roll变化时两次地面叠加会不重叠，同样Z方向也是。）

第二步 利用角点优化

第一步优化完pitch、roll以及z之后，我们仍需对另外三个自由度的变量进行估计，此时我们用提取的角点进行优化，由于多线激光雷达提取的角点通常是垂直的边缘特征，比如电线杆。这些特征对x、y以及yaw有着比较好的能观性，通过对角点的优化结合地面点优化的结果可以得到六自由度的帧间优化结果。

代码如下，分成两步优化更新前端优化后的六自由度的位姿：

  void updateTransformation(){

        if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)  //角点要大于10，面点要大于100
            return;

        for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
            laserCloudOri->clear();
            coeffSel->clear();

            // 这里是作者手写的非线性优化，利用点面约束优化row、pitch和Z。找到对应的特征平面,然后计算协方差矩阵，保存在coeffSel队列中
            // laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
            findCorrespondingSurfFeatures(iterCount1);

            if (laserCloudOri->points.size() < 10)
                continue;
            // 通过面特征的匹配，计算变换矩阵
            if (calculateTransformationSurf(iterCount1) == false)
                break;
        }

        for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

            laserCloudOri->clear();
            coeffSel->clear();

            // 这里是作者手写的非线性优化，利用点和角点之间约束优化yaw、x和y。找到对应的特征边/角点
            // 寻找边特征的方法和寻找平面特征的很类似
            findCorrespondingCornerFeatures(iterCount2);

            if (laserCloudOri->points.size() < 10)
                continue;
            // 通过角/边特征的匹配，计算变换矩阵
            if (calculateTransformationCorner(iterCount2) == false)
                break;
        }
    }

总结：

LeGO-LOAM这个框架主要还是针对纯激光雷达SLAM部署在轻量化嵌入式平台，由于主要用了激光雷达这个传感器，其里程计精度不会很高。虽然也用了IMU的数据，但只是松耦合的方式。后续作者开源的激光雷达和IMU紧耦合的LIO-SAM框架的里程计精度提升更大。而对于后端，LeGO-LOAM比A-LOAM还是有很多提升，增加了回环检测和图优化，使得地图全局一致性更好，这在后面LIO-SAM框架中再具体分析下。

项目工程代码介绍

由于LeGO-LOAM框架的很多代码是直接从LOAM框架中直接移植过来的，移植过来的代码可读性不强。比如mapping模块中有大量欧拉角的计算，同时手写基于高斯牛顿法的非线性优化代码，如果用第三方库调用模板类，代码可读性会更强（但是可以学习其算法实现思路）。所以，就代码工程而言LeGO-LOAM框架的参考价值没有A-LOAM框架的大。我们主要关注LeGO-LOAM框架对LOAM的改进，对其工程就不做太多介绍，简单介绍下代码框架就可以。而且框架作者后面开源的LIO-SAM和LVI-SAM框架的可读性强了很多，这些框架的代码工程参考价值更大。

项目工程中包括很多图片，主要是展示效果和实物，readme里面包含了作者信息和项目信息。

我们主要看src文件夹，cloud _msgs定义了一些自定义消息类型。Readme文件是这个工程如何部署的一些信息。LeGO-LOAM文件夹中主要是src源文件和include头文件还有launch文件。Include文件夹里面的utility.h包含很多参数文件，其实这部分应该写到yaml文件或者launch文件中，每次运行程序初始化时读取参数配置，这样方便使用和调试。

我们看src文件夹源码。featureAssociation.cpp和 imageProjection.cpp 两个源文件主要是前端的处理。因为这块代码实现比较乱，比如雷达和惯导数据读取在不同的节点中，再加上后面LIO-SAM的代码结构好了很多，所以这里就不深入分析，只是简单介绍下代码。这里包括：

1.数据的读取（两个传感器话题数据的回调函数）

2.雷达和IMU空间、时间的对齐，即坐标变换和时间同步，然后进行雷达运动畸变的去除（adjustDistortion函数）

3.计算曲率，这里和A-LOAM框架一样（calculateSmoothness函数）

4.标记近点（markOccludedPoints函数）

5.点云特征提取和分类，和A-LOAM框架一样（extractFeatures函数）

6. IMU提供初始位姿（updateInitialGuess函数）

7.分两步优化帧间里程计，更新位姿（updateTransformation函数）

8.更新IMU位姿（integrateTransformation函数）

imageProjection节点主要是完成地面分离和点云聚类。这一块前面已经讲了，后面再结合LIO-SAM深入分析。

transformFusion主要是手写了里程计位姿的变换，太多公式，可读性比较差。

mapOptmization是后端优化和回环检测，在LIO-SAM再详细解读。

主要代码如下：

void runFeatureAssociation()
    {
        // 如果有新数据进来则执行，否则不执行任何操作
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }else{
            return;
        }

        // 主要进行的处理是将点云数据进行坐标变换，时间同步等工作
        adjustDistortion();

        // 不完全按照公式进行曲率计算
        calculateSmoothness();

        // 标记过近的点，指在点云中可能出现的互相遮挡的情况
        markOccludedPoints();

        // 特征提取，分别保存到cornerPointsSharp等队列中去，保存到不同的队列是不同类型的点云，进行了分类的工作，这一步中减少了点云数量，使计算量减少
        extractFeatures();

        // 发布cornerPointsSharp等4种类型的点云数据
        publishCloud();

        if (!systemInitedLM) {
            checkSystemInitialization(); 
            return;
        }

        // IMU提供初始位姿
        updateInitialGuess();

        // 更新变换
        updateTransformation();

        // IMU位姿变换
        integrateTransformation();

        publishOdometry();

        publishCloudsLast();   
    }

附：

参考的博客和Github链接：

CSDN博客的内容链接：

1.[地图优化代码理解](LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）)

2.[图像重投影代码理解](LeGO-LOAM 源码阅读笔记(imageProjecion.cpp))

3.[特征关联代码理解](LeGO-LOAM 源码阅读笔记(featureAssociation.cpp))

4.[LeGO-LOAM代码中数学公式的解析]

(LeGO-LOAM中的数学公式推导_wykxwyc的博客-CSDN博客_lego loam 数学推导)

5.[LeGO-LOAM论文翻译(内容精简)](LeGO-LOAM论文翻译（内容精简)_wykxwyc的博客-CSDN博客)

6.[关于位姿变换的一道题](关于位姿变换概念的一道笔试题_wykxwyc的博客-CSDN博客_位姿的概念)

Github链接:

1.[地图优化代码理解](LeGO-LOAM 源码阅读笔记（mapOptmization.cpp）)

2.[图像重投影代码理解](LeGO-LOAM 源码阅读笔记(imageProjecion.cpp))

3.[特征关联代码理解](LeGO-LOAM 源码阅读笔记(featureAssociation.cpp))

4.[LeGO-LOAM代码中数学公式的解析](wykxwyc的博客 | wykxwyc's Blog)

5.[LeGO-LOAM论文翻译(内容精简)](wykxwyc的博客 | wykxwyc's Blog)

总结：后面在读LIO-SAM框架的时候再进行详细分析，并且结合A-LOAM框架中调用第三方库，分析算法代码如何实现，如何手写和调用库，之间的区别是？