A-LOAM源码解析——scanRegistration.cpp

A-LOAM主要包括四个主要的源程序，刚看完scanRegistration，顺便分享总结一下，剩余三个以后看完会及时更新，本文件主要是通过曲率来对角点以及平面点进行提取。

1.首先是对后面用到的一些变量进行定义，主要包括，激光雷达扫描周期，初始化标志，雷达线数，以及6类点等等。

//定义激光雷达的扫描周期，频率为10Hz，周期为0.1s
const double scanPeriod = 0.1;
const int systemDelay = 0;
//计数过了多少帧
int systemInitCount = 0;
//系统初始化标志
bool systemInited = false;
//激光雷达线数 初始值为0
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];

//两个点曲率比较
bool comp (int i,int j) { return (cloudCurvature[i] pubEachScan;

//是否发布每行扫描
bool PUB_EACH_LINE = false;
//定义最小距离参数，用于根据距离远近剔除点
double MINIMUM_RANGE = 0.1; 

 2.定义了一个剔除点的函数，主要删除一些距离比较近的点，并重新定义点云的size。

//定义剔除点函数（距离较近的点）
void removeClosedPointCloud(const pcl::PointCloud &cloud_in,
                              pcl::PointCloud &cloud_out, float thres)
{//统一header（时间戳）和size
    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())//如果有点剔除，size发生改变
    {
        cloud_out.points.resize(j);//重新定义size
    }

    cloud_out.height = 1;//无序数据为1
    cloud_out.width = static_cast(j);//无序数据为总点数
    cloud_out.is_dense = true;//点云数目是否有限
}

3.定义点云的回调函数，也就是核心部分，每来一帧点云，执行一次。先将ros的点云转换成pcl，然后对点进行曲率计算，进行分类划分，最后再将划分出来的点转换回ros形式进行发布。

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    if (!systemInited)//如果系统没初始化
    { 
        systemInitCount++;//计数+1
        if (systemInitCount >= systemDelay)//当大于这个数
        {
            systemInited = true;//系统初始化设为真
        }
        else
            return;
    }

    TicToc t_whole;//整体时间
    TicToc t_prepare;//计算曲率前的预处理时间
   //定义一个容器，记录每次扫描有曲率的点开始和结束索引
    std::vector scanStartInd(N_SCANS, 0);
    std::vector scanEndInd(N_SCANS, 0);
   //定义一个pcl点云类型
    pcl::PointCloud laserCloudIn;
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);//把ros的点云，转换为pcl形式
    std::vector indices;
   //去除过远点（无效点）函数，下面会遇到详细的定义
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
   //去除离激光雷达太近的点 0.1
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

   //定义该次扫描的点数
    int cloudSize = laserCloudIn.points.size();
   //atan2的范围-pi到+pi，负号是因为激光雷达是顺时针旋转，而角度逆时针才为正
    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;
   //保证激光扫描的范围在pi-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> 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;
   //计算垂直俯仰角（后文详细推导）
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;//16线激光雷达垂直视场为-15到15
        int scanID = 0;//激光帧序号

        if (N_SCANS == 16)//如果为16线激光，进入
        {
            scanID = int((angle + 15) / 2 + 0.5);//0.5为了四舍五入 int只能保留整数部分，angle的范围为-pi/2到pi/2，/2是每两个扫描之间差2度,angle范围为-15到15,带入求出scanID范围为0-15
            if (scanID > (N_SCANS - 1) || scanID < 0)//如果在16以上或者为负数，总点数-1结束本次循环
            {
                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();
        }
        //printf("angle %f scanID %d \n", angle, scanID);
        float ori = -atan2(point.y, point.x);//计算这个点的水平旋转角度
        if (!halfPassed)//判断是否扫描一半，进行补偿，保证扫描终止起始扫描范围在pi-2pi
        { //如果没过半，选择起始位置进行差值计算，来补偿，确保-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//如果过半，选择终止位置进行差值计算，来补偿，确保-3*pi/2 < ori - endOri < pi/2
        {
            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;//一个整数+一个小数，整数部分是线号（0-15），小数部分是相对时间（旋转了多少，对应的时间），scanPeriod为0.1，也就是一圈0.1s，小数部分不会超过0.1,完成了按照时间排序的需求
        laserCloudScans[scanID].push_back(point); //把当前点，放入对应的scanid的线上
    }
    
    cloudSize = count;//把总点数给回cloudSize
    printf("points size %d \n", cloudSize);
    //定义一个数据集合，把所有的线保存到里面
    pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud());
    for (int i = 0; i < N_SCANS; i++)//读取每个线
    { 
        scanStartInd[i] = laserCloud->size() + 5;//忽略前五个点
        *laserCloud += laserCloudScans[i];//将scanID为i的scan放入laserCloud
        scanEndInd[i] = laserCloud->size() - 6;//忽略后五个点，开始和结束处的点云scan容易产生不闭合的“接缝”，对提取edge feature不利
    }

    printf("prepare time %f \n", t_prepare.toc());

    //计算曲率，使用每个点相邻的前五个和后五个，最前五个点和最后五个已经忽略
    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;//点没被选择过，设置为0.，后续当被选为特征点之后，将被设置为1
        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 cornerPointsSharp;
    pcl::PointCloud cornerPointsLessSharp;
    pcl::PointCloud surfPointsFlat;
    pcl::PointCloud surfPointsLessFlat;

    float t_q_sort = 0;
    for (int i = 0; i < N_SCANS; i++)//按照scan的顺序，提取四种特征点
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)//如果点数少于6个，则跳过，下一个扫描进来，确保将每个扫描进行6等分
            continue;
        pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
        for (int j = 0; j < 6; j++)//6小段进行特征检测
        {
            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;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//利用sort，按照曲率从小到大进行排序
            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)
                {

                    largestPickedNum++;//曲率大的点计数+1
                    //曲率大的前两个点，为角点，最多为两个
                    if (largestPickedNum <= 2)
                    {                        
                        cloudLabel[ind] = 2;//角点 为2
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    //曲率最大的前20个为降采样角点，包括角点
                    else if (largestPickedNum <= 20)
                    {                        
                        cloudLabel[ind] = 1; //降采样角点 为1
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1; //该点被选择处理过了，标志设为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)//如果距离的平方大于0.05,跳过
                        {
                            break;
                        }
                        //否则，距离的平方小于等于0.05,为比较近的点，标记为1,也就是选择处理过
                        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;
                    }
                }
            }
            //提取曲率比较小的点（平面点）
            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; //-1表示曲率很小的点
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)//选取曲率最小的前四个点
                    { 
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;//该点被选择处理过了，标志设为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;//距离小于等于0.05的被设置为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;
                    }
                }
            }
            //将剩余的点（非角点（2和1）），组成surf_less_flat进行降采样
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)//-1 0 也就是除了角点之外的所有点
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }
        //因为点比较多，进行降采样（体素栅格滤波器），执行结果放入到scanDS
        pcl::PointCloud surfPointsLessFlatScanDS;
        pcl::VoxelGrid downSizeFilter;
        downSizeFilter.setInputCloud(sufPointsLessFlatScan);//设置需要降采样处理的点云
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);//设置滤波时创建的体素体积
        downSizeFilter.filter(surfPointsLessFlatScanDS);//处理结果放入DS
        surfPointsLessFlat += surfPointsLessFlatScanDS;//每次汇总降采样处理之后的点云
    }
    printf("sort q time %f \n", t_q_sort);//输出分类排序的时间
    printf("seperate points time %f \n", t_pts.toc());//输出降采样的时间

    //进行点云格式转换，便于发布
    sensor_msgs::PointCloud2 laserCloudOutMsg;//定义ros点云格式
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);//将pcl的*laserCloud转换成ros的laserCloudOutMsg
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;//时间戳
    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);

    // 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");
}

