LeGo-LOAM源码阅读笔记(二)---imageProjecion.cpp

1. imageProjecion.cpp概述

imageProjecion.cpp进行的数据处理是图像映射，将得到的激光数据分割，并在得到的激光数据上进行坐标变换。下列是在投影过程中主要用到的点云：

    pcl::PointCloud<PointType>::Ptr laserCloudIn;//接受到的来自激光Msg的原始点云数据
    pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;//用 laserCloudInRing 存储含有具有通道R的原始点云数据

    //深度图点云：以一维形式存储与深度图像素一一对应的点云数据
    pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
    //带距离值的深度图点云:与深度图点云存储一致的数据，但是其属性intensity记录的是距离值
    pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range

    //注：所有点分为被分割点、未被分割点、地面点、无效点。
    pcl::PointCloud<PointType>::Ptr groundCloud;//地面点点云
    pcl::PointCloud<PointType>::Ptr segmentedCloud;//segMsg 点云数据:包含被分割点和经过降采样的地面点
    pcl::PointCloud<PointType>::Ptr segmentedCloudPure;//存储被分割点点云，且每个点的i值为分割标志值
    pcl::PointCloud<PointType>::Ptr outlierCloud;//经过降采样的未分割点

    //另外使用自创的rosmsg来表示点云信息
    // segMsg点云信息(存储分割结果并用于发送)
    cloud_msgs::cloud_info segMsg;

2.构造函数

imageProjecion()构造函数的内容如下：

1. 订阅话题：订阅来自velodyne雷达驱动的topic ,订阅的subscriber是subLaserCloud

   1. “/velodyne_points”(sensor_msgs::PointCloud2)

2. 发布话题，这些topic有：

   1. "/full_cloud_projected(sensor_msgs::PointCloud2)
   2. “/full_cloud_info”(sensor_msgs::PointCloud2)
   3. “/ground_cloud”(sensor_msgs::PointCloud2)
   4. “/segmented_cloud”(sensor_msgs::PointCloud2)
   5. “/segmented_cloud_pure”(sensor_msgs::PointCloud2)
   6. “/segmented_cloud_info”(cloud_msgs::cloud_info)
   7. “/outlier_cloud”(sensor_msgs::PointCloud2)

然后分配内存(对智能指针初始化)，初始化各类参数。

上述的cloud_msgs::cloud_info是自定义的消息类型，其具体定义如下：

Header header 

int32[] startRingIndex  // 长度：N_SCAN
int32[] endRingIndex    // 长度：N_SCAN

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 

关于上面的自定义消息，另外还需要说明的是，segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;或者segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;表示的是将第0线和第16线点云横着排列后。每一线点云有一个startRingIndex和endRingIndex,表示这一线点云中的一部分，如下图绿色部分。
黑色部分是整体这一线点云中筛选出来满足条件的。

3.cloudHandler

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)是这个文件中最主要的一个函数。由它调用其他的函数：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
    copyPointCloud(laserCloudMsg);
    findStartEndAngle();
    projectPointCloud();
    groundRemoval();
    cloudSegmentation();
    publishCloud();
    resetParameters();
}

整体过程如下：
1.复制点云数据
       点云数据函数copyPointCloud(laserCloudMsg) 是将ROS中的sensor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。
2.找到开始和结束的角度
3.点云投影
4.地面检测，移除地面点
5.点云分割
6.发布处理后的点云数据
7.重置参数，清空此次的点云变量

4.velodyne 雷达数据

从VLP给的雷达数据手册上（63-9243 Rev B User Manual and Programming Guide,VLP-16.pdf）查找一下它的坐标系定义：

velodyne雷达在上面的坐标系下输出"/velodyne_points"(sensor_msgs::PointCloud2) 的点云数据，其数据格式可以理解为x,y,z,intensity 这4个量。

5.findStartEndAngle

void findStartEndAngle()是将起始点与最末点进行角度的转换，进行关于segMsg(cloud_msgs::cloud_info segMsg)的三个内容的计算：
1）计算开始角度（segMsg.startOrientation）；
2）计算结束角度（segMsg.endOrientation）；
3）计算雷达转过的角度（开始和结束的角度差，segMsg.orientationDiff）。

