四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）_goldqiu的博客-CSDN博客

先看主函数的注释：主要是SLAM前端的参数读取，订阅话题和发布话题初始化。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");  //节点名称
    ros::NodeHandle nh; //注册ROS句柄
    nh.param("scan_line", N_SCANS, 16); //从launch文件参数服务器中获取多少线的激光雷达，如果没有则默认16线

    nh.param("minimum_range", MINIMUM_RANGE, 0.1);
    // 从launch文件参数服务器中获取激光雷达的最小扫描距离MINIMUM_RANGE，小于MINIMUM_RANGE的点将被滤除，单位为M，如果没有则默认0.1。
    printf("scan line number %d \n", N_SCANS);
    // 只有线束是16、32、64的才可以继续
    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("/velodyne_points", 100, laserCloudHandler); //100为消息队列的长度 ，第三个参数为回调函数的入口

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

    pubCornerPointsSharp = nh.advertise("/laser_cloud_sharp", 100);

    pubCornerPointsLessSharp = nh.advertise("/laser_cloud_less_sharp", 100);

    pubSurfPointsFlat = nh.advertise("/laser_cloud_flat", 100);

    pubSurfPointsLessFlat = nh.advertise("/laser_cloud_less_flat", 100);

    pubRemovePoints = nh.advertise("/laser_remove_points", 100);

    if(PUB_EACH_LINE)  //这个条件没有进去
    {
        for(int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise("/laser_scanid_" + std::to_string(i), 100); //分线进行发送点云话题
            pubEachScan.push_back(tmp);
        }
    }
    ros::spin();// 循环执行回调函数

    return 0;
}

接下来看订阅雷达消息的回调函数，前端雷达处理和特征提取主要都在这一块：

// 订阅lidar消息
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    //下面是做初始化，但是这里延时为0，相当于没有延时，只是留下了初始化的接口。
    if (!systemInited)
    { 
        systemInitCount++;
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }

    //作者自己设计的计时类，以构造函数为起始时间，以toc()函数为终止时间，并返回时间间隔(ms)
    TicToc t_whole; //计算整个回调函数的时间
    TicToc t_prepare; //计算雷达点云有序化的时间

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

    pcl::PointCloud laserCloudIn;
    // 把点云从ros格式转到pcl的格式
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
    std::vector indices;
    // 去除掉点云中的nan点，即调用pcl库的函数移除无效点
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    // 去除距离小于阈值的点，这里调用作者写的函数
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

下面要计算点云角度范围，是为了使点云有序，需要做到两件事：为每个点找到它所对应的扫描线（SCAN）；为每条扫描线上的点分配时间戳。要计算每个点的时间戳，首先我们需要确定这个点的角度范围。可以使用中的atan2( )函数计算点云点的水平角度。

这块在后面的激光框架基本都不需要了，因为后面雷达的驱动将每个点的线号、角度、时间戳都发出来了。所以说这一块可以跳过不看。

  // 计算起始点和结束点的角度，由于激光雷达是顺时针旋转，这里取反就相当于转成了逆时针
    int cloudSize = laserCloudIn.points.size();
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    /*
     * atan2()函数是atan(y， x)函数的增强版，不仅可以求取arctran(y/x)还能够确定象限
     * startOri和endOri分别为起始点和终止点的方位角
     * atan2范围是[-Pi,PI]，这里加上2PI是为了保证起始到结束相差2PI符合实际
     */
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;
    // 但总有一些例外，比如这里大于3PI，和小于PI，就需要调整到合理范围。
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }

    //为点云点找到对应的扫描线，每条扫描线都有它固定的俯仰角，我们可以根据点云点的垂直角度为其寻找对应的扫描线。
    bool halfPassed = false;
    int count = cloudSize;
    PointType point;
    std::vector> laserCloudScans(N_SCANS);

    // 遍历每一个点
    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;

        //通过计算垂直视场角确定激光点在哪个扫描线上（N_SCANS线激光雷达）
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
        int scanID = 0;
        if (N_SCANS == 16)
        {
            // 如果是16线激光雷达，结算出的angle应该在-15~15之间，+-15°的垂直视场，垂直角度分辨率2°，则-15°时的scanID = 0。
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                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();
        }    

        // 计算水平角
        float ori = -atan2(point.y, point.x);
        // 根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿，如果此时扫描没有过半，则halfPassed为false
        if (!halfPassed)
        { 
            // 确保-PI / 2 < ori - startOri < 3 / 2 * PI， 如果ori-startOri小于-0.5pi或大于1.5pi，则调整ori的角度
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }
                 
        //扫描点过半则设定halfPassed为true，如果超过180度，就说明过了一半了
            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else
        {
            // 确保-PI * 3 / 2 < ori - endOri < PI / 2
            ori += 2 * M_PI;    // 先补偿2PI
            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);
        // 整数部分是scan线束的索引，小数部分是相对起始时刻的时间
        point.intensity = scanID + scanPeriod * relTime;
        // 根据每条线的idx送入各自数组，表示这一条扫描线上的点
        laserCloudScans[scanID].push_back(point); 
    }
    // cloudSize是有效的点云的数目
    cloudSize = count;
    printf("points size %d \n", cloudSize);

