3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读

3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读

  • A-LOAM代码的结构
  • Code

A-LOAM代码的结构

A-LOAM的cpp有四个,其中 kittiHelper.cpp 的作用是将kitti数据集转为rosbag
剩下的三个是作为 slam 的 部分,分别是:

  • laserMappin.cpp ++++ 当前帧到地图的优化
  • laserOdometry.cpp ++++ 帧间里程计
  • scanRegistration.cpp ++++ 前端lidar点预处理及特征提取

本片主要解读 前端lidar点特征提取部分的代码

之前要做点的预处理,这部分的代码在之前分析过了
链接

Code

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //计算曲率
    for (int i = 5; i < cloudSize - 5; i++)
    {
      
        //计算x维度的曲率
        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;//此标志位置1时代表这个点被选位特征点了
        cloudLabel[i] = 0;//特征点的标签
    }

计算每个点的曲率
并将每个点的曲率保存在一个向量中
将每个点的索引保存在一个向量中
初始化的点标签向量

这里的计算曲率就是对应的论文中的这个公式
在这里插入图片描述
在代码里就是分别求一个轴维度的曲率,以x轴为例,就是
前五个点x的值+后五个点x的值-10*当前点x的值
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //声明 特征点的点云
    pcl::PointCloud<PointType> cornerPointsSharp;   //角点特征 点云
    pcl::PointCloud<PointType> cornerPointsLessSharp;  //弱角点特征 点云
    pcl::PointCloud<PointType> surfPointsFlat;//面点特征 点云
    pcl::PointCloud<PointType> surfPointsLessFlat;//弱面点特征 点云

声明 特征点的点云
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
注意:下面代码是一个大的嵌套循环
按每个scan一个循环
每个scan等分6份,每份一个循环

    //声明 特征点的点云
    pcl::PointCloud<PointType> cornerPointsSharp;   //角点特征 点云
    pcl::PointCloud<PointType> cornerPointsLessSharp;  //弱角点特征 点云
    pcl::PointCloud<PointType> surfPointsFlat;//面点特征 点云
    pcl::PointCloud<PointType> surfPointsLessFlat;//弱面点特征 点云 经过体素滤波处理

声明 特征点的点云

    //遍历每个scan
    for (int i = 0; i < N_SCANS; i++)
    {
             
    // 没有有效的点了,就continue
        if( scanEndInd[i] - scanStartInd[i] < 6)
            continue;

        //用来存储不太平整的点  不经过体素滤波  
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);

遍历每个scan
当这个scan上面的 起始点和结束点的id差小于6 ,也就是 有效点过少,则不处理这个scan直接continue
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
注意这个循环是嵌套在上面的那个里面的

        // 将每个scan等分成6等分   分别计算里面的特征点
        for (int j = 0; j < 6; j++)
        {
     
            // 计算等分 的 每个起始点 和 结束点 的 id
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;  //起始点的id
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//结束点的id

            TicToc t_tmp;//算时间用的

            //对点云按照曲率进行排序,小的在前,大的在后
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);

            t_q_sort += t_tmp.toc();//计算时间

            int largestPickedNum = 0;//挑选最大的曲率特征点的个数  即角点的个数

这个地方涉及到数组的处理了,要好好看下

A-LOAM算法为了让特征点分部均匀,将每一条scan分成6份,在每一份中分部提取曲率最大的和最小的几个点作为角点和面点

在这里要把前面的数据给总结下了,要不基本是乱的

laserCloud 是依次放的由 scan1 到 scan16的点云
图
cloudCurvature 存放的上面的每个点对应的 曲率值
图
cloudSortInd 存放的点对应的id ,一开始 每个元素是由 5~n,后面会根据曲率大小重新排
图
scanStartInd[i] 是第i个scan的起始点的id scanEndInd[i] 是第i个scan的结束点的id
3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读_第1张图片

sp和ep分别是一条scan上面等分6份,中一份的 起始点id和结束点id
所以计算sp的时候是这样的:
sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6
计算ep的时候是这样的:
ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

然后下面的代码 sort那部分,是利用std的排序方法,将cloudSortInd 对应的那部分 将点云的id进行了重新排序,曲率小的在前面,曲率大的在后面
3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读_第2张图片

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

 		int largestPickedNum = 0;//挑选最大的曲率特征点的个数  即角点的个数
            //挑选曲率比较大的部分
            for (int k = ep; k >= sp; k--)
            {
     
                //排序后顺序就乱了,  用之前存的 索引值 取到 点的id 
                int ind = cloudSortInd[k]; 

重新排序后,cloudSortInd 上的ep位置就是存放的曲率最大点的id,取出来这个id

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                //1 看这个点是否是有效点,同时曲率是否大于阈值
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)
                {
     
                    //2 满足要求,则选择的 角点个数 加1
                    largestPickedNum++;

                    // 每段选择2个曲率最大的点
                    if (largestPickedNum <= 2)
                    {
     
                        //3 label 为2 是曲率大的标记                        
                        cloudLabel[ind] = 2;
                        //4 cornerPointsSharp存放曲率大的点
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        //cornerPointsLessSharp 存放曲率稍微大的点
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    //5 以及20个曲率稍微大些的点
                    else if (largestPickedNum <= 20)
                    {
       
                        // 6 label 为1 是曲率稍微大的标记                       
                        cloudLabel[ind] = 1; 
                        //7
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    //超过20个的就不要了
                    else
                    {
     
                        break;
                    }

                    //8 这个点被选中后 pick标志位置 1
                    cloudNeighborPicked[ind] = 1; 
                    
                    //9 为了保证特征点不过度集中,将选中的点周围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)
                        {
     
                            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;
                    }
                }
            }

上面的代码对照注释依次做了如下事情,注释里都标了对应的序号:

  1. cloudNeighborPicked[ind] == 0 代表这个点可以挑选,然后曲率大于0.1 则满足要求
  2. 满足要求则选择的点个数+1
  3. 将最大的两个曲率点标签置为2
  4. 然后将这两个点存入cornerPointsSharp
  5. 然后再选较大的20个点,
  6. 标签置位1
  7. 然后存入cornerPointsLessSharp
  8. 被选为 角点和弱角点后,点的选取标志位置为1,则这个点就不能在后面被选取了,因为一开始有那个判断
  9. 为了保证特征点不过度集中,将选中的点周围5个点都置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; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;

                    //选择4个面点
                    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;
                    }
                }
            }

选择面点的时候和角点基本一致,不再细写.
有区别的地方就是:
只选4个面点,没有像角点一样选几个弱面点,这部分在下面

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

            //除了角点和弱角点的都是弱面点
            for (int k = sp; k <= ep; k++)
            {
     
                if (cloudLabel[k] <= 0)
                {
     
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }

弱面点的处理就在接着的下面
通过标签判断,前面的标签已将打上了 角点为-2 ,弱角点为-1 ,面点为1 .剩下的均为弱面点

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        //将弱面点 进行体素滤波  因为剩下的都是弱面点了,点就很多了
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

        surfPointsLessFlat += surfPointsLessFlatScanDS;

将弱面点 进行体素滤波 因为剩下的都是弱面点了,点就很多了

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //发布所有的点云
    sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    laserCloudOutMsg.header.frame_id = "/camera_init";
    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);

将上面整理的特征点转为ros的格式,然后发布topic出去
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

以上为A-LOAM 特征点提取部分的代码解读
之前要做点的预处理,这部分的代码在之前分析过了
链接

你可能感兴趣的:(SLAM,自动驾驶,ALOAM)