lego-loam代码分析（4)-mapping流程

run主流程

整个mapping为一个独立线程完成，通过输入的传感器信息：特征点云和odometry两个信息，进行mapping；其程序入口为

void MapOptimization::run() {}

其整个slam流程如下：

• 当前帧根据odom转换世界坐标系；
• 提取submap用于点云匹配；
• 对当前帧激光点云进行降采样；
• 当前帧对submap进行匹配优化；
• 存储当前帧pose信息和cloud；
• 闭环优化后结果更新；

transformAssociateToMap

基于获取的里程计信息，估算mapping时位姿。源码中采用基本的坐标转换，代码难以理解，需要仔细推导。其基本思想为，根据当前odometry信息和上时刻的odometry，可获取运动增量。根据上刻的在mapping中的位姿和运动增量从而获取mapping当前时刻的估计位置。代码中变量解释如下：

// transformSum为当前里程计信息
// transformBefMapped 上次里程计信息
// transformAftMapped 上次map中的位置
// transformTobeMapped 为里程计估计的当前时刻的位置

如果采用Eigen库代替，即采用矩阵运算，则代码形式如下：

transformTobeMapped = transformAftMapped * transformBefMapped.transpose() * T_w_laserOdom;

提取submap（extractSurroundingKeyFrames）

此函数主要对历史存储的pose及其对应点云cloud进行提取，根据是否闭环需求分成两种提取方法；lego-loam相对于loam为改进版，最主要的改进有两点：1.scan-to-scan改为scan-to-map降低帧间匹配的累计误差；2.增加后端闭环优化； 而提取的submap则为其中关键的一步，即用于帧间匹配的map。
注：scan-to-map和后端闭环思想在许多slam中都已经应用，如cartographer，katro slam等，且原作者的代码很少使用库，如优化库和矩阵运算库等，其数学功底非常深厚。

非闭环处理

由于不做闭环处理，因此闭环检测和优化的时间可省略，为提高每一步增量匹配更加精准，采用历史缓存中与当前位置距离在一定范围内所有keypose的集合，根据keypos对应的所有点云形成submap；

    kdtreeSurroundingKeyPoses.setInputCloud(cloudKeyPoses3D);
    kdtreeSurroundingKeyPoses.radiusSearch(
        currentRobotPosPoint, (double)_surrounding_keyframe_search_radius,    // 没有闭环时，仅搜索与当前位置在50m内的位置，用于构建submap
        pointSearchInd, pointSearchSqDis);

    for (int i = 0; i < pointSearchInd.size(); ++i){                                                    // 将50m内所有帧对应位置放入队列中
      surroundingKeyPoses->points.push_back(
          cloudKeyPoses3D->points[pointSearchInd[i]]);
    }

    for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {        // 将附近搜索范围内的n组点云形成一个整点云
      *laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
      *laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
      *laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
    }

注：代码在维护这个集合时，集合中采用增加和删减的keypose（满足和不满足条件），而非简单的每次重新构建

闭环处理

