SLAM：loam 3D激光里程计与三维环境地图的重建（三）

这篇文章是LOAM系列的第三篇，对LOAM中的laserMapping节点进行深入解析，这部分对应于文章Lidar Mapping部分的内容。

先来梳理一下：点云数据进来后，经过前两个节点的处理可以完成一个完整但粗糙的视觉里程计，可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响，这个运动估计的结果足够精确，没有任何漂移，那我们可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但这就如同现实中不存在一个不受外力就能匀速直线运动的小球一样，量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。（有兴趣的读者可以试一试，直接拿这两个节点的结果在rviz里显示一下，能不能分辨的出这一大坨仅仅依靠位姿估计拼接起来的点到底是个什么玩意。）再回过头来说说这个节点的代码，估计每一个开始看的人都会很崩溃，因为真的是毫无注释，大体思路清晰但很多小细节十分难懂，然而很多小细节对于实际的数据处理而言很重要（尽管它们在论文中不会被提及）。更绝望的是论文对应部分的描述也十分粗略，甚至连一个公式都没有。我也是一边崩溃着一遍硬着头皮看，为了帮助更多人爬坑，所在这儿也把一点心得分享给大家，各位有什么见解也欢迎与我探讨交流。

概述

为了搞懂这部分内容，首先有两个问题需要明确：

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

先来回答第一个问题：为什么要有建图节点。正如我们刚才所说，Lidar里程计的结果不准确，拼起来的点也完全不成样子，且它会不断发散，因此误差也会越来越大。试想一下我们对特征的提取仅仅只是关注了他们的曲率，这种方式怎么可能完美的得到两帧点云准确的配准点。且点云中的点是离散的，我们也无法保证上一帧的点在下一帧中仍会被扫到。总之，无论我们多努力想让Lidar里程计的估计结果变得精准，残酷且冰冷的显示都会把我们的幻想击碎。因此，我们需要依靠别的方式去优化Lidar里程计的位姿估计精度。在SLAM领域，一般会采用与地图匹配的方式来优化这一结果。其实道理也很简单，我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。因此我们对已经构建地图的信任程度远高于临帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。

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

有了这个整体的思路，我们就可以来看看具体是怎么做的：在[ , ]内，Lidar得到的点云为 ，Lidar里程计估计的相对运动为 。定义在 时刻的地图为 ，且该时刻Lidar在地图中的的位姿为 。于是，我们可以根据 将 扩展为 并将 转换到世界坐标系{W}下得到 。然后通过 不断调整 得到最优的 从而实现 与 的配准。

这块内容的整体思路还是很简单的，但实现起来还是要费一些功夫。前面依旧时ROS的套路代码，就不贴了，看不懂的可以参考之前的两篇文章。代码的整体框架长这个样子：在订阅器订阅到了laserOdometry发布的消息后即可开始进行处理。

 int frameCount = stackFrameNum - 1; // initialize as 0
  int mapFrameCount = mapFrameNum - 1; // initialize as 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();
  }

坐标转换

这部分十分清晰，是点云数据处理前的准备工作。首先计算 ，而后根据 将测量得到的点坐标转换到世界坐标系{W}下。两个坐标转换函数都采用欧拉角表示姿态旋转，有兴趣的可以拿出演草纸自己推导一下。

       // 将相关坐标转移到世界坐标系下->得到可用于建图的Lidar坐标
	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, , in 10m cubic areas. The points in the cubes that intersect with are extracted and stored in a 3D KD-tree in {W}. We find the points in within a certain region (10cm × 10cm × 10cm) around the feature points.

就是说：将地图 保存在一个10m的立方体中，若cube中的点与当前帧中的点云 有重叠部分就把他们提取出来保存在KD树中。我们找地图 中的点时，要在特征点附近宽为10cm的立方体邻域内搜索（实际代码中是10cm×10cm×5cm）。

虽然就这么两句话，但是要用一大堆代码坑去填啊。实际处理中，我们首先将地图点云 保存在一个大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总数

而后我们就要找当前估计的Lidar位姿 属于哪个子cube。I、J、K对应了cube的索引。可以看出，当坐标属于[-25,25]时，cube对应与(10,5,10)即正中心的那个cube。

       PointType pointOnYAxis; // 当前Lidar坐标系{L}y轴上的一点(0,10,0)
        pointOnYAxis.x = 0.0;
        pointOnYAxis.y = 10.0;
        pointOnYAxis.z = 0.0;
        pointAssociateToMap(&pointOnYAxis, &pointOnYAxis); // 转到世界坐标系{W}下

	// cube中心位置索引
        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;

        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。

       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];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              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();
            }
          }
          // 对应索引加1
          centerCubeI++;
          laserCloudCenWidth++;
        }

        while (centerCubeI >= laserCloudWidth - 3) {	
          // 将点的指针向中心方向平移
          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];
              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的邻域内找对应的配准点了。

       int laserCloudValidNum = 0;
        int laserCloudSurroundNum = 0;
        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) {

		// 计算子cube对应的点坐标 
                // NOTE: 由于ijk均为整数，坐标取值为中心点坐标
                float centerX = 50.0 * (i - laserCloudCenWidth);
                float centerY = 50.0 * (j - laserCloudCenHeight);
                float centerZ = 50.0 * (k - laserCloudCenDepth);
                
                // 取邻近的8个点坐标
                bool isInLaserFOV = false;
                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);

                      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) {
                        isInLaserFOV = true;
                      }
                    }
                  }
                }

                if (isInLaserFOV) {
                  laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 
                                                       + laserCloudWidth * laserCloudHeight * k;
                  laserCloudValidNum++;
                }
                laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 
                                                             + laserCloudWidth * laserCloudHeight * k;
                laserCloudSurroundNum++;
              }
            }
          }
        }

这里还需要判断一下该点是否属于当前Lidar的可视范围内，可以根据余弦公式对距离范围进行推导。根据代码中的式子，只要点在x轴±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(); // 有效的特征面上的点的个数

	// 将世界坐标系下的当前帧特征点转到当前Lidar坐标系下
        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中，并在地图数组中保存。

       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]);

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

          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帧发布一次。

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

至此，我们就结束了LOAM三个节点代码的分析，最后一段代码后续更新。欢迎各位看官一起交流学习。