A-LOAM——scanRegistration.cpp

scanRegistration.cpp

参考了网上多篇文章，给aloam代码加了详尽的注释

removeClosedPointCloud

该函数去除半径thres内的点，以此来保证留下的点不在激光雷达附近，再对保留下的点进行resize。

template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)     //去除不想要的点    
{
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0; //size_t在32位架构上是4字节，在64位架构上是8字节，在不同架构上进行编译时需要注意这个问题。
                           //而int在不同架构下都是4字节，与size_t不同；且int为带符号数，size_t为无符号数。
                           //与int固定四个字节有所不同,size_t的取值range是目标平台下最大可能的数组尺寸,
                           //一些平台下size_t的范围小于int的正数范围,又或者大于unsigned int. 使用Int既有可能浪费，又有可能范围不够大。

    for (size_t i = 0; i < cloud_in.points.size(); ++i) //对输入进来的每个点
    {
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;
        cloud_out.points[j] = cloud_in.points[i];       //遍历输入点云cloud_in.point[]，如果当前点的x^2+y^2+z^2  >= thres^2, 保存到cloud_out.points[]，否则去除
                                                                                                 //即：去除以激光雷达坐标原点半径为thres内的球体里的点
        j++;
    }

    if (j != cloud_in.points.size()) //如果上面确实发现有被剔除的点，则对输出点云cloud_out.points进行resize(j)
    {
        cloud_out.points.resize(j);
    }

    cloud_out.height = 1; //如果cloud 是无序的 height 是 1
    cloud_out.width = static_cast<uint32_t>(j); //点云的长度；static_cast强制将j转换为uint32_t类型，不可能为负数
    cloud_out.is_dense = true;
}

laserCloudHandler

​ 该函数为接收到激光雷达点云消息的回调函数。该函数的参数为激光雷达的点云消息，sensor_msgs::PointCloud2ConstPtr型的引用法输入 &laserCloudMsg。
​ 函数是订阅的点云句柄，在其中进行了格式转换、1.中函数的调用并进行曲率求解，特征分割，并准备消息发布工作。

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
//每接收到一帧点云就执行一次laserCloudHandler
{
    
    // 作用就是延时，为了确保有IMU数据后, 再进行点云数据的处理
    if (!systemInited)   
    { 
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

    TicToc t_whole;
    TicToc t_prepare;
    
    //每条扫描线上的可以计算曲率的点云点的起始索引和结束索引
    std::vector<int> scanStartInd(N_SCANS, 0);  //长度为N_SCANS，值全为0
    std::vector<int> scanEndInd(N_SCANS, 0);

    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); //将ROS系统接收到的laserCloudMsg转为laserCloudIn点云作为A-LOAM的输入点云
    std::vector<int> indices;

    //首先对该点云进行滤波，去除NaN值的无效点，以及在Lidar坐标系原点MINIMUM_RANGE距离以内的点
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);  //调用removeClosedPointCloud(const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, float thres) ，剔除半径10cm内点


    int cloudSize = laserCloudIn.points.size();  //点云数目
    
    //atan2( )默认返回逆时针角度，由于LiDAR通常是顺时针扫描，所以往往使用-atan2( )函数。
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); // double atan2(double y, double x), 返回以弧度表示的 y/x 的反正切
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +  2 * M_PI;  

    //通过上面的公式，endOri - startOri理应在pi~3pi之间
    //正常情况下在这个范围内：pi < endOri - startOri < 3*pi，异常则修正
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    //printf("end Ori %f\n", endOri);

    bool halfPassed = false;
    int count = cloudSize;
    PointType point;  //typedef pcl::PointXYZI PointType;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); //定义一个长度为16的点云向量laserCloudScans
    for (int i = 0; i < cloudSize; i++)
    {//对于每一个点云
        point.x = laserCloudIn.points[i].x; //pcl::PointXYZI point;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; 
        int scanID = 0;

        //针对不同线数的雷达，确定该点的scanID。
        if (N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);   //确定当前点的scanID：scanID[0~16]
            if (scanID > (N_SCANS - 1) || scanID < 0)
            { //如果scanID超出了范围，则舍弃该点
                count--;   //count=CloudSize
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);

        //按照角度分配时间戳，进行运动补偿。
        float ori = -atan2(point.y, point.x); //对每个点都求一次y/x的反正切
        if (!halfPassed) 
        { //扫描没有过半
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }

        //relTime 是一个0~1之间的小数，代表占用扫描时间的比例，乘以扫描时间得到真实扫描时刻。
        //scanPeriod扫描时间默认为0.1s。
        float relTime = (ori - startOri) / (endOri - startOri);  //得到一个relTime用来给每个点分配fireID

        //假设16线Lidar的水平角度分辨率为0.2°（实际是0.1~0.4），则每个scan理论上有360/0.2=1800个点，为方便叙述，称每个scan点的ID为fireID，即fireID[0~1799]，相应的scanID[0~16]
        //通过Lidar坐标系下的仰角和水平夹角计算点云的scanId和fireID
        point.intensity = scanID + scanPeriod * relTime;  //intensity的整数部分存放scanID，小数部分存放归一化后的fireID
        laserCloudScans[scanID].push_back(point);      //将点根据scanID放到对应的子点云laserCloudScans中
    }
    
    cloudSize = count;
    printf("points size %d \n", cloudSize);

    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>()); //定义一个laserCloud，将合并后的laserCloudScans存进来
   
    //计算曲率
    //将子点云laserCloudScans合并成一帧点云laserCloud，注意这里的单帧点云laserCloud可以认为已经是有序点云了，按照scanID和fireID的顺序存放
    for (int i = 0; i < N_SCANS; i++)
    { 
        scanStartInd[i] = laserCloud->size() + 5;   //记录每个scan的开始index，忽略前五个点
        *laserCloud += laserCloudScans[i];   //把scanID[i]加入到laserCloud中
        scanEndInd[i] = laserCloud->size() - 6; //记录每个scan的结束index，忽略后五个点，开始和结束处的点云容易产生步闭合的"接缝"，对提取edge_feature不利
    }

    printf("prepare time %f \n", t_prepare.toc());// 将一帧无序点云转换成有序点云消耗的时间

