【点云处理】LeGO-LOAM在robosense激光雷达上的地面点云分割实验

    LeGO-LOAM是一种激光雷达SLAM算法，算法在软件架构上分为5个流程，分别是：

1. 分割聚类：这部分主要操作是分离出地面点云；同时，对剩下的点云进行聚类，剔除噪声(数量较少的点云簇，被标记为噪声)；

2. 特征提取：对分割后的点云(排除地面点云部分)进行边缘点和面点特征提取；

3. Lidar里程计：在连续帧之间(边缘点和面点)进行特征匹配找到连续帧之间的位姿变换矩阵；

4. Lidar Mapping：对特征进一步处理，然后在全局的 Point Cloud Map 中进行配准；

5. Transform Integration：Transform Integration 融合了来自 Lidar Odometry 和 Lidar Mapping 的 pose estimation 进行输出最终的 pose estimate。

本次实验主要是探究LeGO-LOAM中的分割聚类算法在一款Robosense激光雷达上的分割效果，雷达参数信息大致如下，其中垂直视场角经实际测算应该是-25度到+0.2度，应该是属于路侧版本，而官网中给的是车载版本的参数。

1. 地面分割的思路

        Lego-LOAM给出的地面分割的思路比较简单，如上图所示，相连两条扫描线打在地面同一列的两点A和B。计算A,B两点的高度差Hz=|Az-Bz|以及平面欧式距离Dxy。计算Hz和Dxy构建的三角形的角度值=atan2(Hz,Dxy)，如果<10，则认为两点A,B属于地面点。

 2.地面分割相关代码

2.1 文件结构

(base) ➜  LeGO-LOAM git:(master) ✗ tree -L 2
.
├── CMakeLists.txt
├── include
│   └── utility.h           #关键配置参数
├── launch
│   ├── block.png
│   ├── dataset-demo.gif
│   ├── demo.gif
│   ├── google-earth.png
│   ├── jackal-label.jpg
│   ├── odometry.jpg
│   ├── run.launch
│   ├── seg-total.jpg
│   └── test.rviz
├── package.xml
└── src
    ├── featureAssociation.cpp
    ├── imageProjection.cpp     #地面分割相关代码
    ├── mapOptmization.cpp
    └── transformFusion.cpp

2.2 关键参数配置

Lego-LOAM项目对VLP-16激光雷达做了适配，同时也在参数配置文件LeGO-LOAM/LeGO-LOAM/include/utility.h中还加入了HDL-32E、VLS-128、Ouster OS1-16、Ouster OS1-64等激光雷达的关键参数。不好意思的是并没有我手上的这款Robosens RS-Ruby Lite，我将訪款雷达的参数加入到配置文件中。

// RS-Ruby Lite
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used
extern const int N_SCAN = 80;   //激光雷达的扫描线数
extern const int Horizon_SCAN = 1800;   //一条线束1800个点,360度环绕
extern const float ang_res_x = 360./float(Horizon_SCAN); //单条线束的水平分辨率
extern const float ang_res_y = (25.+0.2)/float(N_SCAN);  //单条线束的垂直分辨率,我经过实际验证手上这款RS-Ruby Lite雷达的垂直视场角是-25度至+0.2度.
extern const float ang_bottom = 25.;  //从下往上第1根线的角度绝对值
extern const int groundScanInd = 76;    // 地面线数,因为激光雷达是水平放置,并非所有线都会扫向地面

LeGO-LOAM在检测地面点时并非遍历所有扫描线，因为雷达是水平旋转，会有部分扫描线是打向天空。框架代码中只取groundScanInd参数设置的扫描线。这个跟你实际安装的位置有很大的关系。

2.3 分割核心代码

地面提取功能在LeGO-LOAM/src/imageProjection.cpp文件中，由groundRemoval函数实现。

    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){   //遍历列,e.g. 1800
            for (size_t i = 0; i < groundScanInd; ++i){ //遍历行, e.g. 地面线束

                //同一列相连两行的点云id
                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                //如果所取的两点存在无效点,该点lowerInd或者(i,j)在点云地图groundMat中也为无效点
                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    // no info to check, invalid points
                    groundMat.at(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;

                // 相邻两条扫瞄线打在地面同一列的两点A([ax,ay,az]),B([bx,by,bz])
                // 计算A,B高度差hz=|az-bz|,平面距离dxy=sqrt((ax-bx)^2 + (ay-by)^2)
                // 计算hz,dxy构建的三角角度theta = atan2(hz,dxy)
                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;
                }
            }
        }
        // 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
        // 在labelMat中移除地面点和无效点,labelMat对所有点进行了聚类标记,
        // 我们要在其中剔除地面点和无效点,无效点可以理解为未收到返回信号的点
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                //如果为地面或者rangeMat为空，则标记为-1，后面去除这些点
                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]);
                }
            }
        }
    }

3. 看效果

代码实际运行后，订阅不同的topc可以看到分割效果：

• 订阅/rslidar_points可以看到原始点云。
• 订阅/ground_cloud查看提取出来的地面点。

 

• 订阅/segmented_cloud查看剔除地面点以后的点。

• 同时订阅/rslidar_points和/ground_cloud。

总的来说，未经优化直接使用地话在訪款雷达上地面分割效果不佳。主要表现在近处地面点提取不出来，误检的现象比较严重。 

【参考】

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

论文：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/Shan_Englot_IROS_2018_Preprint.pdf
YouTube：https://www.youtube.com/watch?v=O3tz_ftHV48