loam论文与Aloam代码学习归纳（三）--scanRegistration节点

最新推荐文章于 2024-02-18 20:20:29 发布

CHEGCHE

最新推荐文章于 2024-02-18 20:20:29 发布

阅读量1.3k

点赞数 8

分类专栏： LidarSLAM 文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_41586768/article/details/108123854

版权

LidarSLAM 专栏收录该内容

3 篇文章 3 订阅

订阅专栏

一.作用

数据预处理，特征点提取，用以接下来的Odometry和mapping的点云配准。所以这个节点本身就是一个计算的过程：根据曲率C划分不同类型的特征点（边缘点或是平面点或是非特征点）。
根据VLP16的激光扫描模型, 对单帧点云（paper中称为一个Sweep）进行分线束（分为16束）, 每束称为一个Scan，并记录每个点所属线束和每个点在此帧电云内的相对扫描时间（相对于本帧第一个点）。针对单个Scan提取特征点, 而相对时间会在laserOdometry中用于运动补偿.所有Scan的特征点,拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云).至此,每帧点云,输出两帧对应的特征点云, 给下一个节点laserOdometry。

二.总体分析

1.ALOAM的scanRegistration通过单独一个ROS节点完成，这个节点在scanRegistration.cpp中实现。
2.两个回调函数:laserCloudHandler 点云的回调函数
removeClosedPointCloud 第一个中的回调函数，用以去除多余点
一个主函数 main

三.各部分代码详解

1.主函数main

//订阅前一个节点发布的点云topic,接受到一帧点云就执行一次回调函数laserCloudHandler;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");//初始化节点
    ros::NodeHandle nh;//创建句柄，启动ROS
   //定义参数与线数
    nh.param<int>("scan_line", N_SCANS, 16);
    //定义要去除点的最小距离的参数与距离值0.1
    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;
    }

    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);
    //发布每行scan
    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;
}

根据主函数可以看到scanRegistration节点订阅了velodyne_points话题，通过这个话题来获取原始的激光雷达数据，并且发布了一下5个话题：
（1）velodyne_cloud_2：将原始点云滤波，模型化后重组成的点云 - laserCloud
（2）laser_cloud_sharp：大曲率特征点–角点 cornerPointsSharp
（3）laser_cloud_less_sharp：小曲率特征点–降采样角点也包含大曲率特征点〔〕 cornerPointsLessSharp
（4）laser_cloud_flat：平面点 surfPointsFlat
（5）laser_cloud_less_flat：除了角点之外全部点，后面又进行了滤波处理 surfPointsLessFlat
（6）laser_remove_points：没有发布任何消息

2. laserCloudHandler回调函数

根据的话题机制，每当激光雷达向velodyne_points话题发送一个sweep扫描数据时，便会进入该回调函数中处理，该回调函数中主要执行一下几步：

（1）点云数据的预处理：去除无效点与临近点0.1

接收到velodyne的一个sweep数据后，便会将它由ROS msg转换成PCL数据 PointCloud, 并且通过一个vector points存储任意一点的值，于是就有一个疑问，一个完整sweep点云的点在points中的存放是有序的，（从上到下，从左到右）从前往后按照激光雷达的扫描顺序，即先从初始旋转角度开始，依次获取垂直方向上所有的点，然后按旋转顺序，获取下一个旋转角度下每个垂直方向的点，直到旋转一周完成。
然后开始基本的预处理，主要策略是最初的一帧数据抛弃，等待数据完整，另外是会执行去NaN点与距离滤波。

//回调函数： laserCloudHandler中的去除函数；
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
    if (&cloud_in != &cloud_out)//统一header(时间戳)和size
    {
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0;

    //逐点距离比较
    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)//x*x+y*y+z*z<r*r 球体
            continue;
        cloud_out.points[j] = cloud_in.points[i];
        j++;
    }
    if (j != cloud_in.points.size())//有点被剔除时，size改变
    {
        cloud_out.points.resize(j);
    }

    cloud_out.height = 1;//数据行数，默认1为无组织的数据，指定点云的height为1，那么他的width即为点云的大小
    cloud_out.width = static_cast<uint32_t>(j);//可以理解成点数
    cloud_out.is_dense = true;//指定点云中所有数据都是有限的
}
//接受到一帧点云就执行一次回调函数laserCloudHandler;
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)//G使用类的方法作为回调函数，&引用
{
  /*******************************1 点云数据预处理：去除无效点与临近点0.1*******************************/
  if (!systemInited){ //G：相对于loam，无imu，不用弃用前20帧初始数据，前变量设为0,此为真
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

    TicToc t_whole;
    TicToc t_prepare;
    //G记录每个scan有曲率的点的开始和结束索引
    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包的点云转化为pcl形式
    
     // 首先对点云滤波，去除NaN(不是数）值得无效点云，以及在Lidar坐标系原点MINIMUM_RANGE距离以内的点
    std::vector<int> indices;//G容器类对象定义一个动态数组： indices   保存去除点的索引
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);//去除无效点
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);//去除距离雷达太近的点  0.1

（2）激光雷达数据模型化

