LeGo-LOAM 源码解析

A lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (placed horizontal) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.

LeGO-LOAM（激光SLAM，IMU+LiDAR），以LOAM为基础，实现与其同等的精度同时大大提高了速度。利用点云分割区分噪声，提取平面与边特征，使用两步LM优化方法求解位姿，并且添加回环检测减小漂移。

github：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

0、整体框架

首先看一下代码结构，cloud_msgs、launch、include、src（核心程序），接下来具体看一下：

• CMakeList.txt

...
find_package(GTSAM REQUIRED QUIET)
find_package(PCL REQUIRED QUIET)
find_package(OpenCV REQUIRED QUIET)
...
1
2
3
4
5

依赖库：GTSAM、PCL、OPenCV

GTSAM 是一个在机器人领域和计算机视觉领域用于平滑（smoothing）和建图（mapping）的C++库，采用因子图（factor graphs）和贝叶斯网络（Bayes networks）的方式最大化后验概率。

• launch/run.launch

节点：rviz、camera_init_to_map、base_link_to_camera、imageProjection、featureAssociation、mapOptmization、transformFusion

主要节点就是后4个，也是对应src目录下的核心程序。

• include/utility.h

定义数据结构：点云、话题、激光雷达参数、图像分割参数、特征提取参数、优化参数、建图参数

...
/*
    * A point cloud type that has "ring" channel
    */
struct PointXYZIR
{
   
    PCL_ADD_POINT4D;
    PCL_ADD_INTENSITY;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (uint16_t, ring, ring)
)
...
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

这里定义了一种包含 ring_id 的点云数据类型，velodyne 的激光雷达是有这个数据的，其它厂商的不一定有；区别于laser_id，ring_id是指从最下面的激光线依次向上递增的线号（0-15 for VLP-16）。

...
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

// VLP-16
extern const int N_SCAN = 16;//number of lasers
extern const int Horizon_SCAN = 1800;//number of points from one laser of one scan
extern const float ang_res_x = 0.2;//angle resolution of horizon
extern const float ang_res_y = 2.0;//angle resolution of vertical
extern const float ang_bottom = 15.0+0.1;
extern const int groundScanInd = 7;//表示地面需要的线圈数;
...
1
2
3
4
5
6
7
8
9
10
11
12

针对不同激光雷达的参数设置，要根据自己的实际情况去调整，假如使用数据集的数据，比如KITTI，就需要针对HDL-64E进行调整，而且由于该雷达垂直角分辨率不是均匀的，还需要自己编写后续的投影程序。

1、imageProjection —— 点云分割

将激光点云处理成图像

总体流程：订阅点云数据回调处理 -> 点云转换到pcl预处理 -> 截取一帧激光数据 -> 投影映射到图像 -> 地面移除 -> 点云分割 -> 发布点云数据 -> 重置参数;

0. main()

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

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

    ImageProjection IP;

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

    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13

