cartographer源码解析(4)

本节主要介绍ceres扫描匹配实现细节。从上节cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc中ScanMatch函数开始，我们发现实时相关性扫描匹配输出结果后，该结果会作为初值传入ceres_scan_matcher_进行精匹配。ceres_scan_matcher_是cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc中定义的类CeresScanMatcher2D的实例。其初始化仅仅是读取配置文件，主要关注Match函数。

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

  // added by yct
  std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
  char pose_str[2048];
  memset(pose_str, 0 ,2048);
  sprintf(pose_str, "

ceres matcher match called.
origin pose is: %.3f %.3f %.3f
", 
            ceres_pose_estimate[0], ceres_pose_estimate[1], ceres_pose_estimate[2]);
// 添加地图概率值代价
  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.);
  // 添加平移代价
  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]);

  // added by yct
  std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
  int duration =
      std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0).count();
  sprintf(pose_str+strlen(pose_str), "occupied, trans, rot weights: %.3f %.3f %.3f
", 
            options_.occupied_space_weight(), options_.translation_weight(), options_.rotation_weight());
  sprintf(pose_str+strlen(pose_str), "final pose is: %.3f %.3f %.3f
", 
            ceres_pose_estimate[0], ceres_pose_estimate[1], ceres_pose_estimate[2]);
  sprintf(pose_str+strlen(pose_str), "brief report: 
%s
solve time: %d us", 
            (summary->BriefReport()).c_str(), duration);
//   LOG(WARNING) << pose_str;
}

观察代码，我们发现ceres扫描匹配实际上将点云与地图的匹配定义成了一个优化问题，将代价函数定义为三个残差块(即地图概率值、平移量、旋转量)的和。此处地图概率值是默认使用概率值地图(probability_grid)的情况，tsdf地图暂时没研究，感兴趣可以自行尝试。平移量与旋转量残差块ceres仿函数定义如下，其功能相当于L2正则化，避免优化产生的结果过大，陷入局部最优解。

// cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h
template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[0] - x_);
    residual[1] = scaling_factor_ * (pose[1] - y_);
    return true;
  }

// cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h
template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[2] - angle_);
    return true;
  }

最关键的还是地图概率值残差块的定义，其原理是根据当前的旋转与平移变换点云，将点云中所有点的坐标映射到地图的整形坐标中，获得点云中每个点的概率值，并转换为代价。其中概率地图中定义墙壁概率值大，空白概率值小；代价则与之相反，墙壁代价小，空白代价大。因此当点云经过一个欧式坐标变换后，假如其中的点尽可能落在概率值地图中认为是墙壁的地方，其产生的地图代价最小，此时的变换为最优变换。地图残差块仿函数定义如下。

template <typename T>
  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();
    Eigen::Matrix<T, 3, 3> transform;
    transform << rotation_matrix, translation, T(0.), T(0.), T(1.);

    const GridArrayAdapter adapter(grid_);
    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;
      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提供的双三线性插值(bicubic linear interpolation)，代码中标记红色的部分即使用了该对象的部分。感兴趣可以查看ceres双三线性插值的源码，原理不复杂，即取当前坐标附近4*4的点，按先行后列的方式使用三阶函数(ax^3+bx^2+cx+d)线性插值，获取所有参数后，即可计算出该范围内任意位置的值与梯度。

解决了地图离散性的问题，还需要解决地图大小限制的问题，即一旦点云变换后存在部分脱离地图范围的点，这些点的代价值需要定义。cartographer中的做法是在地图周围增加一个巨大的边框(kPadding)，并且通过一个地图适配器定义点落在边框中的代价值。

private:
  static constexpr int kPadding = INT_MAX / 4;
  class GridArrayAdapter {
   public:
    enum { DATA_DIMENSION = 1 };

    explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}

    // map上下左右各增加一条kPadding，此范围内都是最大cost，map内部计算各自cost
    void GetValue(const int row, const int column, double* const value) const {
      if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
          column >= NumCols() - kPadding) {
        *value = kMaxCorrespondenceCost;
      } else {
        *value = static_cast<double>(grid_.GetCorrespondenceCost(
            Eigen::Array2i(column - kPadding, row - kPadding)));
      }
    }

    int NumRows() const {
      return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
    }

    int NumCols() const {
      return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
    }

   private:
    const Grid2D& grid_;
  };

解决了上述两点问题，ceres匹配功能就定义完善了。不过考虑到地图的梯度不均匀，ceres匹配对初值要求相当高。一旦初值离真实值过远，很容易陷入局部最优解，导致优化失败。这也是为什么前面提到ceres匹配对物体运动速度与角速度有限制。经测试该限制大致为速度V<2m/s，角速度Va<1.05rad/s。