LeGO-LOAM分析之点云分割（一）

阳光穿透心脏的1/2处 2022-09-12 05:41 218阅读 0赞

转载地址：[https://zhuanlan.zhihu.com/p/382460472][https_zhuanlan.zhihu.com_p_382460472]  
LeGO-LOAM是一种激光雷达SLAM算法，对应的论文为《LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain》，同时有开源代码。

下面我们将结合论文和代码对LeGO-LOAM做一个简单分析，LeGO-LOAM一共分为4个流程，代码结构上也大致按照这4个流程进行划分，因此接下来也会分4个章节来介绍LeGO-LOAM的算法流程。本章首先介绍LeGO-LOAM的主流程，然后重点介绍**点云分割**的流程。

### LeGO-LOAM介绍 ###

一开始以为LeGO-LOAM的LeGO是代表乐高积木，可以类似乐高积木的方式来搭建SLAM算法，直到看到论文才搞清楚，LeGO的Le表示轻量级（Lightweight），GO表示基于地面优化（Ground-Optimized）。也就是说LeGO-LOAM一是一个轻量级，基于地面优化的激光雷达SLAM算法。

LeGO-LOAM采用的硬件平台是Jackal UGV，整体的系统架构如下图。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_15_color_FFFFFF_t_70_g_se_x_16]  
LeGO-LOAM一共分为4个步骤。

1.  Segmentation （filter out noise）
2.  Feature Extraction（distinctive planar and edge features）
3.  Lidar Odometry ：Label Matching, Two-step L-M Optimization
4.  Lidar Mapping（pose-graph SLAM）

首先是对输入的原始点云进行点云分割（Segmentation），找到地面并且进行点云分割，接着对分割好的点云进行特征提取（Feature Extraction），找到面特征和边特征，提取出特征之后接下来进行特征匹配，并且输出位姿，最后对点云进行注册，生成全局地图，并且进行回环检测，对生成的地图进行优化。

接下来我们开始介绍点云分割。

### 点云分割 ###

点云分割主要的流程是先进行地面提取，然后对剩下的点云进行分割，最后拿分割好的点云进行到下一步的特征提取。

由于机器人可能在复杂的环境中，小物体（例如树叶）可能会形成微不足道且不可靠的特征，因为在两次连续扫描中不太可能看到相同的树叶。为了提高效率和可靠性，分割的时候忽略了少于30个点的集群（实际上代码中也保留了5个以上点，并且横向线数大于3的集群）。

在这个过程之后，只保留可能代表大物体的点，例如树干和地面点以供进一步处理。下图中(a)是原始点云，(b)是分割后的点云，红色代表地面，剩余点是分割后的点云，可以看到去除了大量的噪音。  
![在这里插入图片描述][548ffd5ed6e04b5b905c14f55fb4ef8e.png]

### 代码分析 ###

点云分割对应的代码在"LeGO-LOAM/src/imageProjection.cpp"中，LeGO-LOAM的4个流程都是独立运行的ROS程序，入口函数如下

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;
    }

主要的工作流程实际上在"ImageProjection IP"中，那么为什么没有任何函数执行就可以工作呢？实际上在ImageProjection类的构造函数中，创建了订阅消息和回调，这样只要ImageProjection类IP一创建就会执行构造函数，也就开启了发布订阅流程，在点云消息到来之后，就会触发点云处理回调。

我们接着看构造函数

ImageProjection():
            nh("~"){
        
    // 1. 点云回调，主要的工作流程
            subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
    
    // 2. 以下为发布地面点云和分割好的点云
            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;
    
    // 3. 申请资源
            allocateMemory();
    // 4. 初始化
            resetParameters();
        }

> 源码中在构造函数申请了资源，有些是智能指针，会自动释放资源，但是另外有4个数组不会，会产生资源泄露。最好在析构函数中进行资源释放。