在 main 函数中，只是实例化了一个对象 IP ，所以 一旦创建了一个对象，就进入到了 Class 中去执行构造函数。

      ImageProjection() : nh("~")
    {
   
		// 订阅原始的激光点云
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
		
        // 转换成图片的点云
        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;

        allocateMemory();
        resetParameters();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

class 中的构造函数中，订阅了原始的激光点云数据（subLaserCloud）然后就进入到了 回调函数（ImageProjection::cloudHandler）对点云数据进行处理。

1. cloudHandler()

订阅激光雷达点云信息之后的回调函数

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   
        // 1. Convert ros message to pcl point cloud    将ros消息转换为pcl点云
        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();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

回调函数中按照以上 7 个逻辑步骤对点云进行处理，主要的逻辑步骤通过一个个封装好的函数呈现出来。

2. copyPointCloud()

由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息，所以在处理的过程中统一需要对消息类型转换，通常的办法就是使用 PCL 库

    // 复制点云   将ros消息转换为pcl点云
    // 使用pcl库函数;
    
    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   
         // 1. 读取ROS点云转换为PCL点云
        cloudHeader = laserCloudMsg->header;
        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

        //2.移除无效的点云 Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

        // 3. have "ring" channel in the cloud or not
        // 如果点云有"ring"通过，则保存为laserCloudInRing
        // 判断是不是使用了 velodyne 的激光雷达
        if (useCloudRing == true){
   
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false) {
   
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }  
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

​这个函数主体内容分为三个步骤，其中的第三步注意区分自己使用的激光雷达是否存在 CloudRing，没有这个的激光雷达接下来的步骤都不会去执行，所以在实际跑这个框架的时候一定要注意这个地方

3. findStartEndAngle()

    // 找到开始结束角度
    // 1.一圈数据的角度差，使用atan2计算;
    // 2.注意计算结果的范围合理性
    
    void findStartEndAngle(){
   
        // start and end orientation of this cloud
        // 1.开始点和结束点的航向角 (负号表示顺时针旋转)   
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);

        // 加 2 * M_PI 表示已经转转了一圈
        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
        
        // 2.保证所有角度落在 [M_PI , 3M_PI] 上
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
   
            segMsg.endOrientation -= 2 * M_PI;
        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

这个函数在 LOAM 代码里面也存在。
首先通过一个反正切函数，找到一帧激光点云起始时刻和结束时刻的航向角，第二步的作用是将一帧激光点云的航向角度限制在 [pi , 3pi] 之间，方便后续计算出一帧激光点云的时间。

4. projectPointCloud()

距离图像投影
将3D point cloud投影映射到2D range image
将激光雷达数据投影成一个16x1800（依雷达角分辨率而定）的点云阵列

    // 距离图像投影
    // 将3D point cloud投影映射到2D range image
    // 将激光雷达数据投影成一个16x1800（依雷达角分辨率而定）的点云阵列
    void projectPointCloud(){
   
        // range image projection
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();

        // 遍历整个点云 
        for (size_t i = 0; i < cloudSize; ++i){
   
            // 提取点云中 x y z 坐标数值
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;

            // find the row and column index in the iamge for this point
            // 判断是不是使用了 velodyne 的雷达
            if (useCloudRing == true){
   
                // 提取激光雷达线束到 rowIdn 
                rowIdn = laserCloudInRing->points[i].ring;
            }
            //  是其他的雷达 就通过俯仰角 确定当前的激光点是来自哪个线束 index 
            else{
   
                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;  //确定行索引
            }
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            // 保证 columnIdn 的大小在 [0 , 1800)
            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; //确定列索引
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            // 如果距离小于 1米 则过滤掉 通常是过滤自身（距离传感器比较近的点）
            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);    //确定深度
            if (range < sensorMinimumRange)
                continue;
            
            // 将计算下来的距离传感器的 数值保存到 rangeMat 中 
            // 这是一个 16 * 1800 的矩阵 rowIdn为线束数值  columnIdn 是 一圈圆形 滩平之后的数值
            // range  是特征点云点到原点的数值
            // 这样就将一个三维的坐标 转换到一个 矩阵中了.
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            // 将 index 和 横坐标存储在 intensity 中 整数部分是线束数值  小数部分是方向角度
            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            //深度图的索引值  index = 列号 +  行号 * 1800 
            index = columnIdn  + rowIdn * Horizon_SCAN;

            // fullCloud 存放的是坐标信息（二维的）
            // fullInfoCloud 增加了点到传感器的距离信息（三维的） 
            fullCloud->points[index] = thisPoint;
            fullInfoCloud->points[index] = thisPoint;

            // intensity 中存放的是点云点到传感器的距离
            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72

此处对激光点云的处理是 LEGO-LOAM 和 LOAM 的第一个区别。这里将一帧激光点云处理成一个 16 * 1800 像素的图片。 16代表的是激光雷达的线束，1800代表的是激光扫描一圈时候是 间隔0.5度发射一个激光束 所以 360 * 0.5 = 1800

​ 再者，rangeMat.at(rowIdn, columnIdn) 这里调用了一个 opencv 的多维数组，数组中的行与列 对应的就是图片的长和宽，然后数组中存放的数值就是图片相应位置上面的点到激光雷达的距离。

​ 最后将点云的坐标信息存储在 intensity 中，采用的方法是：整数+小数 存储。整数部分存储的是激光线束数值，小数部分存储的水平航向值。

atan2(y，x);

反正切的角度等于X轴与通过原点和给定坐标点(x, y)的直线之间的夹角，结果为正表示从X轴逆时针旋转的角度，结果为负表示从X轴顺时针旋转的角度;

1. 确定行索引

   根据ring id或者根据坐标计算：
   

2. 确定列索引([ − π , π ] 转换为 [ 0 , 1800 ])：
   

3. 确定深度值：（注意数据的有效范围）
   

5. groundRemoval()

首先。判断地面点的标志就是相邻两个激光线束扫描到点的坐标，如果两个坐标之间的差值 或者两个坐标之间的斜率大于一个设定的值，则将该点

判断是地面点。所以此处用到了激光点云图片的深度信息

    // 移除地面点
    // 根据上下两线之间点的位置计算两线之间俯仰角判断，小于10度则为地面点
    
    void groundRemoval(){
   
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;
        
        // groundMat
        // -1, no valid info to check if ground of not  没有有效的信息确认是不是地面
        //  0, initial value, after validation, means not ground    确认不是地面点
        //  1, ground
        for (size_t j = 0; j < Horizon_SCAN; ++j){
   
            //  前7个激光雷达扫描线束足够满足地面点的检测 所以只遍历 7 次
            for (size_t i = 0; i < groundScanInd; ++i){
   
                //groundScanInd定义打到地面上激光线的数目
                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                // 如果之前计算过的 intensity 是 -1 则直接默认为是一个无效点
                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
   
                    // no info to check, invalid points  对这个无效点直接进行贴标签
                    groundMat.at<int8_t>(i,j) = -1;
                    continue;
                }
                    
                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;

                 //垂直方向相邻两点俯仰角小于10度就判定为地面点；相邻扫描圈
                if (abs(angle - sensorMountAngle) <= 10){
   
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }
            }
        }

        // extract ground cloud (groundMat == 1)
        // mark entry that doesn't need to label (ground and invalid point) for segmentation
        // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan

        // 给地面点 标记一个符号 为 -1 
        for (size_t i = 0; i < N_SCAN; ++i){
   
            for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
   
                    labelMat.at<int>(i,j) = -1;
                }
            }
        }

        // 发布点云信息，只发布地面点云信息
        if (pubGroundCloud.getNumSubscribers() != 0){
   
            for (size_t i = 0; i <= groundScanInd; ++i){
   
                for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                    if (groundMat.at<int8_t>(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                }
            }
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75

垂直相邻两点俯仰角小于10度就判定为地面点。

6. cloudSegmentation()

​从论文中可以看到，分割函数的来源参考了两篇论文 （ Effificient Online Segmentation for Sparse 3D Laser Scans ，另外一篇 Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation）的方法 。方法的原理可以参考知乎文章 《地面点障碍物快速分割聚类》 。

    // 点云分割
    // 首先对点云进行聚类标记，根据标签进行对应点云块存储;
    
    void cloudSegmentation(){
   
        // segmentation process
        for (size_t i = 0; i < N_SCAN; ++i)     // 16线的 一个线束一个的遍历
            for (size_t j = 0; j < Horizon_SCAN; ++j)       // 水平 的 1800
                if (labelMat.at<int>(i,j) == 0)     // 对非地面点进行点云分割
                    labelComponents(i, j);      //  调用这个函数对点云进行分割聚类

        int sizeOfSegCloud = 0;

        // extract segmented cloud for lidar odometry
        // 提取分割点云 用于激光雷达里程计
        for (size_t i = 0; i < N_SCAN; ++i) {
   

            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

            for (size_t j = 0; j < Horizon_SCAN; ++j) {
   
                if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
   
                    // outliers that will not be used for optimization (always continue)
                    // 勾勒出优化过程中不被使用的值

                    // 1. 如果label为999999则跳过
                    if (labelMat.at<int>(i,j) == 999999){
   
                        if (i > groundScanInd && j % 5 == 0){
   
                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                            continue;
                        }else{
   
                            continue;
                        }
                    }

                    // majority of ground points are skipped
                    // 2. 如果为地，跳过index不是5的倍数的点
                    if (groundMat.at<int8_t>(i,j) == 1){
   
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    // mark ground points so they will not be considered as edge features later
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    // mark the points' column index for marking occlusion later
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    // save range info
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                    // save seg cloud
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of seg cloud
                    ++sizeOfSegCloud;
                }
            }

            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }
        
        // extract segmented cloud for visualization
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
   
            for (size_t i = 0; i < N_SCAN; ++i){
   
                for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                    if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
   
                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
                    }
                }
            }
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79

7. labelComponents()

    // 标签组件
    // 局部特征检测聚类
    // 平面点与边缘点
    
    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};

        // 传进来的两个参数，按照坐标不同分别给他们放到 X 与 Y 的数组中
        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;
            // Mark popped point
            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)));      //平面聚类公式，角度越大两点越趋向于平面
                //segmentTheta=60度
                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;
                }
            }
        }

        // check if this segment is valid
        bool feasibleSegment = false;

        // 如果是 allPushedIndSize 累加的数量增加到了30 个 则判断这部分点云属于一个聚类
        if (allPushedIndSize >= 30)
            feasibleSegment = true;     //面点

        // 如果垂直 方向上点的数量大于 5个 默认是一个有效的聚类
        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;
            }
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