根据激光雷达模型，获得每个激光点的scanID以及扫描时间，并将每个点按照帧的scanID划分到laserCloudScans中，根据激光雷达的数据存放顺序，我们也能知道，每一scanID的laserCloudScans点依然是按照旋转顺序排列的。
（velodyne 16雷达每次返回的数据称为一帧（sweep），一帧由16条线组成（每条线称为一个scan），每个scan有很多点。如果将velodyne 16雷达的扫描频率设置为10Hz，那么一秒就返回10帧数据。工作在10Hz的频率下，这个雷达的水平扫描角度的分辨率是0.2°，我们可以算出来理论上一帧有360/0.2×16=28800360/0.2×16=28800个点，但是实际上你可以试试，每次的点数不是完全一样的，有时多一点有时少一点，在程序中将存储点的数组定义为40000个元素，这是选了一个保守的上限。）
1)首先求出一完整sweep数据中激光的起始角度startOri和终止角度endOri。
2)求出每一个激光点的垂直俯仰角，从而利用这个俯仰角求出该激光点属于哪一个scan，即scanID，然后求出该点的旋转角ori，并利用该旋转角ori以及起始startOri和终点角度endOri和一个扫描周期的时间scanPeriod计算该点的时间，最后设置intensity参数：
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
最后将激光点放置于laserCloudScans[scanID]中，放到vector容器中
这部分完整代码如下：

 /*******************************2 激光雷达的数据模型化*******************************/
   //2.1 将激光扫描的范围定到1pi到3pi
    int cloudSize = laserCloudIn.points.size();
    //lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转,atan2逆时针为正角
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    //由于机械抖动激光雷达扫描不一定180度，也不一定回归0度在计算，所以加上2pi以保证Zaire周期范围内
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;
   // 处理  保证  M_PI < endOri - startOri < 3 * M_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);
     //lidar扫描线是否旋转过半
    bool halfPassed = false;
    int count = cloudSize;
    PointType point;
    
    
    //2.2 按使用雷达的线束的类别（16,32,64线）分别保存点云集合到容器vector中
    
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);//定义雷达线束，动态数组集合
    // (1)遍历当前扫描帧全部点，读取每一个点的坐标；求出每个点的旋转角和scanID  根据旋转角确定时间
    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;
    //（2）计算垂直俯仰角，scan ID的序号即垂直激光帧的序号 (根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度）
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;

        if (N_SCANS == 16)
        {
            scanID = int((angle + 15) / 2 + 0.5);// + 0.5 是用于四舍五入   因为 int 只会保留整数部分  如 int(3.7) = 3  
	                                         //  /2是每两个scan之间的间隔为2度，+15是过滤垂直上为[-,15,15]范围内
            if (scanID > (N_SCANS - 1) || scanID < 0)//scanID是0-15号
            {//该点不符合要求，舍弃
                count--;
                continue;
            }
        }
        
         // 下面两种为32线与64线
        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);
        //（3）计算该激光雷达点的旋转角
        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;
            }
        }
        else
        {
            ori += 2 * M_PI;
	    //确保-3*pi/2 < ori - endOri < pi/2
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }
        //（4）保存
        //-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）
        float relTime = (ori - startOri) / (endOri - startOri);
	//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
        point.intensity = scanID + scanPeriod * relTime; // scanPeriod每一帧的时间 
        laserCloudScans[scanID].push_back(point);  // 将该点放入  scanID 帧中
    }
    
    cloudSize = count;
    printf("points size %d \n", cloudSize);

注意：
1）有关激光雷达的线号问题;
在这里插入图片描述

2）loam这部分有有关雷达坐标系与车体坐标系之间的转化
坐标轴转化：从蓝色的传感器坐标系到红色的车体坐标系。velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系，车体坐标系为z轴向前,x轴向左,y轴向上的右手坐标系

    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;

在这里插入图片描述
在LOAM源码中对于仰角的定义为： $\theta =arctan(z/\sqrt{x^{2}+ y^{2}})$

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

在这里插入图片描述

（3）特征点提取

