Cartographer源码阅读2D-前端CSM-CeresScanMatcher2D

Cartographer源码阅读-2D前端CSM-CeresScanMatcher2D

基本原理：

其实，在代码里实现的时候，又加上了旋转和平移的残差，防止点云匹配的结果和初值差太多。

CeresScanMatcher2D

class CeresScanMatcher2D {
 public:
  explicit CeresScanMatcher2D(const proto::CeresScanMatcherOptions2D& options);
  virtual ~CeresScanMatcher2D();

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

  // Aligns 'point_cloud' within the 'grid' given an
  // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
  // 'summary'.
  void Match(const Eigen::Vector2d& target_translation,
             const transform::Rigid2d& initial_pose_estimate,
             const sensor::PointCloud& point_cloud, const Grid2D& grid,
             transform::Rigid2d* pose_estimate,
             ceres::Solver::Summary* summary) const;

 private:
  const proto::CeresScanMatcherOptions2D options_;
  ceres::Solver::Options ceres_solver_options_;
};

// CSM入口函数
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.);
    // 点云匹配残差块
    problem.AddResidualBlock(
        OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
            options_.occupied_space_weight() /
            std::sqrt(static_cast<double>(point_cloud.size())),
            point_cloud, grid),
        nullptr /* loss function */, ceres_pose_estimate);
    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]);
}            

CSM中包含了三个残差：点云与grid的匹配残差、平移的残差、角度的残差。其中，点云匹配的权重总和为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.);
	// 构造二阶线性插值类
    const GridArrayAdapter adapter(grid_);
    ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
    const MapLimits& limits = grid_.limits();
    // 针对每个点，计算该点匹配残差：1-Smooth(Tp)
    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.));
      // Tp：将点转换到全局坐标系下
      const Eigen::Matrix<T, 3, 1> world = transform * point;
      // 计算1-Smooth(Tp)
      interpolator.Evaluate(
          // 传进来的时候x和y都分别加了kPadding，在这里为什么会在取概率时用limits的max的xy减掉真实的xy得到cell_index，这个会在后面插入submap时有解释，暂时展开分析
          (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;
  }

 private:
  // 注意，该kPadding是为了解决有些点可能跑到地图外面去的情况，所以加了一个超大的值，即将地图上下左右边界分别扩大kPadding
  static constexpr int kPadding = INT_MAX / 4;
  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 {
      // 跑到了地图外面，在扩大的地方，直接赋值最大cost
      if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
          column >= NumCols() - kPadding) {
        *value = kMaxCorrespondenceCost;
      } else {
        // 在地图里，直接取概率值，在这里需要减掉kPadding，因为在传进来的时候，已经加了kPadding
        *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_;
  const sensor::PointCloud& point_cloud_;
  const Grid2D& grid_;
};

关于二阶线性插值，稍后再分析。

2D平移残差块构建

比较简单，不做详细分析。

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

2D旋转残差块构建

比较简单，不做详细分析。

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));
  }
  // yaw角直接相减
  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_;
};