前面处理了雷达点云数据，下面是前端的雷达特征提取，主要提取了线特征和面特征。LOAM提出了一种简单而高效的特征点提取方式，根据点云点的曲率来提取特征点。即把特别尖锐的边线点与特别平坦的平面点作为特征点。

曲率是求取做法是同一条扫描线上取目标点左右两侧各5个点，分别与目标点的坐标作差，得到的结果就是目标点的曲率。当目标点处在棱或角的位置时，自然与周围点的差值较大，得到的曲率较大，这时属于线特征；反之当目标点在平面上时，周围点与目标点的坐标相近，得到的曲率自然较小，这时属于面特征。

下面是曲率计算的程序：

    pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
    // 全部集合到一个点云里面去，但是使用两个数组标记起始和结果，这里分别+5和-6是为了计算曲率方便
    for (int i = 0; i < N_SCANS; i++)
    { 
        // 前5个点和后5个点都无法计算曲率，因为他们不满足左右两侧各有5个点
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6;
    }
    // 将一帧无序点云转换成有序点云消耗的时间，这里指的是前面处理雷达数据的时间
    printf("prepare time %f \n", t_prepare.toc());

    // 计算每一个点的曲率，这里的laserCloud是有序的点云，故可以直接这样计算
    // 但是在每条scan的交界处计算得到的曲率是不准确的，这可通过scanStartInd[i]、scanEndInd[i]来选取
    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;

        //存储每个点的曲率的索引
        /* 
         * cloudSortInd[i] = i相当于所有点的初始自然序列，每个点得到它自己的序号（索引）
         * 对于每个点，选择了它附近的特征点数量初始化为0，
         * 每个点的点类型初始设置为0（次极小平面点）
         */
        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;
        cloudLabel[i] = 0;
    }

曲率计算完成后进行特征分类，提取特征点有几点原则：

1.为了提高效率，每条扫描线分成6个扇区，在每个扇区内，寻找曲率最大的20个点，作为次极大边线点，其中最大的2个点，同时作为极大边线点；

2. 寻找曲率最小的4个点，作为极小平面点，剩下未被标记的点，全部作为次极小平面点。