如果需要做闭环处理，则应提高匹配性能，且做简单的增量submap即可，即采用当前位置索引提取历史中前方按顺序索引的N个位置。即采用滑窗方法仅使用最近的N个点云数据形成submap；其目的是暂时无需考虑其他点云带来的影响，仅是增量mapping，如此可提高相邻帧的匹配精度，而整体mapping的累计误差则需要回环修正。
闭环处理获取的submap，仅是整个点云中的一部分，其大小即为关键帧的个数，采用滑窗方法，仅保留最新的设置个数的keypose形成的点云集合；

   if (recentCornerCloudKeyFrames.size() <
        _surrounding_keyframe_search_num) {  // queue is not full (the beginning
                                         // of mapping or a loop is just
                                         // closed)
                                         // clear recent key frames queue
      recentCornerCloudKeyFrames.clear();
      recentSurfCloudKeyFrames.clear();
      recentOutlierCloudKeyFrames.clear();
      int numPoses = cloudKeyPoses3D->points.size();             // 按照最近遍历每一个轨迹点 location pose放入滑窗队列中
      for (int i = numPoses - 1; i >= 0; --i) {
        int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
        PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
        updateTransformPointCloudSinCos(&thisTransformation);
        // extract surrounding map
        recentCornerCloudKeyFrames.push_front(                       // 将历史帧按最新数据的顺序放入
            transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
        recentSurfCloudKeyFrames.push_front(
            transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
        recentOutlierCloudKeyFrames.push_front(
            transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
        if (recentCornerCloudKeyFrames.size() >= _surrounding_keyframe_search_num)
          break;
      }
    } else {  // queue is full, pop the oldest key frame and push the latest  如果最近帧数据队列已满，
              // key frame
      if (latestFrameID != cloudKeyPoses3D->points.size()-1) {       // 如果机器人移动，需要剔除原来的数据，加入新的帧数据
        // if the robot is not moving, no need to
        // update recent frames

        recentCornerCloudKeyFrames.pop_front();
        recentSurfCloudKeyFrames.pop_front();
        recentOutlierCloudKeyFrames.pop_front();
        // push latest scan to the end of queue
        latestFrameID = cloudKeyPoses3D->points.size() - 1;
        PointTypePose thisTransformation =
            cloudKeyPoses6D->points[latestFrameID];
        updateTransformPointCloudSinCos(&thisTransformation);
        recentCornerCloudKeyFrames.push_back(
            transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
        recentSurfCloudKeyFrames.push_back(
            transformPointCloud(surfCloudKeyFrames[latestFrameID]));
        recentOutlierCloudKeyFrames.push_back(
            transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
      }
    }

    for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i) {     // 构建submap的点云
      *laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
      *laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
      *laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
    }

最后提取出来的submap经过降采样进行存储。

当前帧降采样（downsampleCurrentScan）

由于构建的submap都采用了稀疏点云存储，因此输入传感器点云（包括角点、平面点、非特征点）也应降按照相同参数进行降采样；

scanTOmap优化匹配（scan2MapOptimization）

当目标点云数目足够多的时候，调用LM优化算法进行scanTOmap匹配。获取匹配后位姿；

  if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {   // 如果当前激光帧特征点个数达到一定个数时，才进行匹配优化
    kdtreeCornerFromMap.setInputCloud(laserCloudCornerFromMapDS);
    kdtreeSurfFromMap.setInputCloud(laserCloudSurfFromMapDS);

    for (int iterCount = 0; iterCount < 10; iterCount++) {                       // 优化匹配迭代10次，
      laserCloudOri->clear();
      coeffSel->clear();

      cornerOptimization(iterCount);
      surfOptimization(iterCount);

      if (LMOptimization(iterCount) == true) break;
    }

    transformUpdate();                                                           // 获取匹配后当前帧的位姿 

cornerOptimization

将当前点云中每个点在submap中寻找的最近5个点，作为对应点。求出五个点的协方差矩阵，进行QR分解，获取直线方程（需判断是否满足直线特征）。

      float cx = 0, cy = 0, cz = 0;
      for (int j = 0; j < 5; j++) {                                        // 
        cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
        cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
        cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
      }
      cx /= 5;
      cy /= 5;
      cz /= 5;                                                             // 获取5个点坐标均值 

      float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
      for (int j = 0; j < 5; j++) {
        float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
        float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
        float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

        a11 += ax * ax;
        a12 += ax * ay;
        a13 += ax * az;
        a22 += ay * ay;
        a23 += ay * az;
        a33 += az * az;
      }
      a11 /= 5;
      a12 /= 5;
      a13 /= 5;
      a22 /= 5;
      a23 /= 5;
      a33 /= 5;                                                           // 获取5个点的方差

      matA1(0, 0) = a11;
      matA1(0, 1) = a12;
      matA1(0, 2) = a13;
      matA1(1, 0) = a12;
      matA1(1, 1) = a22;
      matA1(1, 2) = a23;
      matA1(2, 0) = a13;
      matA1(2, 1) = a23;
      matA1(2, 2) = a33;                                                 // 获取协方差矩阵，即分布状况

      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(matA1);

      matD1 = esolver.eigenvalues().real();                              // 获取特征值和特征向量
      matV1 = esolver.eigenvectors().real();                             // 特征向量

根据直线方程，先求出直线上的两个点PointA和PointB。与与里程计中scan-to-scan方法一致，求出对应的点到线的距离；
注意：获取的5个点不一定在一条直线上，需要判断是直线，才认为有效匹配，即(matD1[2] > 3 * matD1[1])

surfOptimization

将当前点云中每个点在submap中寻找的最近5个点，作为对应点。根据5个点构建平面方程，进行QR分解，获取平面方程解。

     // 创建Ax=B，并求解
      for (int j = 0; j < 5; j++) {
        matA0(j, 0) =
            laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
        matA0(j, 1) =
            laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
        matA0(j, 2) =
            laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
      }
      // QR分解求出平面方程
      matX0 = matA0.colPivHouseholderQr().solve(matB0);

      float pa = matX0(0, 0);
      float pb = matX0(1, 0);
      float pc = matX0(2, 0);
      float pd = 1;

已知平面方程，与里程计中scan-to-scan方法一致，求出对应的点到面的距离；
注意：获取的5个点不一定在同一平面上，需要判断是平面，才认为有效匹配

      // 平面是否满足条件，即5个点是否在平面阈值范围内
      bool planeValid = true;
      for (int j = 0; j < 5; j++) {
        if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                 pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                 pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z +
                 pd) > 0.2) {
          planeValid = false;
          break;
        }
      }

LMOptimization

与里程计中scan-to-scan中的LM优化方法一致。

其实本内容与里程计采用的方法基本一致，仅是将上一阵激光点云改为了submap（即多帧点云构建），其目的是降低因匹配带来的累计误差。LM优化方法完全一致，而寻找对应点的方法则是采用最小二乘方法，求解平面与直线方程。

存储当前帧pose信息和cloud(saveKeyFramesAndFactor)

采用最基本的图优化理论，存储关键轨迹，即稀疏的keypose。
（cartographer，基本图优化架构slam基本结构）

闭环优化

1.基于距离最近的特点，寻找可能闭环的关键点。前面已经经过，如果开启闭环操作时，其用于匹配的submap仅是相邻的几帧点云组建完成，因此具有较高的相邻匹配度，但是会存在一定的累计误差。但当前位置距离历史位置较近时，表明可能存在闭环。

  // find the closest history key frame
  std::vector<int> pointSearchIndLoop;
  std::vector<float> pointSearchSqDisLoop;
  kdtreeHistoryKeyPoses.setInputCloud(cloudKeyPoses3D);                            // 当前机器人所在位置附近，搜索历史所有范围在闭环搜索范围内（默认为7m） 的key location，
  kdtreeHistoryKeyPoses.radiusSearch(
      currentRobotPosPoint, _history_keyframe_search_radius, pointSearchIndLoop,
      pointSearchSqDisLoop);

  closestHistoryFrameID = -1;
  for (int i = 0; i < pointSearchIndLoop.size(); ++i) {                           // 所有在闭环搜索范围的位置ID
    int id = pointSearchIndLoop[i];
    if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0) {       // 所有范围内location的时刻与现在时刻需在30s以上，否则无需闭环
      closestHistoryFrameID = id;                                                 // 仅需找到一帧曾经的位置与当前位置在 搜索范围内，且时间满足30s以上
      break;
    }
  }

2. 找到最近历史中pose后，将最近pose索引前后5帧激光数据，即共11帧数据构建loop_submap，用于闭环匹配；

  // save history near key frames
  for (int j = - _history_keyframe_search_num; j <= _history_keyframe_search_num; ++j) {   // 将历史中找到的一帧closestHistoryFrameID的前后_history_keyframe_search_num个作为闭环的submap
    if (closestHistoryFrameID + j < 0 ||
        closestHistoryFrameID + j > latestFrameIDLoopCloure)
      continue;
    *nearHistorySurfKeyFrameCloud += *transformPointCloud(                                 // 包括 corner 和 surf两种特征点云
        cornerCloudKeyFrames[closestHistoryFrameID + j],
        &cloudKeyPoses6D->points[closestHistoryFrameID + j]);
    *nearHistorySurfKeyFrameCloud += *transformPointCloud(
        surfCloudKeyFrames[closestHistoryFrameID + j],
        &cloudKeyPoses6D->points[closestHistoryFrameID + j]);
  }

3. 利用当前cloud点云信息和loop_submap采用ICP进行匹配；

  // ICP Settings
  pcl::IterativeClosestPoint<PointType, PointType> icp;
  icp.setMaxCorrespondenceDistance(100);
  icp.setMaximumIterations(100);
  icp.setTransformationEpsilon(1e-6);
  icp.setEuclideanFitnessEpsilon(1e-6);
  icp.setRANSACIterations(0);
  // Align clouds
  icp.setInputSource(latestSurfKeyFrameCloud);                        // 历史帧队列中最新的一帧的点云
  icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);                 // 闭环检测提取的submap
  pcl::PointCloud<PointType>::Ptr unused_result(
      new pcl::PointCloud<PointType>());
  icp.align(*unused_result);                                          // icp 匹配

将可能闭环的keypos前后相邻几帧构建submap，并与当前帧采用ICP进行匹配，如果匹配度满足一定阈值时，表明存在闭环，并获取闭环匹配的位姿转移矩阵。

闭环优化

采用gtsam库，对位姿进行优化，与g2o相类似，以位姿pose为节点，两个位姿差为约束条件。当闭环存在时，即出现闭环约束时，进行优化后，更新位姿；