直到这里才找到真正的点云处理函数，也就是点云的回调函数"cloudHandler"。cloudHandler的流程非常清晰，分为7步。

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        
    
            // 1. 将 ros 消息转换为 pcl 点云
            copyPointCloud(laserCloudMsg);
            // 2. 扫描的开始和结束角度
            findStartEndAngle();
            // 3. 距离图像投影
            projectPointCloud();
            // 4. 标记地面点云
            groundRemoval();
            // 5. 点云分割
            cloudSegmentation();
            // 6. 发布分割后的点云
            publishCloud();
            // 7. 为下一次迭代重置参数
            resetParameters();
        }

接下来我们逐步分析这7个过程，其中主要的过程是“**3. 距离图像投影、4. 标记地面点云和5. 点云分割**”

### 1. copyPointCloud ###

将 ros 消息转换为 pcl 点云，这里对velodyne的lidar做了区分处理，以16线雷达为例，激光雷达通过16根雷达光束进行旋转扫描，从而得到一周360°的点云。velodyne的lidar对点云属于16线中的哪一线做了标记，这个标记被称为Ring index，其中的Ring表示环的意思。如果不是velodyne的lidar后面会通过计算得出点云属于哪个线束。因此保存点云的时候分别保存为"laserCloudIn"和"laserCloudInRing"中。

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. 去除点云中的Nan points
            std::vector<int> indices;
            pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
            // 3. 如果点云有"ring"通过，则保存为laserCloudInRing
            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();
                }  
            }
        }

### 2. findStartEndAngle\* ###

查找起始角度和终止角度，这里的角度是激光雷达旋转一周的起始角度和终止角度，选取的起点是points\[0\]，选取的终点是points\[laserCloudIn->points.size() - 1\]，是否是说点云是按照顺序排列的呢？另外这里最后一个点并不一定是最大角度。

此外还对x,y进行了坐标转换，关于这一步的结果会在特征提取中用到。

void findStartEndAngle(){
        
            // start and end orientation of this cloud
            segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
            segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                         laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_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;
        }

### 3. projectPointCloud ###

这一步把点云投影到RangeImage上，也就是说得到的原始点云是一个球形，我们把球形投影到一个圆柱体上然后把它展开，下图是一个简单的示例。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16]  
以16线激光为例，转换的range图，长为16，宽为旋转一周的采样次数，这里为1800，因此最后得到的是16\*1800的图像。这个图像可以用数组表示，其中的坐标为x,y，而z会转换为深度信息，最后转换好的rangeImage效果如下图，类似一个深度图。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16 1]  
了解了原理之后我们下面开始分析代码

void projectPointCloud(){
        
            // range image projection
            float verticalAngle, horizonAngle, range;
            size_t rowIdn, columnIdn, index, cloudSize; 
            PointType thisPoint;
    
            cloudSize = laserCloudIn->points.size();
            // 1. 遍历点云
            for (size_t i = 0; i < cloudSize; ++i){
        
                thisPoint.x = laserCloudIn->points[i].x;
                thisPoint.y = laserCloudIn->points[i].y;
                thisPoint.z = laserCloudIn->points[i].z;
                // 2.1 如果有ring index则直接采用
                if (useCloudRing == true){
        
                    rowIdn = laserCloudInRing->points[i].ring;
                }
                else{
        
                    // 2.2 如果没有则通过计算角度得到index
                    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;
                // 3. 计算横坐标
                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;
                // 4. 计算距离，如果小于1M则过滤掉，通常用来过滤自身形成的点云
                range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
                if (range < sensorMinimumRange)
                    continue;
                // 5. 保存距离到矩阵rangeMat
                rangeMat.at<float>(rowIdn, columnIdn) = range;
                // 6. 这里的强度实际上为了保持纵坐标和横坐标
                thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
                // 7. 把点云保存到数组，fullCloud的强度为横纵坐标，fullInfoCloud中的为距离
                index = columnIdn  + rowIdn * Horizon_SCAN;
                fullCloud->points[index] = thisPoint;
                fullInfoCloud->points[index] = thisPoint;
                fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
            }
        }

### 4. groundRemoval ###

接下来的处理都是基于rangeImage，去除地面的过程如下，对rangeImage中行Ringindex小于7的点进行地面检测，如果是32线雷达，则是20以下的，所以这里是利用了Lidar的先验知识。  
![在这里插入图片描述][77c5fc79c55c4b3ea58ee42f07946eec.png]  
地面检测的原理如上图所示，通过判断rangeImage中A和相邻点B之间的夹角ɑ是否足够小，如果角度足够小则判断为地面。这里选择的角度阈值为10°，如果小于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
            // 1. 遍历小于7的点
            for (size_t j = 0; j < Horizon_SCAN; ++j){
        
                for (size_t i = 0; i < groundScanInd; ++i){
        
    
                    lowerInd = j + ( i )*Horizon_SCAN;
                    upperInd = j + (i+1)*Horizon_SCAN;
    
                    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;
                    // 2. 计算垂直的2个点的夹角
                    angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
                    // 3. 如果小于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
            // 4. 如果为地面或者rangeMat为空，则标记为-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;
                    }
                }
            }
            // 5. 这一步主要是为了发布地面点云
            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]);
                    }
                }
            }
        }