取去除地面的 labelMat，逐点开始计算邻域4点（邻域的索引要注意有效范围），与中心点比较出距离的最大值与最小值，通过下面公式结合阈值π / 3，大于该值则构成平面特征为平面点，队列存储索引，labelMat 存储标签值，同时 lineCountFlag 置 true，直到不满足阈值跳出进行下步。

聚类，超过30个点聚为一类，labelCount++；
小于30超过5，统计垂直方向聚类点数，超过3个也标记为一类；
若都不满足，则赋值999999表示需要舍弃的聚类点。

8. publishCloud()

发布各类点云数据

9. resetParameters()

参数重置

LeGO-LOAM首先进行地面分割，找到地面之后，对地面之上的点进行聚类。聚类的算法如下图，主要是根据斜率对物体做切割，最后得到地面和分割后的点云。上述步骤的关键是要理解如何进行地面分割和点云分割。

点云分割完成之后，接下来对分割后的点云提取特征，提取的特征的目的是进行点云配准，从而得出当前位姿。

2、featureAssociation —— 特征提取与匹配

订阅分割点云、外点、imu数据，总体流程：

• （特征提取部分）订阅传感器数据 -> 运动畸变去除 -> 曲率计算 -> 去除瑕点 -> 提取边、平面特征 -> 发布特征点云；
• （特征关联部分）将IMU信息作为先验 -> 根据线特征、面特征计算位姿变换 -> 联合IMU数据进行位姿更新 -> 发布里程计信息

0. main()

主函数内部做了两件事情，一个是实例化一个对象，一个是循环执行类方法

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

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

     // 1. 在构造函数中订阅消息和消息回调函数
    FeatureAssociation FA;

    ros::Rate rate(200);
    // 2. 循环调用runFeatureAssociation，函数的主流程所在
    while (ros::ok())
    {
   
        ros::spinOnce();

        FA.runFeatureAssociation();

        rate.sleep();
    }
    
    ros::spin();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

主函数里面首选实例化了一个对象 FA ，所以和之前的ImageProjection 处理过程一样，执行构造函数的内容，所以在构造函数中会订阅上一个cpp 文件中 pub 出来的消息。然后转到回调函数中处理数据，此处的回调函数都是将相应的信息存储到buff 中，所以不做详细分析。

消息回调主要是接收上一个步骤（点云分割）中分割好的点云消息。

    FeatureAssociation():
        nh("~")
        {
   
        // 1. 接收消息，上一步分割好的点云
        // 订阅了分割之后的点云
        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);
        
        // IMU传感器的消息
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
		
		// 2. 发布消息
        // 发布面特征点 和 边特征点
        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>("/l
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24