A-LOAM代码注释学习（三）scanRegistration.cpp

---

根据代码，对A-LOAM算法框架进一步理解，进行数学推导理解和代码里函数的使用。scanRegistration.cpp对应scanRegistration节点，包括主函数，近点剔除函数和特征点提取回调函数。对各部件进行了详细的代码注释，并对部分难点进行梳理和理解。

一、代码注释

1.1 近点剔除函数removeClosedPointCloud

1. 作用：剔除距离雷达太近的点
2. 使用过程：在回调函数中调用，但是阈值thres由回调函数MINIMUM_RANGE输入，由launch文件配置。

//template函数模板
template <typename PointT>               //PointT 是PCL中基本的点的表示形式
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
    //统一header(时间戳)和size
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;                 //header同步
        cloud_out.points.resize(cloud_in.points.size());    //容器大小同步
    }

    size_t j = 0;                       //size_t一般用来表示一种计数

    //逐点比较欧式距离，把点云中距离小于给定阈值thres的点去除掉，把cloud_in 放入cloud_out
    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];
        j++;                            //有点被剔除时，size改变
    }
    
    //调整容器的长度大小，使其能容纳j个元素。因为，有的i没有放到cloud_out中，i<j的
    if (j != cloud_in.points.size())
    {
        cloud_out.points.resize(j);
    }
    // 这里是对每条扫描线上的点云进行直通滤波，因此设置点云的高度为1，宽度为数量，稠密点云
    cloud_out.height = 1;
    //可以理解成点数，宽度
    cloud_out.width = static_cast<uint32_t>(j);
    //指定点中的所有数据是有限的（true)
    cloud_out.is_dense = true;
    
}

1.2 回调函数laserCloudHandler

1. 作用：处理雷达点云
2. 使用过程：在主函数的Subscriber中回调使用
3. 功能思路：对接收到的点云进行排序，求曲率，根据曲率分类，体速滤波降采样，发送topic

参考链接：
回调函数理解: A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释