4.文档最后，为整个程序的主函数，先启动ROS，判断激光雷达线数是否符合标准，最后定义要发布的topic名称，以及缓冲区大小。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");//初始化节点
    ros::NodeHandle nh;//创建句柄，启动ROS
    nh.param("scan_line", N_SCANS, 16);//scan_line为参数名，N_SCANS为保存参数的值，没有参数值的时候默认值设为16
    nh.param("minimum_range", MINIMUM_RANGE, 0.1);//定义最小距离的参数，没有参数值的时候默认值设置为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("/velodyne_points", 100, laserCloudHandler);
    //全部点云
    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();//到这，一直执行回调函数，直到ros主进程关闭，且不往下继续执行
    return 0;
}

5.重点内容详细解析

（1）垂直俯仰角angle的计算：主要应用了几何关系以及弧度与角度之间的转换

（2）16线激光雷达介绍：Velodyne VLP 16每秒可以高达30万个点数据输出，可以实现水平360扫描，垂直视场-15到+15，因此在计算scanID的时候，angle带入的其实是[-15,15]。

（3）曲率的计算：论文中的计算公式为

         第k帧点云中的一个点Xi，附近的点集合为S。根据公式可以看出，曲率等于该点分别与其前后点差的和的范数，与集合中点数与自身范数乘积的比值。而我们的目的是比较曲率的大小，所以代码中简化了，只求一个点与周围前后5个点差平方和作为该点的曲率，来确定各点曲率的大小关系。

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;

（4）PCL体素栅格滤波器的使用（降采样部分）：

         作用：实现降采样，在保持点运形状特征的情况下，可以减少点云数量，提高配准，曲面重建等算法的速度。

        pcl::PointCloud c;//存放滤波之后结果的点云
        pcl::VoxelGrid a;//滤波器
        a.setInputCloud(b);//需要降采样处理的点云
        a.setLeafSize(0.2, 0.2, 0.2);//设置滤波时创建的体素体积
        a.filter(c);//处理结果放入c

感谢以下博主文章提供的帮助：

https://blog.csdn.net/suyunzzz/article/details/108961925

https://blog.csdn.net/unlimitedai/article/details/105711240

https://blog.csdn.net/qq_41586768/article/details/108123854

 

写在最后：新人小白，如有问题还请见谅;

                  整理不易，觉得有帮助的可以点个赞啥的，感谢！！！