[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇

最新推荐文章于 2024-03-12 15:23:15 发布
最新推荐文章于 2024-03-12 15:23:15 发布
阅读量1.3k 收藏 12
点赞数
分类专栏： 学习笔记 文章标签： SLAM 机器人 算法 机器学习 计算机视觉
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Kamfai_Row/article/details/107291626
版权

[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇

[笔记]三维激光SLAM学习一（4）——LOAM代码laserMapping篇

一些细节

本节点使用实现了一个较为完整的SLAM过程,也就是同时建图和定位.主要工作：

  1. 通过将多帧的激光特征点云基于上一节点提供的pose拼接,形成特征点云地图。（包含corner和surface两种特征点云地图）
  2. 将新入的帧与地图作配准,得到更精准的pose.我们又将基于这个pose执行1过程进行建图。

由于单帧与地图配准计算量较大,本节点没有将所有帧与地图进行配准,而是间隔几帧,比如每5帧配准一次（那么频率就是2hz）。
如果你的运算性能完全够, 你可以一帧一匹配。如果这样,下一个节点transformMaintenance这个节点就没有存在的意义了。

点云配准问题：
上一个节点 laserOdometry 和本节点 laserMapping 中都涉及到点云配准问题。
点云的配准问题一般有一个source和一个target,配准的目的是旋转和平移source点云使得source点云与target点云尽量重叠。（不懂的可以看前面讲的ICP配准方法）

需要注意的是：

  • 在上一节点 laserOdometry 中配准的source是新一帧的点云,target是前一帧的点云。
  • 在本节点 laserMapping 中配准的source是新一帧的点云,target是前面的点云拼接成的地图。

当然,我们这里说的点云指的是特征点云, 分为corner和surface两种,corner对应corner map, surface对应surface map。

优化流程:
通过odom预测得到初始位姿,source中的corner在target的corner中寻找对应直线, source中的surface在target的surface中寻找对应平面,
通过点-线和点-面关联共同构建约束, 以点线距离和点面距离作为loss, 基于非线性最小二乘的方法进行优化.求解得出最优pose。

关键点：如果能将本节点生成的 特征地图（corner对应corner map, surface对应surface map） 存下来, 那么就可以将这些地图作为离线地图。我们可以在在线场景中，把这个地图输入给laserMapping节点,实现定位的功能（需要初始值）。
缺陷点：这个地图功能还不够完善，比如没有回环检测，这时候可以参考LeGO-LOAM或者SC-LeGO-LOAM，以后再做分析。

然后明确两个问题：

  • 为什么要有这个建图节点？
  • 建图节点又到底起了什么作用？

为什么要有建图节点？

由原理篇得，论文中将SLAM问题分为两步：

  1. 高频的运动估计（10Hz）
  2. 低频（1Hz）的环境建图

然而，上一部分Lidar里程计的结果不准确，由它拼起来的点云完全不成样子，而且Lidar里程计会不断发散，因此误差也会越来越大。
在第一部分提取特征的时候只关注了表面曲率，这不足以表达三维世界，这种方式并不能完美的得到两帧点云准确的配准点。
而且点云中的点是离散的，我们也无法保证上一帧的点在下一帧中仍会被扫到。
因此，论文提出第二步的建图，依靠其去优化Lidar里程计的位姿估计精度。其实道理也很简单，我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。因此我们对已经构建地图的信任程度远高于最近帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。

建图节点起到了什么作用？

它的作用就是利用地图来优化Lidar里程计的位姿估计结果。
在得到第一帧点云时你的lidar就扫到了数万个点，此时Lidar的位置我们把它作为（0,0,0），在不考虑测量噪声的情况下这数万个点都是相对精确的，我们把这数万个点所构成的环境作为此时的地图，此刻的Lidar坐标系定义为map坐标系。而后Lidar运动了一小段，我们通过Lidar里程计的方法估算了它的相对运动，于是就可以将此时的Lidar位姿及此时的点按照我们估计的相对运动情况，转换到上一时刻（map坐标系）的坐标系下。只不过由于里程计估计误差，地图可能拼歪了。既然这样，我是如果把地图完整的拼上去，是不是就可以优化此时Lidar的位姿了呢？这就是建图节点起到的关键作用。只不过拿当前扫描的点云和地图中所有点云去配准，这个计算消耗太大，因此为了保证实时性，作者在这里采用了一种低频处理方法，即调用建图节点的频率仅为调用里程计节点频率的十分之一。

节点概述

有上述的两个思路，我们看看论文怎么做的，这一部分论文的解析也是非常简短。
在 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk​,tk+1​]内，Lidar得到的点云为 P ‾ k {\overline P}_{k} Pk​，Lidar里程计估计的相对运动为 T k L ( t k + 1 ) {T}_{k}^{\boldsymbol L}(t_{k+1}) TkL​(tk+1​)。定义在 t k t_k tk​时刻的地图为 Q k − 1 Q_{k-1} Qk−1​，且该时刻Lidar在地图中的的位姿为 T k − 1 W ( t k ) {T}_{k-1}^{\boldsymbol W}(t_{k}) Tk−1W​(tk​)。于是，我们可以根据 T k L ( t k + 1 ) {T}_{k}^{\boldsymbol L}(t_{k+1}) TkL​(tk+1​)将 T k − 1 W ( t k ) {T}_{k-1}^{\boldsymbol W}(t_{k}) Tk−1W​(tk​)扩展为 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​)并将 P ‾ k {\overline P}_{k } Pk​转换到世界坐标系 W {W} W下得到 Q ‾ k {\overline Q}_{k} Q​k​。然后通过 不断调整 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​)得到最优的 T k W ( t k + 1 ) {T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​)从而实现 Q k − 1 {Q}_{k-1} Qk−1​与 Q ‾ k {\overline Q}_{k} Q​k​的配准。

