cartographer 代码思想解读（3）- ceres优化库scan-match

前两节分析了cartographer 中的相关匹配思想和相关匹配优化快速实现，但cartographer之所以局部slam即前端匹配的准确度极高，因为最终采用了优化匹配的方法，即比栅格化的地图相关匹配准确度更高。而cartographer将匹配转换成最小二乘思想，并采用自家的ceres库完成优化匹配。

其详细的代码解释可参考他人博客：
基于Ceres库的扫描匹配器

ceres匹配简单总结

//采用ceres库求解
/*
input:
1.估计位置
2.初始位置
3.点云
4.栅格地图
输出：
1.最佳优化值
2.优化信息描述
 */
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                               const transform::Rigid2d& initial_pose_estimate,
                               const sensor::PointCloud& point_cloud,
                               const Grid2D& grid,
                               transform::Rigid2d* const pose_estimate,
                               ceres::Solver::Summary* const summary) const {
    // 估计位置初始值
  double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                   initial_pose_estimate.translation().y(),
                                   initial_pose_estimate.rotation().angle()};
    //求解器
  ceres::Problem problem;
  CHECK_GT(options_.occupied_space_weight(), 0.);
  //两种类型
  switch (grid.GetGridType()) {
      // 概率地图
    case GridType::PROBABILITY_GRID:
        // 增加匹配的代价函数， 添加误差项
      problem.AddResidualBlock(
          CreateOccupiedSpaceCostFunction2D(
              options_.occupied_space_weight() /
                  std::sqrt(static_cast<double>(point_cloud.size())),
              point_cloud, grid),
          nullptr /* loss function */, ceres_pose_estimate);   
      break;
    case GridType::TSDF:
      problem.AddResidualBlock(
          CreateTSDFMatchCostFunction2D(
              options_.occupied_space_weight() /
                  std::sqrt(static_cast<double>(point_cloud.size())),
              point_cloud, static_cast<const TSDF2D&>(grid)),
          nullptr /* loss function */, ceres_pose_estimate);
      break;
  }
  CHECK_GT(options_.translation_weight(), 0.);
  // 增加平移权重，代价函数， 平移代价，即优化的位置与target_translation，？？？？不理解，理论上迭代初始值不应该是预测的值吗
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation),
      nullptr /* loss function */, ceres_pose_estimate);
  CHECK_GT(options_.rotation_weight(), 0.);
  // 增加旋转权重，代价函数，？？？？，和优化本身比较，有什么意义？？？？？
  problem.AddResidualBlock(
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.rotation_weight(), ceres_pose_estimate[2]),
      nullptr /* loss function */, ceres_pose_estimate);

    // 优化器求解
  ceres::Solve(ceres_solver_options_, &problem, summary);

  *pose_estimate = transform::Rigid2d(
      {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

由于采用了ceres库进行优化求解，其流程较为简单，仅简单描述下其模型。将匹配转换成最小二乘的问题，即需建立最小二乘等式。scan-match包含3个代价函数，可配置其相关权重。

1. 点云在栅格地图的匹配程度，期望：匹配度越高；
2. 优化的pose与估计的target_pose偏移程度，期望：偏移量越小；
3. 优化的angle与init_pose的angle偏移程度，期望：偏移量越小；

其中2和3较为简单，可自行看源码，注意：第一节中的一个疑问，相关匹配后的结果也会考虑其与初始位置偏差进行权重化， 说明cartographer认为其匹配后的值应该与初始估计值相差不大。而点云在栅格地图的匹配为主要代价函数。

扫描匹配OccupiedSpaceCostFunction2D

点云在栅格地图的匹配程度如相关匹配方法一致，即将点云转换至地图坐标后，统计所有点云在栅格grid图中的概率值，越大表明匹配程度越高。由于为代价函数，因此期望匹配越高，则代价越低，故采用grid中的CorrespondenceCost替代probability值（CorrespondenceCost = 1-probability ）。
由于栅格地图中坐标是根据默认0.05m的分辨率进行采样得到，即地图坐标相对来说较为稀疏，之所以其优化精度高于相关匹配，Cartographer将grid进行了双三次插值，即可认为更高分辨率的栅格地图。采用了ceres自带的双三插值器，十分方便，其代码注释如下。

  // 输入：权重， 点云， 栅格地图
  OccupiedSpaceCostFunction2D(const double scaling_factor,
                              const sensor::PointCloud& point_cloud,
                              const Grid2D& grid)
      : scaling_factor_(scaling_factor),
        point_cloud_(point_cloud),
        grid_(grid) {}
  
  // 类型模板
  template <typename T>
  // pose为输入待优化量， residual为参差
  bool operator()(const T* const pose, T* residual) const {
    // 平移矩阵
    Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
    // 旋转向量
    Eigen::Rotation2D<T> rotation(pose[2]);
    // 旋转矩阵
    Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
    // 2维转移矩阵， 即当前位置在世界坐标系下的转移矩阵
    Eigen::Matrix<T, 3, 3> transform;
    transform << rotation_matrix, translation, T(0.), T(0.), T(1.);

    // 重新定义grid
    const GridArrayAdapter adapter(grid_);
    // 这里将构造时传入的概率栅格图（local submap）加载到一个双三次插值器中
    // Grid2D还可以利用BiCubicInterpolator实现双三次插值，它相对于双线性插值的优点是能实现自动求导
    ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
    const MapLimits& limits = grid_.limits();

    for (size_t i = 0; i < point_cloud_.size(); ++i) {
      // Note that this is a 2D point. The third component is a scaling factor.
      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].position.x())),
                                         (T(point_cloud_[i].position.y())),
                                         T(1.));
      // 将点云转换为世界坐标
      const Eigen::Matrix<T, 3, 1> world = transform * point;
      // 迭代评价函数
      // 将坐标转换为栅格坐标，双三次插值器自动计算中对应坐标的value
      interpolator.Evaluate(
          (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
              static_cast<double>(kPadding),
          (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
              static_cast<double>(kPadding),
          &residual[i]);
      // 所有参差加入同一权重
      residual[i] = scaling_factor_ * residual[i];
    }
    return true;
  }

总结

采用ceres优化匹配为cartographer 算法中前端核心匹配算法，而相关匹配和快速相关匹配则可作为第一步的预测匹配，可为优化匹配提供一个较好的初始值。其中真正前端中相关匹配方法可以进行配置不使能。