LeGO-LOAM源码解析2： imageProjection

loam源码地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.

论文学习：【论文阅读】LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain.

LeGO-LOAM源码解析汇总：

LeGO-LOAM源码解析1 ： 算法整体框架和utility.h.

一、概述

在看完LeGO-LOAM的整体框架和头文件后，我们开始分阶段的阅读各部分源文件。

随着点云信息的输入，首先来到的是imageProjection.cpp文件，在这里主要的工作对点云的分割，并将分割好的点云送往featureAssociation.cpp。LOAM把所有的实现全部都放在了mian函数中，逻辑结构和代码分层不够明显，也存在一定的冗余；与LOAM中的代码书写有了较大的不同，LeGO-LOAM虽然也是分为四个部分，四个mian函数，但是对每一个部分都定义了一个类对函数和变量进行封装，结构脉络清晰。

二、main函数

主函数相对来说比较简单，主要是初始化节点、定义自定义类ImageProjection的对象，以及等待回调函数，对应代码如下：

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类的私有对象

在我们看该类的构造函数之前，我们先查看一下ImageProjection的私有对象，即定义的一些变量。

定义节点句柄：

private:
    ros::NodeHandle nh;

定义接受点云的订阅者：

    ros::Subscriber subLaserCloud;

定义一系列与点云相关的发布者：

    ros::Publisher pubFullCloud;
    ros::Publisher pubFullInfoCloud;

    ros::Publisher pubGroundCloud;
    ros::Publisher pubSegmentedCloud;
    ros::Publisher pubSegmentedCloudPure;
    ros::Publisher pubSegmentedCloudInfo;
    ros::Publisher pubOutlierCloud;

定义一系列点云存储：

	//雷达传出的点云
    pcl::PointCloud<PointType>::Ptr laserCloudIn;
    pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;

	//全部点云
    pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
    //整体点云
    pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range

	//地面点云
    pcl::PointCloud<PointType>::Ptr groundCloud;
    //分割后的点云
    pcl::PointCloud<PointType>::Ptr segmentedCloud;
    //分割后的几何信息
    pcl::PointCloud<PointType>::Ptr segmentedCloudPure;
    //异常点云
    pcl::PointCloud<PointType>::Ptr outlierCloud;

    PointType nanPoint; // fill in fullCloud at each iteration

定义三个矩阵分别代表点云投影后的rang image 、 标签矩阵和地面矩阵：

    cv::Mat rangeMat; // range matrix for range image
    cv::Mat labelMat; // label matrix for segmentaiton marking
    cv::Mat groundMat; // ground matrix for ground cloud marking
    int labelCount;

一帧点云的起始角和结束角：

    float startOrientation;
    float endOrientation;

定义自定义的rosmsg和一些用于BFS的索引（点云分割时使用）：

    cloud_msgs::cloud_info segMsg; // info of segmented cloud
    std_msgs::Header cloudHeader;

    std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process

    uint16_t *allPushedIndX; // array for tracking points of a segmented object
    uint16_t *allPushedIndY;

    uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
    uint16_t *queueIndY;

关于自定义的ros消息类型cloud_msgs的结构如下：

Header header 

//分割的起始和结束的索引 长度：N_SCAN
int32[] startRingIndex
int32[] endRingIndex

//分割的起始和结束的角度和差值
float32 startOrientation
float32 endOrientation
float32 orientationDiff

//与点云分割相关 长度都是 N_SCAN*Horizon_SCAN
bool[]    segmentedCloudGroundFlag # true - ground point, false - other points
uint32[]  segmentedCloudColInd # point column index in range image
float32[] segmentedCloudRange # point range 

四、ImageProjection类的构造函数

关于ROS中句柄定义时nh("~")与nh的一些小区别：以ros::init(argc, argv, "lego_loam");为例，若定义方式为nh("~")，则使用该句柄发布任何主题的消息时，其消息名称前都会自动加上/lego_loam/消息名；而若定义方式为nh时，其消息名称则直接为/消息名。

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

五、初始化函数allocateMemory与resetParameters

1. 内存分配allocateMemory