具体步骤如下
1）排除前后5个点
先将按scanID划分的点云laserCloudScans整合到一个点云集laserCloud中，并依据每一scan中前后5个点不提取特征点的原则，记录每一scan中有效特征点在 laserCloud中的序号索引-scanStartInd、scanEndInd，这个序号是很重要的，因为，在后面计算曲率时，只排除了laserCloud的前后5个点，剩下的scan的前后5个点也都计算了曲率，但是由于记录了有效特征点的序号，这些无效点在后续计算特征时就可以排除。
2）运用公式计算曲率
3）根据曲率选择4种特征点
首先特征点就是一些曲率比较特殊的点：曲率比较大的点即边缘点，角点，曲率比较小的点即平点， loam中选择了4类特征点：
cornerPointsSharp ：曲率很大的特征点，角点 ——每scan的6等份中曲率最大的前2个
cornerPointsLessSharp：曲率稍微小的特征点，降采样角点 —— 每scan的6等份中曲率前20 且曲率要大于0.1的点注意也包括 cornerPointsSharp的点
surfPointsFlat：平面点，挑选每scan的6等份中曲率最小的4个特征点且曲率小于0.1
surfPointsLessFlat：包含所有非曲线特征的点。
选择特征时，以一个scan为单位，每个scan对其有效曲率点的部分均匀分成6个子部分，对每一个子部分进行筛选。
排序部分 std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);comp是比较函数 bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }这里要注意，首先sort()函数前两个参数分别是排序起始位置指针以及指向最后一个参与排序的元素后一位置的指针，即排序的范围是[cloudSortInd + sp,cloudSortInd + ep + 1)，也就是数组[cloudSortInd + sp,cloudSortInd + ep]部分参与排序。
另外，这里cloudSortInd + sp是一种数组名+i的表示方法，数组名可以等效成指向数组首元素的指针，那么* (cloudSortInd + sp) = cloudSortInd[sp]。通过这个排序函数，效果可以等效成cloudCurvature[*(cloudSortInd + i)] < cloudCurvature[*(cloudSortInd + j)] , i<j
对这里6等分计算做一个解释，首先由于int精度的问题这里可能并不是严格6等分的！！另外，关于ep为什么要减一，这是避免前后两份存在重叠的部分，这样每一份提出的特征点都不同，可以想到，若不减一，下一份的sp与上一份的ep处于相同位置，这样可能会重复提取同一个特征点。
4）将PCL的点云类型转换成ROS中的点云类型，进行发布的准备工作
5）发布每一帧点云

代码过程如下：

  
    /*******************************3 特征点提取*******************************/
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    //3.1 排除前后5个点
    for (int i = 0; i < N_SCANS; i++)
    {    
        scanStartInd[i] = laserCloud->size() + 5;// 记录每个scan的开始index，忽略前5个点
        *laserCloud += laserCloudScans[i]; // 将scanID为i的scan放入laserCloud 
        scanEndInd[i] = laserCloud->size() - 6;// 记录每个scan的结束index，忽略后5个点，开始和结束处的点云scan容易产生不闭合的“接缝”，对提取edge feature不利
    }

    printf("prepare time %f \n", t_prepare.toc());
     //3.2 运用公式计算曲率
    for (int i = 5; i < cloudSize - 5; i++)
    { 
       //使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过
        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;// 记录曲率点索引   
        cloudNeighborPicked[i] = 0;// 点有没有被选选择为feature点
        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，因为点太多，最后会降采样
    }


    TicToc t_pts;

    pcl::PointCloud<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> surfPointsLessFlat;
    //3.3 根据曲率选择4种特征点
         //3.3.1 统计每一个scan中点数，并进行分段，按照曲率从小到大排序
    float t_q_sort = 0;
    for (int i = 0; i < N_SCANS; i++)// 按照scan的顺序提取4种特征点，遍历每一个scan
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)// 如果该scan的点数少于6个点就跳过，因为需要6等分
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)// 将该scan分成6小段执行特征检测
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan的起始index
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan的结束index，-1避免重复

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 根据曲率有小到大对subscan的点进行sort冒泡排序
            t_q_sort += t_tmp.toc();
	    
	    
         //3.3.2 按照c的大小，提取边缘点和降采样边缘点,并对已经选择过的特征点进行临近点去除
            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)// 从后往前，即从曲率大的点开始提取corner feature
            {
                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;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)// 该subscan中曲率最大的前20个点认为是corner_less_sharp特征点
                    {                        
                        cloudLabel[ind] = 1; 
                        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;
                    }
                }
            }

            //3.3.3 按照c的大小，提取平面点,并对已经选择过的特征点进行临近点去除
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSortInd[k];

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {

                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

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

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

            
            // 3.3.4 其他的非corner特征点与surf_flat特征点一起组成surf_less_flat特征点
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

           // 3.3.5 最后对该scan点云中提取的所有surf_less_flat特征点进行降采样，因为点太多了（体素栅格滤波，下采样）
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;//创建滤波器对象
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        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());

   //3.4 将PCL的点云类型转换成ROS中的点云类型，进行发布的准备工作
    sensor_msgs::PointCloud2 laserCloudOutMsg;   //定义ROS中的点云类型
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);//将PCL中的点云数据转换成ROS数据；
    laserCloudOutMsg.header.frame_id = "/camera_init";//rviz中参考坐标系
    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);
    
    // 3.5 pub each scam
    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");
}

四.程序流程图

程序流程图不太规范，还希望各位大佬予以指正

在这里插入图片描述

五.参考文献

1.*****https://blog.csdn.net/qq_42700518/article/details/104287167(感谢大佬，过程是按照这个来写的）
2.*https://www.cnblogs.com/wellp/p/8877990.html
3.*https://blog.csdn.net/unlimitedai/article/details/105711240? utm_medium=distribute.pc_aggpage_search_result.none-task-blog- 2_allbaidu_landing_v2~default-1-105711240.nonecase#commentBox
4.https://zhuanlan.zhihu.com/p/29719106
5.*https://zhuanlan.zhihu.com/p/57351961
6.*https://zhuanlan.zhihu.com/p/145857388
7.*https://blog.csdn.net/robinvista/article/details/104379087/