代码的整体框架：在订阅器订阅到了laserOdometry发布的消息后即可开始进行处理。继续从main函数开始

main函数

订阅器、发布器等初始化套路就不贴了

  int frameCount = stackFrameNum - 1;   //0
  int mapFrameCount = mapFrameNum - 1;  //4
  ros::Rate rate(100);
  bool status = ros::ok();
  while (status) {
    ros::spinOnce();

    if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&
        fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
        fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {
      newLaserCloudCornerLast = false;
      newLaserCloudSurfLast = false;
      newLaserCloudFullRes = false;
      newLaserOdometry = false;

      frameCount++;
      /***** 坐标转换 *****/
      if (frameCount >= stackFrameNum) {
      }

      /***** 优化处理 *****/
      if (frameCount >= stackFrameNum) {
      }

    status = ros::ok();
    rate.sleep();
  }

坐标转换

这部分十分清晰，是点云数据处理前的准备工作。首先计算 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​)，而后根据 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​)将测量得到的点坐标转换到世界坐标系 W {W} W下。两个坐标转换函数都采用欧拉角表示姿态旋转。

     //控制跳帧数，>=这里实际并没有跳帧，只取>或者增大stackFrameNum才能实现相应的跳帧处理
      if (frameCount >= stackFrameNum) {
        //获取世界坐标系转换矩阵
        transformAssociateToMap();

        //将最新接收到的平面点和边沿点进行旋转平移转换到世界坐标系下(这里和后面的逆转换应无必要)
        int laserCloudCornerLastNum = laserCloudCornerLast->points.size();
        for (int i = 0; i < laserCloudCornerLastNum; i++) {
          pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
          laserCloudCornerStack2->push_back(pointSel);
        }

        int laserCloudSurfLastNum = laserCloudSurfLast->points.size();
        for (int i = 0; i < laserCloudSurfLastNum; i++) {
          pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
          laserCloudSurfStack2->push_back(pointSel);
        }
      }

优化处理