allocateMemory函数主要涉及到变量的空间分配和初始赋值，只在构造函数中调用一次。
为各点云信息分配存储空间：

    void allocateMemory(){

        laserCloudIn.reset(new pcl::PointCloud<PointType>());
        laserCloudInRing.reset(new pcl::PointCloud<PointXYZIR>());

        fullCloud.reset(new pcl::PointCloud<PointType>());
        fullInfoCloud.reset(new pcl::PointCloud<PointType>());

        groundCloud.reset(new pcl::PointCloud<PointType>());
        segmentedCloud.reset(new pcl::PointCloud<PointType>());
        segmentedCloudPure.reset(new pcl::PointCloud<PointType>());
        outlierCloud.reset(new pcl::PointCloud<PointType>());

重置投影点云的大小：

        fullCloud->points.resize(N_SCAN*Horizon_SCAN);
        fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);

自定义消息的分配空间和初始化：

        segMsg.startRingIndex.assign(N_SCAN, 0);
        segMsg.endRingIndex.assign(N_SCAN, 0);

        segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);
        segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);
        segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);

neighborIterator用于求某个点的的4邻域点，在labelComponents中使用：

        std::pair<int8_t, int8_t> neighbor;
        neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
        neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
        neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
        neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);

索引重置：

        allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
        allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];

        queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
        queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
    }

2. 重置参数resetParameters

resetParameters在回调函数中每次都要调用一次，与allocateMemory不同的是，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);
    }

六、回调函数cloudHandler

每接受到一帧新的点云信息时，都会进入到回调函数中，由于代码的分类和封装，这里主要是调用接口函数即可。我们从回调函数可以简单分析一下分割的基本流程：

    void 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();
    }

除了我们已经看过的resetParameters函数以外，我们接下来的重点就是上面提到的六个函数。

1. 转换点云消息格式copyPointCloud

获取该帧点云的时间戳

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        cloudHeader = laserCloudMsg->header;
        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line

将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针：

        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
        // Remove Nan points

使用pcl库中的removeNaNFromPointCloud函数去除无效点：

        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

是否使用Ring(可以直接得到投影图像的行号)，更多详细的信息参考LeGO-LOAM源码解析1 ： 算法整体框架和utility.h.

        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

对于雷达旋转一圈，其实并不是从0度按某一方向旋转到360度，而是在起始时有一定偏差，而旋转也不是严格的360度，一般情况下要比360度稍微多几度。注意，我们的雷达坐标系为右前上（xyz），所以我们要找到点云的起始角和结束角。这里是以x轴为基准，采用的办法是使用收到的第一个点云点和结束的点云点的角度作为该帧点云的起始角和结束角。

我们一般认为${\theta }_{start}\in [-\pi ,\pi ]$，而${\theta }_{end}\in [\pi ,3\pi ]$。而两者的角度差不会与$2\pi$相差太多。
由上图可知，雷达是顺时针旋转的，所以计算角度$\alpha$是需要添加一个负号:$\alpha =-\arctan y/x$，对角度进行转换(这里到最后就是以x负轴为$-\pi$的顺时针计算角度)：

    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

遍历点云中所有的点：

    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){

计算每个点云在Rang Image中的行号，若使用了CloudRing则直接可以获取，若没有使用则进行计算：首先对于16线激光雷达而言，垂直角度范围为$[-15,15]$,每根激光线束的角度差为2度。Rang Image中的行号对应激光线束的编号，且大于0。所以，行号的计算方式为：$row=（{\theta }_{vertical}+15）/2$。

            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
            if (useCloudRing == true){
                rowIdn = laserCloudInRing->points[i].ring;
            }
            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;

计算每个点云在Rang Image中的列号，首先计算以y轴正方向的基准的水平角度：

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

接下来的处理比较繁琐，上面得到的horizonAngle是属于$[-\pi .\pi ]$,y轴正向为0度。先减去90度得到的列号是以y轴负方向为起始点、逆时针计算的，其范围在[Horizon_SCAN/4,3*Horizon_SCAN/4]，然后再对大于Horizon_SCAN的列数进行处理，可以得到以x轴负方向为起始点、逆时针计算的，其范围在[0，Horizon_SCAN]的列数，描述如下：

            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;

Rang Image中存储点云的深度信息：

            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < sensorMinimumRange)
                continue;
            
            rangeMat.at<float>(rowIdn, columnIdn) = range;