关于具体计算，需要清楚整个雷达的坐标系定义，参考上面雷达坐标系的那张图。

另外在计算segMsg.startOrientation和segMsg.endOrientation时，atan2()函数取了负数的原因是：雷达旋转方向和坐标系定义下的右手定则正方向不一致。参考下图：

代码如下：

 //将起始点与最末点进行角度的转换
   void findStartEndAngle()
   {
       //计算角度时以x轴负轴为基准
       segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
       //因此最末角度为2π减去计算值
       segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 2].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;
   }

6.projectPointCloud

void projectPointCloud()是将点云逐一计算深度，将具有深度的点云保存至fullInfoCloud中。

void projectPointCloud()将激光雷达得到的数据看成一个16x1800的点云阵列。然后根据每个点云返回的XYZ数据将他们对应到这个阵列里去。

1. 计算竖直角度，用atan2函数进行计算。
2. 通过计算的竖直角度得到对应的行的序号rowIdn，rowIdn计算出该点激光雷达是水平方向上第几线的。从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16)。
3. 求水平方向上的角度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI。
4. 根据水平方向上的角度计算列向columnIdn。
   这里对数据的处理比较巧妙，处理的目的是让数据更不容易失真。
   计算columnIdn主要是下面这三个语句：

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;

先把columnIdn从horizonAngle:(-PI,PI]转换到columnIdn:[H/4,5H/4],然后判断columnIdn大小，再将它的范围转换到了[0,H] (H:Horizon_SCAN)。
这样就把扫描开始的地方角度为0与角度为360的连在了一起，非常巧妙。
5. 接着在thisPoint.intensity中保存一个点的位置信息rowIdn+columnIdn / 10000.0，fullInfoCloud的点保存点的距离信息；

具体的转换过程可以看下面这个两张图：

代码如下：

 //逐一计算点云深度，并具有深度的点云保存至fullInfoCloud中
    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) {

            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;

            //计算水平方向上点的角度与所在线数
            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            //round是四舍五入
            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;

            //当前点与雷达的深度
            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            if (range < sensorMinimumRange)
                continue;
            //在rangeMat矩阵中保存该点的深度，保存单通道像素值
            rangeMat.at<float>(rowIdn, columnIdn) = range;

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

7.各种标记的含义

• groundMat：
  1） groundMat.at<int8_t>(i,j) = 0，初始值；
  2） groundMat.at<int8_t>(i,j) = 1，有效的地面点；
  3） groundMat.at<int8_t>(i,j) = -1，无效地面点；
• rangeMat
  1） rangeMat.at(i,j) = FLT_MAX，浮点数的最大值，初始化信息；
  2） rangeMat.at(rowIdn, columnIdn) = range，保存图像深度；
• labelMat
  1） labelMat.at(i,j) = 0，初始值；
  2） labelMat.at(i,j) = -1，无效点；
  3）labelMat.at(thisIndX, thisIndY) = labelCount，平面点；
  4）labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999，需要舍弃的点，数量不到30。

8.groundRemoval

void groundRemoval()是利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云。

void groundRemoval()由三个部分的程序组成。

1. 由上下两线之间点的XYZ位置得到两线之间的俯仰角，如果俯仰角在10度以内，则判定(i,j)和(i,j+1)为地面点,groundMat[i][j]=1，否则，则不是地面点，进行后续操作；
2. 找到所有点中的地面点，并将他们的labelMat标记为-1，rangeMat[i][j]==FLT_MAX 判定为无效的另一个条件。
3. 如果有节点订阅groundCloud，那么就需要把地面点发布出来。具体实现过程：把点放到groundCloud队列中去。这样就把地面点和非地面点标记并且区分开来了。

代码如下：

 //利用不同的扫描圈来表示地面，进而检测地面是否水平。例如在源码中的七个扫描圈，每两个圈之间
    // 进行一次比较，角度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加入到groundCloud点云
    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) {

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

9. cloudSegmentation

void cloudSegmentation()作为本程序的关键部分，首先调用了labelComponents函数，该函数对特征的检测进行了详细的描述，并且是针对于某一特定的点与其邻点的计算过程。

void cloudSegmentation()进行的是点云分割的操作，将不同类型的点云放到不同的点云块中去，例如outlierCloud，segmentedCloudPure等。具体步骤：

1. 首先判断点云标签，这个点云没有进行过分类（在原先的处理中没有被分到地面点中去或没有分到平面中），则通过labelComponents(i, j);对点云进行分类。进行分类的过程在labelComponents函数中进行介绍。
2. 分类完成后，找到可用的特征点或者地面点(不选择labelMat[i][j]=0的点)，按照它的标签值进行判断，将部分界外点放进outlierCloud中。continue继续处理下一个点。
3. 然后将大部分地面点去掉，剩下的那些点进行信息的拷贝与保存操作。
4. 最后如果有节点订阅SegmentedCloudPure，那么把点云数据保存到segmentedCloudPure中去。

代码如下：

//可以看到这是对点云分为地面点与可被匹配的四周被扫描的点，
//将其筛选后分别纳入被分割点云
    void cloudSegmentation()
    {
        //这是在排除地面点与异常点之后，逐一检测邻点特征并生成局部特征
        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);
 
        int sizeOfSegCloud = 0;
        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)
                {
                    //离群点或异常点的处理
                    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;
                        }
                    }
                    if (groundMat.at<int8_t>(i,j) == 1)
                    {
                        //地面点云每隔5个点纳入被分割点云
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    //segMsg是自定义rosmsg
                    //是否是地面点
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    //当前水平方向上的行数
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    //深度
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                    //把当前点纳入分割点云中
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    ++sizeOfSegCloud;
                }
            }
 
            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }
        //如果在当前有节点订阅便将分割点云的几何信息也发布出去
        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);
                    }
                }
            }
        }
    }