这部分比较难理解。
原文是这样说的：

To find correspondences for the feature points, we store the point cloud on the map, Q k − 1 Q_{k-1} Qk−1​, in 10m cubic areas. The points in the cubes that intersect with Q ‾ k {\overline Q}_k Q​k​are extracted and stored in a 3D KD-tree in {W}. We find the points in Q k − 1 Q_{k-1} Qk−1​ within a certain region (10cm × 10cm × 10cm) around the feature points.

翻译过来就是：
将地图 Q k − 1 Q_{k-1} Qk−1​保存在一个10m的立方体中，若cube中的点与当前帧中的点云 Q ‾ k {\overline Q}_k Q​k​有重叠部分就把他们提取出来保存在KD树中。我们找地图 Q k − 1 Q_{k-1} Qk−1​中的点时，要在特征点附近宽为10cm的立方体邻域内搜索（实际代码中是10cm×10cm×5cm）。

原文中用两句话解决的问题，现实中需要用一大堆代码去填坑。实际处理中，我们首先将地图点云 Q k − 1 Q_{k-1} Qk−1​保存在一个大cube中，并将其分割为一些子cube。

int laserCloudCenWidth = 10; // 邻域宽度, cm为单位
int laserCloudCenHeight = 5; // 邻域高度
int laserCloudCenDepth = 10; // 邻域深度
const int laserCloudWidth = 21; // 子cube沿宽方向的分割个数
const int laserCloudHeight = 11; // 高方向个数
const int laserCloudDepth = 21; // 深度方向个数
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; // 子cube总数 4851

而后我们就要找当前估计的Lidar位姿 T ‾ k W ( t k + 1 ) {\overline T}_{k}^{\boldsymbol W}(t_{k+1}) TkW​(tk+1​) 属于哪个子cube。I、J、K对应了cube的索引。可以看出，当坐标属于 [ − 25 , 25 ] [-25,25] [−25,25]时，cube对应与 ( 10 , 5 , 10 ) (10,5,10) (10,5,10)即正中心的那个cube。

        PointType pointOnYAxis;// 当前Lidar坐标系{L}y轴上的一点(0,10,0)
        pointOnYAxis.x = 0.0;
        pointOnYAxis.y = 10.0;
        pointOnYAxis.z = 0.0;
        //获取y方向上10米高位置的点在世界坐标系下的坐标
        pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);

        //立方体中点在世界坐标系下的（原点）位置
        //过半取一（以50米进行四舍五入的效果），由于数组下标只能为正数，而地图可能建立在原点前后，因此
        //每一维偏移一个laserCloudCenWidth（该值会动态调整，以使得数组利用最大化，初始值为该维数组长度1/2）的量
        int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
        int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
        int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;

        //由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一
        if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
        if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
        if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;