//计算点的曲率
    for (int i = 5; i < cloudSize - 5; i++)
    { 
        //从第6个点开始，当前点的曲率由该点及其前后五个点的对应坐标之和的平方和表示。与论文不同的是没有进行除法。
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        
        //cloudSortInd[i] = i相当于所有点的初始自然序列，从此以后，每个点就有了它自己的序号（索引）
        cloudSortInd[i] = i;   //一轮循环后变为cloudSize - 6
        cloudNeighborPicked[i] = 0; //标记点i是否已经被选择过
        cloudLabel[i] = 0; //点类型
        //  Label 2：corner_sharp
                                             //  Label 1：corner_less_sharp, 包含Label 2
                                             //  Label -1：surf_flat
                                             //  Label 0：surf_less_flat，包含Label -1，因为点太多，最后会降采样
                                             //先默认每个点都是surf_less_flat
    }


    TicToc t_pts;
    //定义四种特征点云
    pcl::PointCloud<PointType> cornerPointsSharp; //极大边缘点
    pcl::PointCloud<PointType> cornerPointsLessSharp; //次极大边缘点
    pcl::PointCloud<PointType> surfPointsFlat; //极小平面点
    pcl::PointCloud<PointType> surfPointsLessFlat;//次极小平面点，经过降采样防止特征点聚堆

    float t_q_sort = 0;// 用来记录排序花费的总时间
    /*
    1、为了提高效率，每条扫描线分成6个扇区，在每个扇区内，寻找曲率最大的20个点，作为次极大边线点，其中最大的2个点，同时作为极大边线点；寻找曲率最小的4个点，作为极小平面点，剩下未被标记的点，全部作为次极小平面点。
	2、为了防止特征点聚堆，每提取一个特征点（极大/次极大边线点，极小平面点），都要将这个点和它附近的点全都标记为“已选中”，在下次提取特征点时，将会跳过这些点。对于次极小平面点，采取降采样的方法避免聚堆。
    */
    
    //根据曲率计算四种特征点：边缘点特征：sharp、less_sharp；面特征：flat、less_flat
    for (int i = 0; i < N_SCANS; i++) //按照scan的顺序提取4种特征点
    {
        if( scanEndInd[i] - scanStartInd[i] < 6) //如果该scan的点数小于7个点，就不能分为6个扇区，跳过；第i线的扫描末端索引 — 扫描开始索引
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>); //定义surfPointsLessFlatScan储存该扫描线上的less_flat点云
        
        // 为了使特征点均匀分布，将一个scan分成6个subcans
        for (int j = 0; j < 6; j++) //将第i个scan分成6小段subscan执行特征检测
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;  // subscan的起始index，j=0,sp=scanStartInd[i]; j=1, sp=scanStartInd[i]+第i线的scan点数的1/6
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; //subscan的结束index

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp); //根据曲率由小到大对subscan的点进行sort;  对每一线都得到了6个曲率由小到大的点集
                                                                                                                         //bool comp (int i,int j) { return (cloudCurvature[i]
                                                                                                                         //cloudSortInd为数组首元素地址，该函数表示在内存块 cloudSortInd+sp 到 cloudSortInd+ep+1内，通过比较曲率之间的大小改变对应内存块储存的曲率值实现排序
            t_q_sort += t_tmp.toc();// t_q_sort累计每个扇区曲率排序时间总和
            
            // 选取极大边线点（2个）和次极大边线点（20个）
            int largestPickedNum = 0;

            //提取corner_sharp的feature，选择该subscan曲率最大的前2个点为corner_sharp，曲率最大的前20个点认为是corner_less_sharp
            for (int k = ep; k >= sp; k--)      //从曲率大的点开始提取corner feature，此时sp到ep内对应的曲率由小到大
            {
                int ind = cloudSortInd[k]; //初始k=ep，ind为曲率最大的点的索引

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)  //如果该点没有被选择过，并且曲率大于0.1
                {

                    largestPickedNum++;
                    if (largestPickedNum <= 2) //该subscan中曲率最大的前2个点认为是Corner_sharp特征点
                    {                        
                        cloudLabel[ind] = 2;    //Label 2：corner_sharp
                        cornerPointsSharp.push_back(laserCloud->points[ind]);//选取该点为极大边缘点
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]); //也将这两个corner_sharp点认为是次极大边缘点
                    }
                    else if (largestPickedNum <= 20) //该subscan中曲率最大的前20个点认为是corner_less_sharp特征点
                    {                        
                        cloudLabel[ind] = 1;  // Label 1：corner_less_sharp
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]); //只选取该点为次极大边缘点
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;  //标记该点被选择过

                    //与当前点簇距离的平方小于等于0.05的点标记为选择过，避免特征点密集分布
                    for (int l = 1; l <= 5; l++) 
                    {//右侧
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {//左侧
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            //提取surf_flat的feature，选择该subscan曲率最小的前4个点为surf_flat
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)  //从曲率小的点开始提取surf_flat特征，此时sp到ep内对应的曲率由小到大
            {
                int ind = cloudSortInd[k];  //ind对应曲率最小的点的索引

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)  //如果ind对应的点未被选中，且曲率小于0.1
                {

                    cloudLabel[ind] = -1;   //  Label -1：surf_flat
                    surfPointsFlat.push_back(laserCloud->points[ind]);  //将ind对应点加入surf_flat

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)     //选择曲率最小的前4个点为surf_flat
                    { 
                        break;//跳出循环层
                    }

                    cloudNeighborPicked[ind] = 1; //将该点标记为已选
                    //与当前点距离的平方小于等于0.05的点标记为选择过，避免特征点密集分布
                    for (int l = 1; l <= 5; l++)
                    { 
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            //目前选出了曲率最大的前20个点认为是corner_less_sharp(其中前两个为corner_sharp)，选出了曲率最小的4个点认为是surf_flat
            //其他的非corner特征点与surf_flat特征点共同组成surf_less_flat特征点
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0) //  Label -1：surf_flat
                { //  Label 0：surf_less_flat，包含Label -1，因为点太多，最后会降采样 
                  //最初将所有点都标记为0了，见  cloudLabel[i] = 0;
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]); //包含了极小平面点
                }
            }
        }

        //最后对该scan点云中提取的所有surf_less_flat特征点进行降采样
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter; //体素网格滤波
        downSizeFilter.setInputCloud(surfPointsLessFlatScan); //surfPointsLessFlatScan中存着surf_less_flat特征点
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2); //设置体素大小
        downSizeFilter.filter(surfPointsLessFlatScanDS);//执行滤波

        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }
    printf("sort q time %f \n", t_q_sort);
    printf("seperate points time %f \n", t_pts.toc());

    //将四种特征点云转为ROS格式的点云
    //将当前帧点云提取出的4种特征点与滤波后的当前帧点云进行publish
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg); 
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; //时间戳保持不变
    laserCloudOutMsg.header.frame_id = "/camera_init"; //坐标系
    pubLaserCloud.publish(laserCloudOutMsg);

    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "/camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "/camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

    if(PUB_EACH_LINE)
    { //如果要把每一条扫描线都发不出来
        for(int i = 0; i< N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            pcl::toROSMsg(laserCloudScans[i], scanMsg);
            scanMsg.header.stamp = laserCloudMsg->header.stamp;
            scanMsg.header.frame_id = "/camera_init";
            pubEachScan[i].publish(scanMsg);
        }
    }

    printf("scan registration time %f ms *************\n", t_whole.toc());
    if(t_whole.toc() > 100)
        ROS_WARN("scan registration process over 100ms");
}

main

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");
    ros::NodeHandle nh;

    // 设置激光雷达的扫描线数量，预设可以使用16线/32线/64线
    nh.param<int>("scan_line", N_SCANS, 16);
    // 设置点云点与激光雷达的最近距离MINIMUM_RANGE，小于MINIMUM_RANGE的点将被滤除
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);

    printf("scan line number %d \n", N_SCANS);

    if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
    {
        printf("only support velodyne with 16, 32 or 64 scan line!");
        return 0;
    }

    // 订阅初始的激光雷达数据，并执行回调函数laserCloudHandler
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);

    // 发布话题：有序点云（删除过近点、设置索引），极大边线点集合，次极大边线点集合，极小平面点集合，次极小平面点集合
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);

    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);

    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);

    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);

    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);

    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);

    // 如果PUB_EACH_LINE == true ,则发布每条扫描线上的点云点
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
            pubEachScan.push_back(tmp);
        }
    }
    // 循环执行回调函数
    ros::spin();

    return 0;
}