{
    // 作用就是延时，为了确保有IMU数据后, 再进行点云数据的处理
    if (!systemInited)
    { 
        systemInitCount++;
        //初始化的次数大于等待时间 进行初始化
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

    
    TicToc t_whole;             //计时：整个回调函数的时间
    TicToc t_prepare;           //计时：雷达点云有序化的时间

    //每条雷达扫描线上（scan）的可以计算曲率的点云点的起始索引和结束索引，分别用scanStartInd数组和scanEndInd数组记录
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);

    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;        //定义一个pcl形式的输入点云
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);      //把ros包的laserCloudMsg点云转化laserCloudIn为pcl形式
    std::vector<int> indices;

    //以下，使用 pcl::removeNaNFromPointCloud 和 removeClosedPointCloud 的 cloud_in 和 cloud_out都是 laserCloudIn
    //也就是，输入laserCloudIn，处理之后，再将输出定为laserCloudIn，
    //所有，没有RemovePoints， 相关publisher也没用
    // pcl库函数移除无效点，pcl库函数
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    
    // 移除过近点，自定义子函数
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

    //下面要计算点云角度范围，是为了使点云有序，需要做到两件事：
    //1、为每个点找到它所对应的扫描线（SCAN）
    //2、为每条扫描线上的点分配时间戳。
    //要计算每个点的时间戳，首先我们需要确定这个点的角度范围。可以使用<cmath>中的atan2( )函数计算点云点的水平角度。
    int cloudSize = laserCloudIn.points.size();         //计算点云点的数量

    //每次扫描是一条线，看作者的数据集激光x向前，y向左，那么下面就是线一端到另一端
    //atan2的输出为-pi到pi(PS:atan输出为-pi/2到pi/2),
    //atan2()函数是atan(y， x)函数的增强版，不仅可以求取arctran(y/x)还能够确定象限
    //计算旋转角时取负号是因为velodyne是顺时针旋转，速腾顺时针旋转，Y朝前
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;  //这里加上2PI是为了将atan2的范围转换到[0, 2π)
    
    //激光间距 1pi到3pi
    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;
    //按线数保存的点云集合
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
    
    //循环对每个点进行以下操作
    //1、为每个点找到它所对应的扫描线（SCAN）
    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        //求仰角atan输出为-pi/2到pi/2，实际看scanID应该每条线之间差距是2度
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;         //线数ID

        //根据不同线数使用不同参数对每个点对应的第几根激光线进行判断
        if (N_SCANS == 16)      //如果是16线激光雷达，结算出的angle应该在-15~15之间，+-15°的垂直视场，垂直角度分辨率2°，则-15°时的scanID = 0。
        {
            scanID = int((angle + 15) / 2 + 0.5);           //int舍尾取整，+0.5是误差限，最接近的线数，但不是四舍五入
            //如果判定线在16以上或是负数则忽视该点回到上面for循环
            if (scanID > (N_SCANS - 1) || scanID < 0)       //过滤点云数据，不在角度范围内的点是噪声
            {
                count--;
                continue;
            }
        }
        //32线雷达， 垂直视场角+10.67~-30.67°，垂直角度分辨率1.33°，-30.67°时的scanID = 0
        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);
        
        //根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿
        if (!halfPassed)        //如果没过半
        { 
            //确保-pi/2 < ori - startOri < 3*pi/2   为什么？
            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，如果超过180度，就说明过了一半了
            {
                halfPassed = true;
            }
        }
        //确保-3*pi/2 < ori - endOri < pi/2
        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;
            }
        }     
        float relTime = (ori - startOri) / (endOri - startOri);     //计算旋转角占总角度的比值 即点云中的相对时间
        point.intensity = scanID + scanPeriod * relTime;            //scanID为线号， 小数点后面为相对时间，放入point信息
        laserCloudScans[scanID].push_back(point);                   //按线把点存好
    }
    
    cloudSize = count;                                  // cloudSize是有效的点云的数目
    printf("points size %d \n", cloudSize);

    //曲率计算
    //把所有线保存在laserCloud一个数据集合里，
    //把每条线的第五个和倒数第五个位置反馈给scanStartInd和scanEndInd
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    { 
        //处理每个scan  每条scan上面的 点的起始 id  前5个点不要（为了后面计算曲率） 结束的id 为后6个点不要（其中一个不参与计算）
        //开始和结束处的点云容易产生步闭合的"接缝"，对提取edge_feature不利
        //整理后重新放入lasercloud
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;    
    }
     //预处理部分完成，记录一下时间
    printf("prepare time %f \n", t_prepare.toc());

    // 曲率公式
    //由于计算中我们只是为了比较曲率大小，所以只计算了前后五个点与当前点的差，然后求其平方代替曲率计算
    for (int i = 5; i < cloudSize - 5; i++)
    { 
        //使用每个点的前、后五个点共11个点计算曲率
        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;                //记录曲率索引，一轮循环后变为cloudSize - 6
        cloudNeighborPicked[i] = 0;         //标记是否筛选过
        cloudLabel[i] = 0;                  //标记点类型
    }

    
    TicToc t_pts;                                           //计时
    //定义四种特征点
    pcl::PointCloud<PointType> cornerPointsSharp;           //曲率很大的特征点  对应2
    pcl::PointCloud<PointType> cornerPointsLessSharp;       //曲率稍微大的特征点 对应1
    pcl::PointCloud<PointType> surfPointsFlat;              //曲率很小的特征点   对应-1
    pcl::PointCloud<PointType> surfPointsLessFlat;          //曲率稍微小的特征点 对应0

    float t_q_sort = 0;                                     // 用来记录排序花费的总时间
    
    //遍历所有scan
    for (int i = 0; i < N_SCANS; i++)
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)            //如果该scan的点数小于6个点，就不能分为6个扇区，跳过
            continue;
        
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);   //定义surfPointsLessFlatScan储存该扫描线上的less_flat点云
        //对每个扫描进行6等分
        for (int j = 0; j < 6; j++)
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;           //起始序号
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; //结束序号    -1是避免提取到相同的点

            TicToc t_tmp;       //计时，排序sort时间

            // 对cloudSortInd进行排序   依据的是序号cloudSortInd[i]的点的曲率从小到大
            //sort（数组的起始位置，数组的结束位置， 比较方法） 
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
            t_q_sort += t_tmp.toc();

            int largestPickedNum = 0;                   //记录曲率大点的数目

            //挑选各个分段曲率很大和比较大的点
            for (int k = ep; k >= sp; k--)
            {
                int ind = cloudSortInd[k];              //曲率最大点的序号

                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)    //3～20这18个点
                    {                        
                        cloudLabel[ind] = 1;            // Label 1：corner_less_sharp
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);    //只属于cornerPointsLessSharp
                    }
                    else
                    {
                        break;
                    }

                    //筛选过的点标志为 1 不进行下次的计算
                    cloudNeighborPicked[ind] = 1; 

                    // 考虑该特征点序号前后5个点，如果有两两之间距离过于接近的点
                    // 则不考虑将其前一个作为下一个特征点的候选点  这是防止特征点聚集，使得特征点在每个方向上尽量分布均匀
                    for (int l = 1; l <= 5; l++)
                    {
                          
                        // 提取该点 后 五个点的xyz方向的导数
                        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;       //距离平方小于0.05的，标记为计算过
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        //提取该点  前  五个点的xyz方向的导数
                        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;
                        }

                        //计算过的点标志 1
                        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; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)
                    { 
                        break;
                    }

                    //避免每个特征点过于集中，将选中的点的周围5个点都置1，避免后续会选到
                    cloudNeighborPicked[ind] = 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;
                    }
                    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特征点
            //最初将所有点都标记为0了，初始化的时候，见  cloudLabel[i] = 0;
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

        //由于less flat点最多，对每个分段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);            //执行滤波
        

        //less flat点汇总  surfPointsLessFlatScanDS是滤波中间集合，滤波后又赋值回surfPointsLessFlat，label = 0
        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }

    printf("sort q time %f \n", t_q_sort);                          //打印排序时间
    printf("seperate points time %f \n", t_pts.toc());              //打印点云分类时间

    //发布点云信息  laserCloud 
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    laserCloudOutMsg.header.frame_id = "camera_init";
    pubLaserCloud.publish(laserCloudOutMsg);

    //发布曲率大的点云信息 即cornerPointsSharp  label = 2
    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    //发布曲率一般大的点云信息 即cornerPointsLessSharp label = 1
    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    //发布曲率小的点云信息 即surfPointsFlat  label = -1
    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    //发布曲率较小的点云信息 即surfPointsLessFlat  label = 0, 降采样后的常规点云
    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

    //laserCloudIn，试验pubRemovePoints
    pubRemovePoints.publish(surfPointsLessFlat2);
    
    // pub each scam
    //发布每一条线的信息
    if(PUB_EACH_LINE)             //由一开始进行设置，默认不发送，false
    {
        for(int i = 0; i< N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            pcl::toROSMsg(laserCloudScans[i], scanMsg);             //laserCloudScans按线数保存的集合，无序，但是划分线数
            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");                       //处理超时
}

1.3 主函数main

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");      //ROS节点初始化
    
    ros::NodeHandle nh;                             //创建句柄，启动ROS

    nh.param<int>("scan_line", N_SCANS, 16);        //设置激光雷达的扫描线数量，由launch决定

    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);      //点与激光雷达的最近距离，由launch决定

    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);
    
    //发布点云信息  以下六类点
    //创建一个publisher，发布名为/velodyne_points_2的topic，消息类型为sensor_msgs::PointCloud2，队列长度为100
    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);

    //检测每条发布的激光数据，并注明id存好
    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;
}