如果取到的子cube在整个大cube的边缘则将点对应的cube的索引向中心方向挪动一个单位，这样做主要是截取边沿cube。

       //调整之后取值范围:3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18
        //如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位
        while (centerCubeI < 3) {
          for (int j = 0; j < laserCloudHeight; j++) {
            for (int k = 0; k < laserCloudDepth; k++) {//实现一次循环移位效果
              int i = laserCloudWidth - 1;
              //指针赋值，保存最后一个指针位置
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];//that's [i + 21 * j + 231 * k]
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              //循环移位，I维度上依次后移
              for (; i >= 1; i--) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              }
              //将开始点赋值为最后一个点
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeI++;
          laserCloudCenWidth++;
        }

        //如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
        while (centerCubeI >= laserCloudWidth - 3) {//18
          for (int j = 0; j < laserCloudHeight; j++) {
            for (int k = 0; k < laserCloudDepth; k++) {
              int i = 0;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              //I维度上依次前移
              for (; i < laserCloudWidth - 1; i++) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeI--;
          laserCloudCenWidth--;
        }

处理完毕边沿点，接下来就是在取到的子cube的 5 ∗ 5 ∗ 5 5*5*5 5∗5∗5的邻域内找对应的配准点了。

        int laserCloudValidNum = 0;
        int laserCloudSurroundNum = 0;
        //在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（前后250米范围内，总共500米范围），三个维度总共125个cube
        //在这125个cube里面进一步筛选在视域范围内的cube
        for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {
          for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {
            for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {
              if (i >= 0 && i < laserCloudWidth && 
                  j >= 0 && j < laserCloudHeight && 
                  k >= 0 && k < laserCloudDepth) {//如果索引合法

                // NOTE: 由于ijk均为整数，坐标取值为中心点坐标
                //计算子cube对应的点坐标,换算成实际比例，在世界坐标系下的坐标
                float centerX = 50.0 * (i - laserCloudCenWidth);
                float centerY = 50.0 * (j - laserCloudCenHeight);
                float centerZ = 50.0 * (k - laserCloudCenDepth);

                bool isInLaserFOV = false;//判断是否在lidar视线范围的标志（Field of View）
                for (int ii = -1; ii <= 1; ii += 2) {
                  for (int jj = -1; jj <= 1; jj += 2) {
                    for (int kk = -1; kk <= 1; kk += 2) {
                      //上下左右八个顶点坐标
                      float cornerX = centerX + 25.0 * ii;
                      float cornerY = centerY + 25.0 * jj;
                      float cornerZ = centerZ + 25.0 * kk;

                      //原点到顶点距离的平方和
                      float squaredSide1 = (transformTobeMapped[3] - cornerX) 
                                         * (transformTobeMapped[3] - cornerX) 
                                         + (transformTobeMapped[4] - cornerY) 
                                         * (transformTobeMapped[4] - cornerY)
                                         + (transformTobeMapped[5] - cornerZ) 
                                         * (transformTobeMapped[5] - cornerZ);

                      //pointOnYAxis到顶点距离的平方和
                      float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) 
                                         + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
                                         + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);

                      float check1 = 100.0 + squaredSide1 - squaredSide2
                                   - 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                      float check2 = 100.0 + squaredSide1 - squaredSide2
                                   + 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                      if (check1 < 0 && check2 > 0) {//if |100 + squaredSide1 - squaredSide2| < 10.0 * sqrt(3.0) * sqrt(squaredSide1)
                        isInLaserFOV = true;
                      }
                    }
                  }
                }

                //记住视域范围内的cube索引，匹配用
                if (isInLaserFOV) {
                  laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 
                                                       + laserCloudWidth * laserCloudHeight * k;
                  laserCloudValidNum++;
                }
                //记住附近所有cube的索引，显示用
                laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 
                                                             + laserCloudWidth * laserCloudHeight * k;
                laserCloudSurroundNum++;
              }
            }
          }
        }

这里还需要判断一下该点是否属于当前Lidar的可视范围内，可以根据余弦公式对距离范围进行推导。根据代码中的式子，只要点在 x x x轴 ± 60 ° ±60° ±60°的范围内都认为是FOV中的点(作者这么做是因为Lidar里程计的估计结果太不准确了，只能概略的取一个较大的范围)。于是我们就得到了在当前Lidar位置的邻域内有效的地图特征点。

所以，我们就不需要对庞大的所有地图点云进行处理了，只需要处理这些邻域cube内的地图特征点即可，可以节省大量的运算资源。为了保证当前帧的点云足够平滑，还对点云进行了滤波处理。

        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();
        //构建特征点地图，查找匹配使用
        for (int i = 0; i < laserCloudValidNum; i++) {
          *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
          *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
        }
        int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
        int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();

        /***********************************************************************
          此处将特征点转移回local坐标系，是为了voxel grid filter的下采样操作不越
          界？好像不是！后面还会转移回世界坐标系，这里是前面的逆转换，和前面一样
          应无必要，可直接对laserCloudCornerLast和laserCloudSurfLast进行下采样
        ***********************************************************************/
        int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();
        for (int i = 0; i < laserCloudCornerStackNum2; i++) {
          pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);
        }

        int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();
        for (int i = 0; i < laserCloudSurfStackNum2; i++) {
          pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);
        }

        laserCloudCornerStack->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);//设置滤波对象
        downSizeFilterCorner.filter(*laserCloudCornerStack);//执行滤波处理
        int laserCloudCornerStackNum = laserCloudCornerStack->points.size();//获取滤波后体素点尺寸

        laserCloudSurfStack->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);
        downSizeFilterSurf.filter(*laserCloudSurfStack);
        int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

        laserCloudCornerStack2->clear();
        laserCloudSurfStack2->clear();