将点云信息存入fullCloud和fullInfoCloud，两者的区别在于第一个中的强度信息填入了行号和列号相关的信息，而后者填入的是深度信息：

            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            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

关于groundMat的说明：
=-1：无效值，表示无法判断是否为地面点；
=0： 代表初始值，不是地面点；
=1： 表示是地面点。

关于labelMat的说明：
=-1：无效值，不进行点云分割；
=0： 初始值；
=labelCount： 平面点；
=999999： 舍弃点。

关于rangeMat的说明：
FLT_MAX： 初始化值；
range： 点云的深度值。

在提取地面点之前首先需要明确的一点，在激光雷达安装时是水平的，一般认为最中间的激光线束与水平面平行，所以对于一个16线的激光雷达而言，地面点在前7束激光线之中提取。即groundScanInd=7，遍历前7束所有点云：

    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){
            for (size_t i = 0; i < groundScanInd; ++i){

对于地面点的判定：对于$i$束中的激光点，在$i+1$束找到对应点，这两点与雷达的连线所形成的向量的差向量如下图如所示，若该差向量与水平面所形成的夹角小于10度，则认为是一个地面点。（之前的理解有误，感谢大佬zkk9527的纠错）

                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;

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

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

5. 点云分割cloudSegmentation

(1) 点云聚类函数labelComponents

该函数对特征的检测进行了详细的描述，并且使用BFS以(row，col)为中心向外面扩散，针对于某一特定的点与其邻点的进行计算：

简单阐述一下BFS在这里的思想：首先假设传入点为V0，判断V0的四邻域内的点（V1、V2、V3、V4）是否在一个平面内，若都在一个平面内，则依次计算四个点的四邻域的点是否在一个平面内，若在一个平面内，则保存（下图表现为V5、V6、V7、V8），等到V1-V4的邻域都判断完，就开始判断V5-V18的四邻域，直到不再有点的四邻域点属于这个平面，则搜索结束。

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

                // 是个环状的图片，左右连通
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;

若搜索点的四邻域已被搜索过，则跳过：

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

计算搜索点与其四邻域某点是否在一个平面，基本思路如下图所示，若计算出来的angle角度越大（一般要大于60度），则说明在一个平面的可能性大。

计算点到雷达的距离：

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

水平分辨率和垂直分辨率对应的角度不同:

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

计算代表平面的angle：

				// 通过下面的公式计算这两点之间是否有平面特征
				// 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<int>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;

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

聚类结束后，当类中点的数目大于30或者数目大于30且线束大于3个时为有效类：

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

有效类之后标签+1，否则设置所有点无效：

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

(2) 点云分割

去除地面点与异常点后，调用labelComponents实现点云聚类：

    void cloudSegmentation(){
        // segmentation process
        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)
                    labelComponents(i, j);

去除每束激光前5个点：

        int sizeOfSegCloud = 0;
        // extract segmented cloud for lidar odometry
        for (size_t i = 0; i < N_SCAN; ++i) {

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

对于因为点数太少而无法聚类的点的一部分（列数为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)
                    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;
                        }
                    }

地面点云稀疏化（地面点中列数不是5的倍数，舍弃）：

                    // majority of ground points are skipped
                    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;
        }

若分割点云segmentedCloudPure被订阅， 赋值：

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

6. 发布消息publishCloud

在我们计算的过程中参考系均为本体参考系，frame_id为base_link。

发布自定义消息：

 // 发布各类点云内容
    void publishCloud(){
    	// 发布cloud_msgs::cloud_info消息
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);

        sensor_msgs::PointCloud2 laserCloudTemp;

发布异常点云：

		// pubOutlierCloud发布界外点云
        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubOutlierCloud.publish(laserCloudTemp);

发布分块点云（包括分割点云和地面点云）：

		// pubSegmentedCloud发布分块点云
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);

发布全部点云，fullCloud的点云中强度为深度：

        if (pubFullCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullCloud.publish(laserCloudTemp);
        }

发布地面点云

        if (pubGroundCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*groundCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubGroundCloud.publish(laserCloudTemp);
        }

发布分割点云（没有地面点云）：

        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
            pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubSegmentedCloudPure.publish(laserCloudTemp);
        }

fullInfoCloud的点云中强度为Range

        if (pubFullInfoCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullInfoCloud.publish(laserCloudTemp);
        }
    }