天天看点

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]<cloudCurvature[j]); }
ros::Publisher pubLaserCloud;//整体点云
ros::Publisher pubCornerPointsSharp;//大曲率特征点——角点
ros::Publisher pubCornerPointsLessSharp;//小曲率特征点——降采样角点
ros::Publisher pubSurfPointsFlat;//平面点
ros::Publisher pubSurfPointsLessFlat;//降采样平面点
ros::Publisher pubRemovePoints;//剔除点
std::vector<ros::Publisher> pubEachScan;

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

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

//定义剔除点函数(距离较近的点)
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;
        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<uint32_t>(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<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);
   //定义一个pcl点云类型
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);//把ros的点云,转换为pcl形式
    std::vector<int> 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<pcl::PointCloud<PointType>> 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<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    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<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> 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<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        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<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> 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<int>("scan_line", N_SCANS, 16);//scan_line为参数名,N_SCANS为保存参数的值,没有参数值的时候默认值设为16
    nh.param<double>("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<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);
    //每行扫描到的点
    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();//到这,一直执行回调函数,直到ros主进程关闭,且不往下继续执行
    return 0;
}
           

5.重点内容详细解析

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

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

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

(3)曲率的计算:论文中的计算公式为

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

         第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<PointType> c;//存放滤波之后结果的点云
        pcl::VoxelGrid<PointType> 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

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

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

继续阅读