做完这些工作以后，我们就有了在当前Lidar所在位置附近的所有地图特征点以及当前帧的点云特征点，后面的工作就是怎么把这两坨点匹配在一起啦！于是我们再次拿出KD树，来寻找最邻近的5个点。
对点云协方差矩阵进行主成分分析：

  • 若这五个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向；
  • 若这五个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。

因此我们可以很轻易的根据特征向量找到直线上两点从而利用论文中点到直线的距离公式构建优化问题。平面特征也是相同的思路。
完成了优化问题的构建之后就可以对它进行求解了，求解方法还是L-M迭代。这部分代码与laserOdometry部分的几乎一致，代码略。

截止到这里，我们就完成了当前帧点云与地图点云的配准，并对Lidar里程计的运动估计结果进行完了优化。更新完成后，我们还需要将当前帧扫描得到的特征点云封装在不同的cube中，并在地图数组中保存。

        //将surf points按距离（比例尺缩小）归入相应的立方体
        for (int i = 0; i < laserCloudSurfStackNum; i++) {
          pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);

          int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
          int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
          int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

          if (pointSel.x + 25.0 < 0) cubeI--;
          if (pointSel.y + 25.0 < 0) cubeJ--;
          if (pointSel.z + 25.0 < 0) cubeK--;

          if (cubeI >= 0 && cubeI < laserCloudWidth && 
              cubeJ >= 0 && cubeJ < laserCloudHeight && 
              cubeK >= 0 && cubeK < laserCloudDepth) {
            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
            laserCloudSurfArray[cubeInd]->push_back(pointSel);
          }
        }

        //特征点下采样
        for (int i = 0; i < laserCloudValidNum; i++) {
          int ind = laserCloudValidInd[i];

          laserCloudCornerArray2[ind]->clear();
          downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
          downSizeFilterCorner.filter(*laserCloudCornerArray2[ind]);//滤波输出到Array2

          laserCloudSurfArray2[ind]->clear();
          downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
          downSizeFilterSurf.filter(*laserCloudSurfArray2[ind]);

          //Array与Array2交换，即滤波后自我更新
          pcl::PointCloud<PointType>::Ptr laserCloudTemp = laserCloudCornerArray[ind];
          laserCloudCornerArray[ind] = laserCloudCornerArray2[ind];
          laserCloudCornerArray2[ind] = laserCloudTemp;

          laserCloudTemp = laserCloudSurfArray[ind];
          laserCloudSurfArray[ind] = laserCloudSurfArray2[ind];
          laserCloudSurfArray2[ind] = laserCloudTemp;
        }

最后就是将各种信息发布出去了。这里需要说明的是，为了保证运行效率环境点云每5帧发布一次。

        //特征点汇总下采样，每隔五帧publish一次，从第一次开始
        if (mapFrameCount >= mapFrameNum) {
          mapFrameCount = 0;

          laserCloudSurround2->clear();
          for (int i = 0; i < laserCloudSurroundNum; i++) {
            int ind = laserCloudSurroundInd[i];
            *laserCloudSurround2 += *laserCloudCornerArray[ind];
            *laserCloudSurround2 += *laserCloudSurfArray[ind];
          }

          laserCloudSurround->clear();
          downSizeFilterCorner.setInputCloud(laserCloudSurround2);
          downSizeFilterCorner.filter(*laserCloudSurround);

          sensor_msgs::PointCloud2 laserCloudSurround3;
          pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
          laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
          laserCloudSurround3.header.frame_id = "/camera_init";
          pubLaserCloudSurround.publish(laserCloudSurround3);
        }

