SC-Lego-LOAM解析（上）

正文

SC-Lego-LOAM实际上应该并不对应某一篇特定的论文，而是韩国KAIST在github开源的代码，其实质上是融合了ScanContext和Lego LOAM两篇论文，代码及上述两篇论文的出处在这里就不赘述了，在下面的链接里。

https://github.com/irapkaist/SC-LeGO-LOAM

下面正式开始，根据仓库readme的提示，启动方式为：

roslaunch lego_loam run.launch
所以从launch文件看起～
Launch:

<launch>

    。。。。。。

    <!--- LeGO-LOAM -->
    <node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
    <node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>

</launch>

（简洁起见，这里删除了参数的加载和rviz可视化的代码，只保留主线部分，不是说上面不重要，只是为了思路的清晰。）

可以看出这里共启动了4个node，其中最后一个node好像是输出日志之类的东西，对整体的作用影响不大，主要功能的是由前面3个node来实现的，而且是存在数据流的依次传递和处理。

imageProjection

这个node主要的功能是对激光雷达数据进行预处理，包括分割，标注和发布。

从cmake文件看出来，该node的头文件和具体实现分别在image_projection.h和image_projection.cpp中，大家可以先看一下头文件看看大致包括啥，这里直接进入.cpp实现。

int main(int argc, char **argv) {

    ros::init(argc, argv, "lego_loam");

    lego_loam::ImageProjection IP;

    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
    ros::spin();
    return 0;


}

main函数中对ROS进行了初始化，然后就初始化了lego_loam::ImageProjection这么个东西，所以所有的内容都是在lego_loam::ImageProjection的构造函数中。

ImageProjection::ImageProjection() : nh("~") {

        // init params
        InitParams();
        // subscriber
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,
                                                               &ImageProjection::cloudHandler, this);
        // publisher
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);

        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1);  // 离群点或异常点

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;// 无效点的标志

        // new内存
        allocateMemory();
        // 清空,初始化
        resetParameters();
    }

这里定义了1个subscriber，以及n个publisher，并对其他一系列成员变量进行初始化，重点在于subscriber的回调函数ImageProjection::cloudHandler，它才是激光雷达数据流的走向，进入该callback～

void ImageProjection::cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);
        // 2. Start and end angle of a scan
        findStartEndAngle();
        // 3. Range image projection
        projectPointCloud();
        // 4. Mark ground points
        groundRemoval();
        // 5. Point cloud segmentation
        cloudSegmentation();
        // 6. Publish all clouds
        publishCloud();
        // 7. Reset parameters for next iteration
        resetParameters();
    }

回调函数内又调用了7个函数，完成了该node对单帧激光雷达数据所需的所有处理：

1. 拷贝。将ROS定义的PointCloud保存成PCL的PointCloud，因为前者方便借助ROS进行通信，后者方便处理，同时去除了nan，是为了后面的计算中不会出现各种异常情况；

2. 计算起止角度范围。多线激光雷达第一个点和最后一个点并不是严格的360°，这里计算出起至角度后保存在自定义的cloud_msgs::cloud_info类型的成员变量segMsg中，请注意，这里的segMsg很重要，它保存了当前帧的一些重要信息，包括起至角度，每个线的起至序号，及成员变量fullCloud中每个点的状态。

3. 投影点云。把无序点云以角度展开成图像的形式，计算所在行列和深度，其中不同线对应不同行，不同航向角代表不同列，这里以x轴的负方向开始逆时针列序列号逐渐递增，即图像中的从左到右。以Mat图像保存深度，并以单值索引在pcl::PointCloud fullCloud/fullInfoCloud中依次保存点的三维坐标，所在行列（fullCloud）和深度（fullInfoCloud）等。这里的PointCloud与第1步直接拷贝出来的，主要的不同之处在于，根据计算出来的行列，重新组织了点在PointCloud中的顺序，例如，激光雷达本身可能是先列后行的方向输出的点云，或者是livox雷达那种非重复扫描出来的结果，这里都会被重新组织成先行后列的顺序保存。