二、重要内容解析

2.1 计算水平角atan2函数

函数atan2(y,x)中参数的顺序是倒置的，atan2(y,x)计算的值相当于点(x,y)的角度值。

float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;  

这里加上2PI是为了将atan2的范围转换到[0, 2π)

2.2 曲率计算

曲率是指一定范围内拟合出的圆的半径的倒数所以曲率大的为角点，曲率小的为面点。
diffX=x1+x2+x3+x4+x5−10∗x6+x7+x8+x9+x10+x11
曲率计算参考: SLAM算法 -LOAM框架分析(一)

2.3 tic_toc函数

用到两次TicToc函数计时。
t_q_sort 是float，t_pts是double（TicToc里定义为double），为什么要不一样呢？
个人理解：
点云分类所需要的时间比排序长，需要更高的精度。

//记录排序时间
float t_q_sort = 0;          // 初始化为 0
TicToc t_tmp;                //定义TicToc函数 t_tmp
t_q_sort += t_tmp.toc();     //将获取到的时间长度赋值给 t_q_sort 
printf("sort q time %f \n", t_q_sort); 

//记录点云分类时间
TicToc t_pts;                //定义TicToc函数 t_pts
printf("seperate points time %f \n", t_pts.toc());

2.4 topic的发布格式

这里发布topic的publisher不一样。
一般是这样：

//都在主函数里
ros:Publisher turtle_vel_pub n.advertise<geometry_msgs:Twist>("/turtle1/cmd_vel",10);
turtle_vel_pub.publish(vel_msg);

这里是：

ros::Publisher pubLaserCloud;  //在定义部分创建

sensor_msgs::PointCloud2 laserCloudOutMsg;    //在回调函数装数据
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "camera_init";
pubLaserCloud.publish(laserCloudOutMsg);

pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100); //在主函数创建publisher

2.5 nh.param和nh.getparam区别

kittiHelper.cpp里是getParam()函数获取路径。
scanRegistration.cpp里是param()函数进行赋值。
区别：
param()函数从参数服务器取参数值给变量。如果无法获取，则将默认值赋给变量。

2.6 尚存问题

对水平角ori补偿2pi是为什么？