Ps.这部分代码坑很大，还是得细品。

Kamfai·Row
关注
  • 0
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
LOAM源码解析3——laserMapping
try_again_later的博客
04-20 1774
SLAM系统前面大多是先估计相邻两帧之间相对位姿估计，接下来开始建图，然后根据建图结果对现有姿态进行优化，最后把当前这特征点填充进地图，且估计当前帧相对于世界坐标系下的位姿。 在扫描周期末尾激光雷达里程计输出畸变矫正过的点云Pk+1^\widehat{P_{k+1}}Pk+1​​，同时还有整个周期内每个点的姿态Tk+1LT_{k+1}^{L}Tk+1L​。雷达建图过程是为了把Pk+1^\wideh...
通俗易懂！视觉slam第四部分——slam刚体三维空间运动
01-20
本文将探讨SLAM中的一个重要概念——刚体在三维空间中的运动。三维空间是由三个轴构成，每个点的位置可以用三个坐标来唯一确定。当我们讨论刚体时，不仅需要知道其在空间中的位置，还需要了解其姿态，即朝向。 刚体...
loam源码解析6 ： laserMapping（一）
SLAM小学僧
06-19 1240
laserMapping.cpp解析一 loam源码地址：https://github.com/cuitaixiang/LOAM_NOTED. 论文学习：LOAM: Lidar Odometry and Mapping in Real-time 论文阅读. loam源码解析汇总： loam源码解析1 ： scanRegistration（一）. loam源码解析2 ： scanRegistration（二）. loam源码解析3 ： laserOdometry（一）. loam源码解析4 ： laserOd
LOAM学习-代码解析（五）地图构建 laserMapping
weixin_44041199的博客
12-11 1642
LOAM学习-代码解析（五）地图构建 laserMapping前言一、初始化二、转换矩阵 transformAssociateToMap三、转换矩阵 transformUpdate四、点云注册 pointAssociateToMap五、点云转移六、接收数据 Handler结语 前言 前两篇文章LOAM学习-代码解析（三）特征点运动估计 laserOdometry和代码解析（四）特征点运动估计 laserOdometry，对激光里程计进行了代码解析，本文将继续对LOAM源码的laserMapping进行解析
LOAM laserMapping
zc的博客
01-16 130
LOAM laserMapping
LOAM 系列(4): laserMappingnode 分析
weixin_40072161的博客
12-12 818
一、对应源文件 add_executable(alaserMapping src/laserMapping.cpp) target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 二、ROS interface 1. subscribe topic // sharp pts ros::Subscriber subLaserCloudCornerLast = nh.subsc
激光slam A-LOAM
09-29
作为学习激光SLAM的入门框架，A-LOAM的代码可读性和框架思路都值得激光SLAM初学者仔细研读。这个框架使⽤Eigen以及Ceres-Solver对原始LOAM进⾏重构，在保证算法原理不变的前提下，对代码框架进⾏优化，使得代码变得...
ROS与SLAM从零开始学习笔记（一）——ROS概述与环境搭建
01-20
写在前面：笔者最近在做一个基于ROS平台的小项目，也算是从零开始学ROS，在这里写下一些学习时的笔记。 笔者学习参考：古月居 一、ROS环境安装 ROS历经多个版本的升级，目前正不断适用于机器人领域的研究与控制，ROS...
学习SLAM-三维空间刚体的旋转.zip
06-06
本压缩包“学习SLAM-三维空间刚体的旋转.zip”聚焦于SLAM中的一个重要组成部分——三维空间刚体的旋转理解。 首先，我们来深入理解三维空间中的刚体旋转。刚体是物理学中的一种理想化模型，指的是在受力作用下，其...
SLAM-三维图形SLAM的交互式地图校正-优质项目实战.zip
02-20
本项目实战主要围绕三维图形SLAM的交互式地图校正展开，旨在帮助学习者深入理解SLAM的基本原理，并通过实践提升在三维空间中的定位和地图构建能力。 SLAM的核心目标是让移动设备在未知环境中自主建立地图并同时确定...
A_loam的laserMapping.cpp约束构建理解
啥也不懂的研究生也淦SLAM的博客
10-12 576
A_loam的laserMapping.cpp约束构建理解
LOAM_velodyne LaserMapping
xiaoma_bk的博客
11-16 2438
注解： 功能：优化Lidar的位姿，在此基础上完成低频的环境建图 解释：经过前两个节点的处理可以完成一个完整激光里程计，可以概略地估计出Lidar的相对运动，可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。通过与地图匹配的方式来优化激光的位姿，利用已构建地图对位姿估计结果进行修正。当前扫描的点云和地图中所有点云去配准，这个...
A-LOAM代码理解laserMapping(三)
qq_20184333的博客
11-07 897
A-LOAM代码理解laserMapping(三)
ALOAM mapping
qq_42700518的博客
04-02 2645
首先将odometryBuf中接受到的里程计数据保存为q_wodom_curr、t_wodom_curr // 获取里程计的数据放置与 q_wodom_curr t_wodom_curr q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x; q_wodom_curr.y() = odometryBuf.front()...
【PLIO学习总结】laserMapping中的时间戳与状态更新逻辑
最新发布
larrydong的博客
03-12 1073
本文仅用于个人学习总结记录。如有错误，请批评指正。
LeGO-LOAM算法详解
热门推荐
晓晨的博客
08-16 1万+
LeGO-LOAM算法详解 整体框架 LeGO-LOAM算法的总体框架如下图所示： 图中新增加了绿框中的Segmentation环节，同时对后续的特征提取、Odometry以及Mapping部分均有一定的修改，主要包括： 增加Segmentation操作，把点云投影为距离图像，分离出地面点与非地面点（分割点，segmented point）。 平滑度计算公式不同，原始LOAM中为使用点集中的坐标相减，而LeGO-LOAM中为使用点集中的欧式距离作差。 特征点选取中原始LOAM为按照平滑度的值及已有特征点
激光slam：LeGO-LOAM---代码编译安装与gazebo测试
月照银海似蛟龙的博客
07-28 2481
LeGO-LOAM的英文全称是。轻量化具有地面优化的激光雷达里程计和建图其框架如下，大体和LOAM是一致的LeGO-LOAM是基于LOAM的改进版本，其主要目的是为了实现小车在多变地形下的定位和建图，针对前端和后端都做了一系列的改进。在原本的LOAM中比如应用场景是AGV在草地中行驶，草地中的点会很有可能提取为角点，但是这种点是不稳定的，相比于楼房、树干等稳定的角点，希望要把地面上的角点去除掉。LeGO-LOAM更轻量化，不影响精度的情况下减轻计算负荷，保障了嵌入式平台的实时性。...
从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析1
HUASHUDEYANJING的博客
10-27 3435
首先LEGO-LOAM是在LOAM基础上基于地面优化，并且加入了简易的闭环检测，而LOAM是实时三维激光SLAM的开山鼻祖，使得实时三维重建成为可能，同时基于特征的激光SLAM基本上都是LOAM的衍生算法，思想大同小异。LEGO-LOAM在LOAM的基础上效果有巨大提升，同时代码可读性较高，所以将它作为入门SLAM的第一个算法非常合适。LEGO-LOAM系统框架如下所示其核心思想是将同时定位和映射的复杂问题划分为两个算法，该问题旨在同时优化大量变量。高频低保真的执行里程计。低频高保真执行地图优化。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值