4. 分割地面。在贴近地面的几个线中提取地面点,并在groundMat中标记1即地面，labelMat中标记-1，即nan或地面点 ，不会参与后面的分割。

5. 非地面分割。这个函数主要完成了两个任务，一是通过广度优先搜索，从非地面点中找出所有连成片的入射角比较小的patch上的点，并在labelMat标注patch的编号（从1开始）；二是把所有地面点和刚分割出来的patch上的点合并保存在segmentedCloud中，这也是该node需要传递给下一个node进行特征提取和匹配的点云，并在segMsg中对应位置处保存每个点的属性（比如该点是不是地面，深度，属于第几列等）。

6. 发布。然后就是发布，包括该帧的segMsg，完整点云fullCloud/fullInfoCloud（步骤3），地面点云（步骤4），发从非地面提取出来的点和降采样的地面点（步骤5），外点（步骤5,实际为比较小的patch上的点）。

7. 重置。清空成员变量，准备下一次的处理

   其他的所有成员都被这个回调函数并所调用，到这里，imageProjection也结束了，因为它的构造函数中定义了雷达消息的回调函数，每当新的数据到来，回调函数调用一系列成员函数进行预处理（主要是分割出地面和非地面的成块的稳定的面，并发布出去）后，又发布出去给后面的node用来提取特征和odo。总结而言，这个node就是对数据进行预处理，分割出来一部分需要的点。

featureAssociation

还是从main函数开始：

int main(int argc, char **argv) {
    ros::init(argc, argv, "lego_loam");

    ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");

    lego_loam::FeatureAssociation FA;
    ros::Rate rate(200);
    while (ros::ok())   // while ( 1 )
    {
        ros::spinOnce();

        FA.runFeatureAssociation();

        rate.sleep();
    }

    ros::spin();
    return 0;
}

跟上一个node差不多，还是把初始化了lego_loam::FeatureAssociation FA这么个东西，推测很多的工作是在构造函数中进行的，比如说订阅消息和回调，不同之处在于这里以200Hz的频率在主动调用FA.runFeatureAssociation()这个函数。

先看lego_loam::FeatureAssociation的构造函数：

FeatureAssociation::FeatureAssociation() : nh("~") {

        InitParams();
        // subscriber
        // callback 基本都是sensor msg->pcl，同时记录该种消息的时间戳和标志位
        // 然后统一到runFeatureAssociation中处理
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1,  // 分割后的点云
                                                               &FeatureAssociation::laserCloudHandler, this);
        subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1,  // 分割后带几何信息的点云
                                                                 &FeatureAssociation::laserCloudInfoHandler, this);
        subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1,
                                                                 &FeatureAssociation::outlierCloudHandler, this);
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);  // imu数据处理

        // publisher
        pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
        pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
        pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
        pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);

        pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
        pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
        pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
        pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);

        initializationValue();
    }

这里订阅了上一节点发出来的分割出来的点云，点云的属性，外点以及IMU消息，并设置了回调函数。对于前三者的回调，感兴趣的读者可以看一下，就是简单的保存并修改标志位（表示收到这种消息了，用来做消息同步），并未对数据做太多额外的处理；对于IMU则复杂一些，它从IMU数据中提取出姿态，角速度和线加速度，其中姿态用来消除重力对线加速度的影响。然后函数AccumulateIMUShiftAndRotation用来做积分，包括根据姿态，将加速度往世界坐标系下进行投影，再根据匀加速度运动模型积分得到速度和位移，同时，对角速度也进行了积分。

好，以上几个回调函数基本只是对数据进行了转寸，IMU也不过是进行了积分，并未涉及点云该如何处理，也没有说怎么与IMU数据做融合。回过头来再看main函数中主动循环调用的runFeatureAssociation函数都做了什么：