10.labelComponents

void labelComponents(int row, int col)对点云进行标记。通过标准的BFS算法对点进行标记：以(row，col)为中心向外面扩散，判断(row,col)是否属于平面中一点。

• BFS过程：

1. 用queueIndX，queueIndY保存进行分割的点云行列值，用queueStartInd作为索引。
2. 求这个点的4个邻接点，求其中离原点距离的最大值d1最小值d2。根据下面这部分代码来评价这两点之间是否具有平面特征。注意因为两个点上下或者水平对应的分辨率不一样，所以alpha是用来选择分辨率的。

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

// 通过下面的公式计算这两点之间是否有平面特征
// atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

• 在这之后通过判断角度是否大于60度来决定是否要将这个点加入保存的队列。加入的话则假设这个点是个平面点。
  -然后进行聚类，聚类的规则是：

1. 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增；
2. 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
3. 竖直方向上超过3个也将它标记为有效聚类
4. 标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个

代码如下：

 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;

        //queueSize指的是在特征处理时还未处理好的点的数量，
        // 因此该while循环是在尝试检测该特定点的周围的点的几何特征
        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));

                //该迭代器的first是0则是水平方向上的邻点，否则是竖直方向上的
                // alpha代表角度分辨率，
                // Y方向上角度分辨率是segmentAlphaY(rad)
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                // 通过下面的公式计算这两点之间是否有平面特征
                // atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
                //这个angle其实是该特定点与某邻点的连线与XOZ平面的夹角，这个夹角代表了局部特征的敏感性
                angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha)));

                //如果夹角大于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;
        //当邻点数目达到30后，则该帧雷达点云的几何特征配置成功
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        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;
            }
        }
    }

11.publishCloud

void publishCloud()发布各类点云数据，在publishCloud函数中可以看到，计算的过程中参考系均为机器人自身参考系，frame_id自然base_link。

代码如下：

  void publishCloud() {
        // 1. Publish Seg Cloud Info
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);
        // 2. Publish clouds
        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
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);
        // projected full cloud
        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
        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
        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
        if (pubFullInfoCloud.getNumSubscribers() != 0) {
            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullInfoCloud.publish(laserCloudTemp);
        }
    }
};

12.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);
}

13.主函数

代码如下：

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