### 5. cloudSegmentation ###

在去除地面之后，对接下来的点进行分割。这里通过BFS（深度优先遍历）递归进行查找，从\[0,0\]点开始，遍历它前、后、左、右的4个点，分别进行对比，如果相对角度大于60°，则认为是同一个点云集群。最后分割出来的点云数量大于30个则认为分割有效（实际上大于5个可能也行）。

这里对cloudSegmentation拆分成2部分进行介绍，先进行标记labelComponents，也就是递归找到特征比较明显的点。

void cloudSegmentation(){
        
            // segmentation process
            for (size_t i = 0; i < N_SCAN; ++i)
                for (size_t j = 0; j < Horizon_SCAN; ++j)
                    // 1. 如果没有标记过，则遍历该节点（递归调用）
                    if (labelMat.at<int>(i,j) == 0)
                        labelComponents(i, j);

下面来我们先分析labelComponents，然后再继续介绍cloudSegmentation。

作者说用vector和deque会变慢速度，不知道是不是数组的动态扩展导致的（当预先分配的vector大小不够时候，会重新malloc2倍的大小，然后把原先的数组拷贝过去，系统自动完成），如果设置固定大小，我认为不会导致明显的性能下降。这里作者用4个数组来代替了queue的操作，看起来有点别扭。

点云分割的原理如下图所示，A和B是Lidar扫描的连续2个点云，如果A和B的夹角β非常大，例如下图中红色的线（代码中为60°）那么证明这2个点不属于同一个物体，反之，如果这2个点的夹角β比较小，例如下图中绿色的线，则可以认为是同一个物体，通过这个方法对点云进行分割。  
![在这里插入图片描述][watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_19_color_FFFFFF_t_70_g_se_x_16]

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;
            // 1. 初始值为[row, col]，长度为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
                // 2.遍历前后左右4个点，计算角度差
                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));
                    // 2.1 纵向角度和横向角度
                    if ((*iter).first == 0)
                        alpha = segmentAlphaX;
                    else
                        alpha = segmentAlphaY;
    
                    angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
                    // 2.2 如果角度大于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;
            // 3. 如果点大于30个，认为成功
            if (allPushedIndSize >= 30)
                feasibleSegment = true;
            // 4. 如果点大于5个，并且横跨3个纵坐标，认为成功
            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
            // 5. 如果为真，则labelCount加1，labelCount代表分割为了多少个簇
            if (feasibleSegment == true){
        
                ++labelCount;
            }else{
         // segment is invalid, mark these points
                // 6. 如果为假，那么标记为999999
                for (size_t i = 0; i < allPushedIndSize; ++i){
        
                    labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
                }
            }
        }

接下来继续分析cloudSegmentation，主要是根据上述标记好的点云，生成分割好的点云。

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;
                        }
                        // 3. 这里有可能是地面，也有可能是分割之后的点
                        // 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
            // 4. 分割后的点云，不包含地面
            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);
                        }
                    }
                }
            }
        }

### 6. publishCloud ###

接下来就是发布分割好的点云，这个函数非常简单，主要是要搞清楚各个点云的作用，代码如下

