cartographer_ceres_scan_matcher_2d

0.理论部分

参考曾书格PPT：

同样的以 $M(x)$ 概率越大越好，通过 $1-M(x)$则转为求最小值，也就是转为了最优化问题。最终目标要求最优的 $T=(T_x,T_y,T_{\theta })$ 通过优化求解。

常数 2 不影响结果，直接丢掉。

cartographer 在代码实现过程中直接使用的 ceres 自动求导，没有对以上公式推导的细节进行代码实现。

误差项的定义与计算在如下文件中：

#include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h"

对外接口实现在 ceres_scan_matcher_2d.h中，核心函数为 Match 函数。

/**
 * @brief 基于Ceres的扫描匹配
 * 
 * @param[in] target_translation 预测出来的先验位置, 只有xy
 * @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta
 * @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
 * @param[in] grid 用于匹配的栅格地图
 * @param[out] pose_estimate 优化之后的位姿
 * @param[out] summary 
 */
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;
  // 1.点云匹配残差块
  CHECK_GT(options_.occupied_space_weight, 0.);
  problem.AddResidualBlock(
      OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
          // 总权重/点云数 = 每个point的权重
          options_.occupied_space_weight /
              std::sqrt(static_cast<double>(point_cloud.size())),
          point_cloud, grid),
      nullptr /* loss function */, ceres_pose_estimate);
  // 2.平移残差块
  CHECK_GT(options_.translation_weight, 0.);
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight, target_translation), // 平移的目标值
      nullptr /* loss function */, ceres_pose_estimate);    // 平移的初值
  // 3.旋转残差块
  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]);
}

1.残差项

残差主要有三个方面的来源：

• (1) 占用栅格与扫描数据的匹配度
• (2) 优化后的位置相对于target_translation的距离
• (3) 旋转角度相对于迭代初值的偏差

此外，还应注意到在配置文件中为这三类来源定义了权重。Cartographer为三类不同的残差源分别定义了一个类，并通过其静态函数CreateAutoDiffCostFunction提供使用Ceres原生的自动求导方法的代价函数计算。

2.残差计算

（1）占用栅格代价函数

class OccupiedSpaceCostFunction2D {
 public:
  // 类的构造函数设为私有, 禁止外部构造对象, 只能通过该函数进行构造
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const sensor::PointCloud& point_cloud,
      const Grid2D& grid) {
    return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
                                           ceres::DYNAMIC /* residuals */,
                                           3 /* pose variables */>(
        new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
        point_cloud.size());
  }

  // 重载运算符(), 用来计算残差
  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.);
    
    // 对grid_进行双三次插值
    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].x())),
                                         (T(point_cloud_[i].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]); // grid_表示的是miss的概率, 本身便是残差
      residual[i] = scaling_factor_ * residual[i];
    }
    return true;
  }

 private:
  static constexpr int kPadding = INT_MAX / 4; // 扩充边界

  // 对grid_进行一层封装, 从而能使用ceres自带的双三次插值器 BiCubicInterpolator
  class GridArrayAdapter {
   public:
    enum { DATA_DIMENSION = 1 };

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

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

  // 类的构造函数设为私有的, 禁止外部构造对象
  OccupiedSpaceCostFunction2D(const double scaling_factor,
                              const sensor::PointCloud& point_cloud,
                              const Grid2D& grid)
      : scaling_factor_(scaling_factor),
        point_cloud_(point_cloud),
        grid_(grid) {}

  OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
  OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
      delete;

  const double scaling_factor_; // 每个point的权重
  const sensor::PointCloud& point_cloud_;
  const Grid2D& grid_;
};

• step 1 将 pose 转为 transform
• step 2 对grid_进行双三次插值
• step 3 针对每个 hit 点计算对应的残差代价
  $$\underset{T_{\epsilon }}{\mathrm{argmin}}\sum _{k=1}^K{\left(1-M_{smooth}\left(T_{\epsilon }{h}_k\right)\right)}^2$$
  • 通过 hit 点坐标与 transform 的相乘得到其在地图坐标系下的坐标 $T_{\epsilon }{h}_k$
  • 通过刚刚构建的迭代器，和地图坐标，获取在hit点出现的概率。该函数调用有三个参数，前两个参数用来描述x,y轴索引， 第三个参数用于记录插值后的结果。这里的xy索引计算的比较奇怪，它通过GridArrayAdapter类中成员函数GetValue获取栅格数据，这里不再细述。
  • 由于占用栅格中原本存储的就是栅格空闲的概率， 所以这里查询出来的概率就是$\left(1-M_{smooth}\left(T_{\epsilon }{h}_k\right)\right)$ 也就是残差项。

（2）平移代价函数

class TranslationDeltaCostFunctor2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const Eigen::Vector2d& target_translation) {
    return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
                                           2 /* residuals */,
                                           3 /* pose variables */>(
        new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
  }

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

 private:
  // Constructs a new TranslationDeltaCostFunctor2D from the given
  // 'target_translation' (x, y).
  explicit TranslationDeltaCostFunctor2D(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
      : scaling_factor_(scaling_factor),
        x_(target_translation.x()),
        y_(target_translation.y()) {}

  TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete;
  TranslationDeltaCostFunctor2D& operator=(
      const TranslationDeltaCostFunctor2D&) = delete;

  const double scaling_factor_;
  const double x_;
  const double y_;
};

可以看出：

• 残差项：二维 --> x, y
• 待优化的参数：三维 --> x, y, theta
• scaling_factor_为平移误差的权重

（3）旋转代价函数

class RotationDeltaCostFunctor2D {
 public:
  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const double scaling_factor, const double target_angle) {
    return new ceres::AutoDiffCostFunction<
        RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>(
        new RotationDeltaCostFunctor2D(scaling_factor, target_angle));
  }

  template <typename T>
  bool operator()(const T* const pose, T* residual) const {
    residual[0] = scaling_factor_ * (pose[2] - angle_);
    return true;
  }

 private:
  explicit RotationDeltaCostFunctor2D(const double scaling_factor,
                                      const double target_angle)
      : scaling_factor_(scaling_factor), angle_(target_angle) {}

  RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete;
  RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) =
      delete;

  const double scaling_factor_;
  const double angle_;
};

可以看出：

• 残差项：一维 --> 角度
• 待优化的参数：三维 --> x, y, theta
• scaling_factor_为旋转误差的权重

4.总结

• 可能有问题的点
  1.平移和旋转的残差项是逼近于先验位姿的, 当先验位姿不准确时会产生问题
• 可能的改进建议
  1.先将地图的权重调大, 平移旋转的权重调小, 如 1000, 1, 1, 或者 100, 1, 1
  2.调参没有作用的时候可以将平移和旋转的残差项注释掉