void FeatureAssociation::runFeatureAssociation() {
        // 200Hz的检查频率，远远高于雷达数据，注意这里没有考虑IMU
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        } else {
            return;
        }
        /**
            1. Feature Extraction
        */
        adjustDistortion();  //  imu去畸变

        // jin: 对分割出来的点和地面点计算曲率
        calculateSmoothness(); //  计算光滑性

        // jin: 标记两个平面中容易被遮挡的点,以及与两侧相比深度突出的点
        markOccludedPoints();  // 距离较大或者距离变动较大的点标记

        // jin: 与loam一样,提取四种特征点,其中一般平面点进行了降采样.为了保证均匀,进行了6分区,且选中的点两侧的点不会被选中
        // 结果保存在成员变量中
        extractFeatures();

        // 发布四种特征点
        publishCloud(); // cloud for visualization

        /**
        2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();// 将当前帧作为上一帧，并并初始化里程计位姿（主要是roll和pitch初始化，因为这两者是和水平面相关的，其余默认为0），然后返回
            return;
        }

        updateInitialGuess();  //  更新初始位姿

        //  一个是找特征平面，通过面之间的对应关系计算出变换矩阵。
        // 另一个部分是通过角、边特征的匹配，计算变换矩阵。
        // jin: 依次使用平面点和角点(都是sharp和flat点,非一般点),对相对位姿进行优化,保存在成员变量matP中
        // 特点在于,先使用平面点经历多次优化后,再使用角点(可能因为平面点更多,因而会更稳定吧)
        updateTransformation();

        // jin: 累计位姿到transformSum中
        integrateTransformation();  //  计算旋转角的累积变化量

        // jin: 发布里程计位姿,发布tf变换
        publishOdometry();

        // jin: 将当前帧数据统一到最终点时刻，重置作为last kd-tree,同时发布角点和面点(都是一般的)
        publishCloudsLast(); // cloud to mapOptimization
    }

当前一个节点发布的分割出来的点云，点云的属性以及外点都被接收到后，该函数才被真正执行。根据注释，该函数分为Feature Extraction和Feature Association两个相对独立的部分：

Feature Extraction

1. adjustDistortion：畸变校正。前面IMU消息的回调函数中对数据进行了转存和积分，这里是根据激光雷达一帧扫描期间，IMU积分得到的位姿变换，通过插值的形式，对点云畸变进行校正。由于激光雷达与IMU坐标系定义不同，因此函数中先对坐标进行了交换，仅仅是为了将点云在IMU坐标系下表示。
   下面仅以姿态为例说明如何进行的畸变校正，对于某个激光pi，采集到该点时雷达本身的位姿Ti已经与采集第一个点p0时的位姿T0不同了，如果我们还认为这个点也是在T0处获取到的，这是不准确的，特别是当雷达扫描频率比较低，且雷达运动比较剧烈的情况下，所以，需要根据T0和Ti之间的相对变换，对点pi进行补偿，补偿的结果，就是这个Ti位姿处观测到的pi，如果是在T0处被观测到的，它的坐标应该是怎样的，这样就消除了雷达本身运动周期的影响，经过畸变消除，可以认为，所有点都是一瞬间在T0处采集得到的，而不存在内畸变的问题。然后，问题就变成了如何确定Ti与T0之间的相对位姿，我们知道IMU一直在计算积分，频率远远比雷达扫描要高，但是两者的时间戳是离散的并不是一一对应的，因此，采集到pi点的时刻ti时雷达的位姿并不能直接给出来，但是，IMU可以给出前后两个距离ti非常接近的时刻积分出来的位姿，这个由于IMU频率非常高所以两者之间时间非常短，可以通过线性插值的形式确定ti时刻的位姿Ti。

2. calculateSmoothness：计算曲率。根据两侧的10个点，计算曲率，这里的曲率并没有特别准确的实际意义，只是一个量的大小的概念，如果有，那就是该点与周围10个点的均值p之间欧式距离的平方的100倍，这里并没有进行开根号操作，是为了减少不必要的计算，因为曲率并不参加最终的优化，只是衡量一个点光滑与否的标志，是个相对的概念。需要注意cloudSmoothness这个vector中保存了曲率与点索引的对，后面根据曲率对点进行排序可以直接得到点的索引顺序。

3. markOccludedPoints：对于深度变化比较明显的相邻两个点，远处的那一个更容易被遮挡，不稳定；如果某个点，和两侧的点距离差距都比较大，那么有可能是噪声等，也不稳定。在cloudNeighborPicked中标注为1,后面的特征提取中不会再考虑。

4. extractFeatures：根据曲率的大小，提取面点和焦点，这里的流程和LOAM基本一样。首先进行分区，把每个线分成了6个区，在每个区内提取一定数量的角点和面点，这是为了让点分布更均匀，匹配时效果会更稳定更好。然后就是在一个区的范围内，按照曲率对索引进行排序，曲率最大的20个非地面点为角点，其中最最大的2个强角点，两者是包含的关系；曲率最最小的4个地面点为强面点，其余所有地面点都是一般面点，点数太多需要进行降采样。还有一个细节就是，在确定提取某个点后，其周围一定范围内的点cloudNeighborPicked会被标注为1,不会被选中，这是为了防止提取出来的特征点过度聚集在某一处。

5. publishCloud：发布4种特征点。

到这里特征提取的工作已经完成了，在LOAM里面，这个其实是作为单独的一个node存在的。

Feature Association部分，主要完成的是一个激光里程计：

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

    updateInitialGuess();  //  更新初始位姿

    //  一个是找特征平面，通过面之间的对应关系计算出变换矩阵。
    // 另一个部分是通过角、边特征的匹配，计算变换矩阵。
    // jin: 依次使用平面点和角点(都是sharp和flat点,非一般点),对相对位姿进行优化,保存在成员变量matP中
    // 特点在于,先使用平面点经历多次优化后,再使用角点(可能因为平面点更多,因而会更稳定吧)
    updateTransformation();

    // jin: 累计位姿到transformSum中
    integrateTransformation();  //  计算旋转角的累积变化量

    // jin: 发布里程计位姿,发布tf变换
    publishOdometry();

    // jin: 将当前帧数据统一到最终点时刻，重置作为last kd-tree,同时发布角点和面点(都是一般的)
    publishCloudsLast(); // cloud to mapOptimization

1. checkSystemInitialization：激光里程计初始化。激光里程计是连续帧之间匹配的过程，需要有一个参考帧，如果系统没有经过初始化，那么就把当前帧作为参考帧，并初始化当前总的位姿transformSum（除了俯仰角和滚转角外，其余参数全部初始化为0）。

2. updateInitialGuess：根据IMU积分的结果，估计一个初始位姿transformCur，这个位姿指的是激光雷达采集到最后一个点时，相对于采集到第一个点时，雷达的发生的相对位姿变换。

3. updateTransformation：通过约束优化相对位姿。对于当前帧的每一个面点，在上一帧找到最近邻以及另外两个不共线的点组成面，该点到面的距离就是要减小的残差，对于角点，就找到最近邻和另外一个点构建约束，被优化的对象就是transformCur，即找到一个相对位姿变换，使得总体的残差最小。需要注意的是，上一帧的点云已经被统一到上一帧的结束时刻，也就是当前帧的起始时刻，因此，这里的匹配是当前帧的终止位姿相对于起始位姿的，即transformCur，这是没有问题的。

4. integrateTransformation：把优化得到的transformCur，累加到transformSum中。

5. publishOdometry：发布里程计位姿位姿和tf变换，这里的变换实际上是odo相对于地图原点的，存在持续的误差积累的;

6. publishCloudsLast：根据优化的结果transformCur，将当前帧特征点云统一到终止时刻，用来作为下次匹配的参考（这也呼应了我在步骤3中最后的表述），同时发布特征点云。

到这里通过连续帧间匹配的方式，odo已经把当前雷达所在的位姿优化到transformSum处，并将点云校正到此处再次进行发布。但是，这是里程计连续帧间匹配的结果，会存在持续的误差的积累的过程，与在地图中的实际位姿可能已经发生了非常大的差距，但是，短时间内的结果都还是准确的。接下来 ，就是如何通过与地图这种绝对的参考进行匹配，对这种积累误差进行修正。

参考：
SC-Lego-LOAM解析（上）
SC-Lego-LOAM解析（中）
SC-Lego-LOAM解析（下）

本文经允许后转自知乎：https://zhuanlan.zhihu.com/p/348281520