void publishCloud(){
        
            // 1. Publish Seg Cloud Info
            // 1. 发布点云，包括采样后的地面和分割后的点云
            segMsg.header = cloudHeader;
            pubSegmentedCloudInfo.publish(segMsg);
            // 2. Publish clouds
            // 2. 发布离群后的点云
            sensor_msgs::PointCloud2 laserCloudTemp;
    
            pcl::toROSMsg(*outlierCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubOutlierCloud.publish(laserCloudTemp);
            // segmented cloud with ground
            // 3. 同1
            pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubSegmentedCloud.publish(laserCloudTemp);
            // projected full cloud
            // 4. rangeimage投影后的点云
            if (pubFullCloud.getNumSubscribers() != 0){
        
                pcl::toROSMsg(*fullCloud, laserCloudTemp);
                laserCloudTemp.header.stamp = cloudHeader.stamp;
                laserCloudTemp.header.frame_id = "base_link";
                pubFullCloud.publish(laserCloudTemp);
            }
            // original dense ground cloud
            //  5. 地面点云
            if (pubGroundCloud.getNumSubscribers() != 0){
        
                pcl::toROSMsg(*groundCloud, laserCloudTemp);
                laserCloudTemp.header.stamp = cloudHeader.stamp;
                laserCloudTemp.header.frame_id = "base_link";
                pubGroundCloud.publish(laserCloudTemp);
            }
            // segmented cloud without ground
            // 6. 分割后的点云，不包含地面
            if (pubSegmentedCloudPure.getNumSubscribers() != 0){
        
                pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
                laserCloudTemp.header.stamp = cloudHeader.stamp;
                laserCloudTemp.header.frame_id = "base_link";
                pubSegmentedCloudPure.publish(laserCloudTemp);
            }
            // projected full cloud info
            // 7. rangeimage投影后的点云
            if (pubFullInfoCloud.getNumSubscribers() != 0){
        
                pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
                laserCloudTemp.header.stamp = cloudHeader.stamp;
                laserCloudTemp.header.frame_id = "base_link";
                pubFullInfoCloud.publish(laserCloudTemp);
            }
        }

### 7. resetParameters ###

最后一步是清空临时变量，没有太多需要说明的。

void resetParameters(){
        
            laserCloudIn->clear();
            groundCloud->clear();
            segmentedCloud->clear();
            segmentedCloudPure->clear();
            outlierCloud->clear();
    
            rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
            groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
            labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
            labelCount = 1;
    
            std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
            std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
        }

### 总结 ###

至此整个LeGO-LOAM点云分割的部分就介绍完毕了，如果需要自己适配，需要关注下"LeGO-LOAM/include/utility.h"中不同型号Lidar的参数，比如夹角和地面查找的线束。

关于文章中点云分割的原理，**上面的2幅图都是引用至下面2篇论文，如果需要加深理解，请参考下面2篇论文。**

*  《Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation》 2016 IROS
 *  《Efficient Online Segmentation for Sparse 3D Laser Scans》 2017

[https_zhuanlan.zhihu.com_p_382460472]: https://zhuanlan.zhihu.com/p/382460472
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_15_color_FFFFFF_t_70_g_se_x_16]: /images/20220828/59172875a3bf496896408fb2dc9c3539.png
[548ffd5ed6e04b5b905c14f55fb4ef8e.png]: /images/20220828/08ad8a25629548b5bdceefb9a3d3dd9f.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16]: /images/20220828/2a4c015da7ca44f5a13f07860d82ce39.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_20_color_FFFFFF_t_70_g_se_x_16 1]: /images/20220828/9532a0756b354c80bdf2b1e8413237f5.png
[77c5fc79c55c4b3ea58ee42f07946eec.png]: /images/20220828/600d46b6f0ed4818a7eecded403058c2.png
[watermark_type_ZHJvaWRzYW5zZmFsbGJhY2s_shadow_50_text_Q1NETiBAQ0NDcnVubmVy_size_19_color_FFFFFF_t_70_g_se_x_16]: /images/20220828/23689b8cd8574c61963437adc58e59a9.png