1.前言

Lidar里程计的结果不准确，拼起来的点也完全不成样子，且它会不断发散，因此误差也会越来越大。就需要用与地图匹配的方式来优化这一结果。在得到第一帧点云时lidar就扫到了数万个点，此时Lidar的位置我们把它作为（0,0,0），在不考虑测量噪声的情况下这数万个点都是相对精确的，我们把这数万个点所构成的环境作为此时的地图。上一章介绍了通过Lidar里程计的方法可以估算lidar的相对运动，于是就可以将此时的Lidar位姿及此时的点按照我们估计的相对运动情况，转换到上一时刻（建立地图时的坐标系）的坐标系下，再和之前的地图进行匹配，就可以优化此时Lidar的位姿了。

同样的雷达建图部分主要包含里程计位姿传播，局部子地图构建，线面误差项优化求解位姿三部分。

2.里程计位姿传播（Odometry Pose Propagation）

由于 Lidar Mapping 处理频率较低（1Hz），但激光雷达里程计（Lidar Odometry，较高频率 10 Hz）可以提供连续的相对运动估计，因此需要利用里程计结果预测当前帧的初始位姿，通过 Lidar Odometry 计算得到的当前帧相对于上一帧的位姿变换。结合上一帧的世界坐标系位姿预测当前帧在世界坐标系的初始位姿，这个位姿作为 ICP 优化的初值。

void LaserMapping::process()
{
  if (!hasNewData()) {
    // waiting for new data to arrive...
    return;
  }

  // reset flags, etc.
  reset();

  // skip some frames?!?
  _frameCount++;
  if (_frameCount < _stackFrameNum) {
    return;
  }
  _frameCount = 0;

  pcl::PointXYZI pointSel;

  // relate incoming data to map将相关坐标转移到世界坐标系下->得到可用于建图的Lidar坐标
  transformAssociateToMap();
// 将上一时刻所有边特征转到世界坐标系下
  size_t laserCloudCornerLastNum = _laserCloudCornerLast->points.size();
  for (int i = 0; i < laserCloudCornerLastNum; i++) {
    pointAssociateToMap(_laserCloudCornerLast->points[i], pointSel);
    _laserCloudCornerStack->push_back(pointSel);
  }
// 将上一时刻所有面特征转到世界坐标系下
  size_t laserCloudSurfLastNum = _laserCloudSurfLast->points.size();
  for (int i = 0; i < laserCloudSurfLastNum; i++) {
    pointAssociateToMap(_laserCloudSurfLast->points[i], pointSel);
    _laserCloudSurfStack->push_back(pointSel);
  }

3.局部子地图构建（Submap Construction）

LOAM 采用 Voxel Cube 分块管理地图，在 Lidar Mapping 过程中，每一帧都会 从历史点云构建一个局部子地图，用于匹配当前帧点云。由于 Lidar 采样的点云较大，直接匹配所有历史点云的计算量太大，因此 只选择当前帧所在的局部区域作为匹配对象。

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

  _laserCloudCenWidth(10),//搜索邻域宽度, cm为单位,
        _laserCloudCenHeight(5),// 搜索邻域高度
        _laserCloudCenDepth(10),// 搜索邻域深度

        _laserCloudWidth(21),//子cube沿宽方向的分割个数，每个子cube 50mm =1m/20
        _laserCloudHeight(11), // 高方向个数 50mm
        _laserCloudDepth(21),// 深度方向个数 50mm
        _laserCloudNum(_laserCloudWidth * _laserCloudHeight * _laserCloudDepth),// 子cube总数

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

 pcl::PointXYZI 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.pos.x() + 25.0) / 50.0) + _laserCloudCenWidth;
  int centerCubeJ = int((_transformTobeMapped.pos.y() + 25.0) / 50.0) + _laserCloudCenHeight;
  int centerCubeK = int((_transformTobeMapped.pos.z() + 25.0) / 50.0) + _laserCloudCenDepth;

  if (_transformTobeMapped.pos.x() + 25.0 < 0) centerCubeI--;
  if (_transformTobeMapped.pos.y() + 25.0 < 0) centerCubeJ--;
  if (_transformTobeMapped.pos.z() + 25.0 < 0) centerCubeK--;
//如果取到的子cube在整个大cube的边缘则将点对应的cube的索引向中心方向挪动一个单位，这样做主要是截取边沿cube。
  while (centerCubeI < 3) {// 将点的指针向中心方向平移
    for (int j =