3. 为了防止特征点过多聚堆，每提取一个特征点（极大/次极大边线点，极小平面点），都要将这个点和它附近的点全都标记为“已选中”，在下次提取特征点时，将会跳过这些点。对于次极小平面点，采取降采样的方法避免过多聚堆。

    TicToc t_pts; //计算特征提取的时间

    // 特征点集合用点云类保存
    pcl::PointCloud cornerPointsSharp; // 极大边线点
    pcl::PointCloud cornerPointsLessSharp; // 次极大边线点
    pcl::PointCloud surfPointsFlat; // 极小平面点
    pcl::PointCloud surfPointsLessFlat; // 次极小平面
    float t_q_sort = 0;   // 用来记录排序花费的总时间
    // 遍历每个scan，对每条线进行操作（曲率排序，选取对应特征点）
   for (int i = 0; i < N_SCANS; i++)
    {
        // 如果最后一个可算曲率的点与第一个的数量差小于6，说明无法分成6个扇区，跳过
        if( scanEndInd[i] - scanStartInd[i] < 6)
            continue;
        pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);    // 用来存储次极小平面点，后面会进行降采样
        //为了使特征点均匀分布，将一个scan分成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;

            TicToc t_tmp; //计算排序时间
            // 对点云按照曲率进行升序排序，小的在前，大的在后 
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
            // t_q_sort累计每个扇区曲率排序时间总和
            t_q_sort += t_tmp.toc();
            // 选取极大边线点（2个）和次极大边线点（20个）
            int largestPickedNum = 0;
            // 挑选曲率比较大的部分，从最大曲率往最小曲率遍历，寻找边线点，并要求大于0.1
            for (int k = ep; k >= sp; k--)
            {
                // 排序后顺序就乱了，这个时候索引的作用就体现出来了
                int ind = cloudSortInd[k]; 

                // 看看这个点是否是有效点，同时曲率是否大于阈值，即没被选过 && 曲率 > 0.1
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)
                {
                    largestPickedNum++;
                    // 每段选2个曲率大的点
                    if (largestPickedNum <= 2)
                    {                        
                        // label为2是曲率大的标记，即设置标签为极大边线点                     
                        cloudLabel[ind] = 2;
                        // cornerPointsSharp用来存放大曲率的点，既放入极大边线点，也放入次极大边线点
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    // 以及20个曲率稍微大一些的点
                    else if (largestPickedNum <= 20)
                    {                        
                        // label置1表示曲率稍微大一些，超过2个选择点以后，设置为次极大边线点，放入次极大边线点容器                       
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    // 超过20个就跳过
                    else
                    {
                        break;
                    }
                    // 这个点被选中后pick标志位置1 ，记录这个点已经被选择了
                    cloudNeighborPicked[ind] = 1; 

                    // 右侧
                    // 为了保证特征点不过度集中，将选中的点周围5个点都置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)           // 相邻scan点距离的平方 <= 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;
                    }
                }
            }

       // 下面开始挑选面点，选取极小平面点（4个）
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSortInd[k];
                // 确保这个点没有被pick且曲率小于阈值
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {
                    // -1认为是平坦的点
                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);
                    smallestPickedNum++;
                    // 这里不区分平坦和比较平坦，因为剩下的点label默认是0,就是比较平坦
                    if (smallestPickedNum >= 4)
                    { 
                        break;
                    }

                    // 下面同理：同样距离 < 0.05的点全设为已经选择过
                    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;
                    }
                }
            }
            // 选取次极小平面点，除了极小平面点，剩下的都是次极小平面点
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }
      pcl::PointCloud surfPointsLessFlatScanDS;
        pcl::VoxelGrid 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()); //打印点云分类时间
  //发布有序点云，极大/次极大边线点，极小/次极小平面点，按需发布每条扫描线上的点云

    sensor_msgs::PointCloud2 laserCloudOutMsg;  // 创建publish msg实例
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);   // 有序点云转化为msg
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp; // 时间戳保持不变
    laserCloudOutMsg.header.frame_id = "/camera_init";  // frame_id名字，坐标系
    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);

    // pub each scam
    // 可以按照每个scan发出去，不过这里是false
    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");
    printf("scan registration time %f ms *************\n", t_whole.toc());   //特征提取的时间，不超过100ms
    if(t_whole.toc() > 100)
        ROS_WARN("scan registration process over 100ms");
}

removeClosedPointCloud函数的作用是对距离小于阈值的点云进行滤除。

void removeClosedPointCloud(const pcl::PointCloud &cloud_in,
                              pcl::PointCloud &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;
    // 把点云距离小于给定阈值的去除掉
    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++;
    }

    // 重新调整输出容器大小
    if (j != cloud_in.points.size())
    {
        cloud_out.points.resize(j);
    }
    // 这里是对每条扫描线上的点云进行直通滤波，因此设置点云的高度为1，宽度为数量，稠密点云
    cloud_out.height = 1;
    cloud_out.width = static_cast(j);
    cloud_out.is_dense = true;
}

下一节讲解laserOdometry.cpp。