(02)Cartographer源码无死角解析-(51) 2D点云扫描匹配→ceres扫描匹配:CeresScanMatcher2D→平移旋转残差

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

前面在对 LocalTrajectoryBuilder2D::ScanMatch() 函数分析时提到，其有两个重要的部分需要进行分析，对于第一部分：

  // 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
  if (options_.use_online_correlative_scan_matching()) {
    
    
    const double score = real_time_correlative_scan_matcher_.Match(
        pose_prediction, filtered_gravity_aligned_point_cloud,
        *matching_submap->grid(), &initial_ceres_pose);
    kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
  }

也就是暴力扫描匹配，已经进行了细致的分析。接下来，就是分析如何使用 ceres 进行扫描匹配了，也就是 LocalTrajectoryBuilder2D::ScanMatch() 函数如下部分代码：

  auto pose_observation = absl::make_unique<transform::Rigid2d>();
  ceres::Solver::Summary summary;
  // 使用ceres进行扫描匹配
  ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                            filtered_gravity_aligned_point_cloud,
                            *matching_submap->grid(), pose_observation.get(),
                            &summary);

该函数具体实现于 src/cartographer/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc 文件中。

在这之前需要注意一个点，那就是在 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函数中调用 LocalTrajectoryBuilder2D::ScanMatch() 函数时，传入的实参 pose_prediction 是由位姿推断器推断出来的位姿。

总的来说呢，对于上述 ceres_scan_matcher_.Match() 函数的调用，如果配置了 use_online_correlative_scan_matching 参数，那么传入的实参 pose_prediction.translation() 就是经过暴力扫描匹配后机器人位置(经过重力矫正local坐标系)，否则就是 pose_prediction 与 initial_ceres_pose 一致，都表示没有经过暴力(相关性)扫描匹配，也就是推断器推断出来经过重力矫正的位姿。

二、CeresScanMatcher2D::CeresScanMatcher2D()

该构造函数比较简单，先看源代码如下：

CeresScanMatcher2D::CeresScanMatcher2D(
    const proto::CeresScanMatcherOptions2D& options)
    : options_(options),
      ceres_solver_options_(
          common::CreateCeresSolverOptions(options.ceres_solver_options())) {
    
    
  ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
}

其主要从配置文件中获取如下几个参数，然后存储于成员变量 ceres_solver_options_ 之中：

  -- ceres匹配的一些配置参数
  ceres_scan_matcher = {
    
    
    occupied_space_weight = 1., 
    translation_weight = 10., --残差平移的权重
    rotation_weight = 40., --残差旋转的权重
    ceres_solver_options = {
    
    
      use_nonmonotonic_steps = false,
      max_num_iterations = 20,
      num_threads = 1,
    },
  },

关于这些参数的具体作用，后续会进行讲解，另外其还设置了线性优化器的类型为 ceres::DENSE_QR，表示使用QR分解。

三、CeresScanMatcher2D::Match()

该函数为ceres优化的核心函数，其共6个形参，如下所示：

1、const Eigen::Vector2d& target_translation： 优化的目标位置，传入的实参为推断器或者经过相关性扫描匹配优化后的位置
2、const transform::Rigid2d& initial_pose_estimate： 初始位姿，推断器推断出来的位置，没有经过相关性扫描匹配
3、const sensor::PointCloud& point_cloud： 基于local坐标系，但是经过重力矫正的点云数据。
4、const Grid2D& grid： 用于匹配的子图，其存储了栅格信息
5、transform::Rigid2d* const pose_estimate： 用于存储优化过后的位姿
6、ceres::Solver::Summary* const summary： 用于优化过程信息的记录

其优化主要分为两个部分：①地图优化；②位姿优化；关于位姿优化部分的代码先对来说比较简单，地图部分的优化相对复杂一些。本博客中关于ceres的基础介绍就不进行讲解了，如果不是很明白的朋友可以找相关的文章阅读一下，或者通过如下链接：

概念介绍 http://www.ceres-solver.org/nnls_tutorial.html#introduction
官方教程 http://www.ceres-solver.org/nnls_tutorial.html#hello-world
中文翻译 https://blog.csdn.net/weixin_43991178/article/details/100532618

该函数的实现步骤比较标准，如下：

$\color{blue}(1)$ 创建Problem，对应于源码 ceres::Problem problem。
$\color{blue}(2)$ 创建残差块，其主要有三个残差块，分别用于优化地图、平移与旋转。
$\color{blue}(3)$ 调用 ceres::Solve() 函数，优化配置进行求解。

源码的简单注释如下，大家粗略看一下即可，后续进行纤细介绍：

/**
 * @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;
  
  // 地图部分的残差
  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.);
  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]);
}

四、TranslationDeltaCostFunctor2D

这里先对平移的残差进行讲解，后续再分析旋转与地图部分的残差块。对于 ceres 编程而言，其核心就是残差块的实现，另外，残差块最主要的部分就是定义一个类，该类需要实现 operator() 函数，以达到仿函数的目的(ceres基本要求)。

对于平移残差块的仿函数类实现于 src/cartographer/cartographer/mapping/internal/2d/scan_matching/translation_delta_cost_functor_2d.h 文件中，该函数包含一个静态的工厂函数，以及一个私有化的构造函数，也就是说想要实例化 TranslationDeltaCostFunctor2D 对象，只能调用其静态函数 TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction()

1、CreateAutoDiffCostFunction()

该函数的目的就是调用私有的构造函数，构建一个 TranslationDeltaCostFunctor2D 类型的实例，进一步利用该实例结合如下函数：

ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
                                           2 /* residuals */,
                                           3 /* pose variables */>

返回一个ceres::CostFunction对象，该类表示的是一个代价函数类，从上可以看出 ceres::AutoDiffCostFunction 是一个模板类。该模板类第一个模板参数是一个函数类型(代价函数)，当然也可以是一个仿函数。第二个非类型模板参数表示残差的维度，上面的2表示2维，因为后续要求的是x,y的残差。第三个非类型模板参数表示 block 0中的参数个数，这里只用到了 block 0，且传入初始平移是以 ceres_pose_estimate 形式传递的，所以这里3，表示3个参数。

总的来说，这里3表示输入变量的维度，2表示输入参数经过计算之后，与目标平移残差的维度。

2、TranslationDeltaCostFunctor2D()→构造函数

private:
  explicit TranslationDeltaCostFunctor2D(
      const double scaling_factor, const Eigen::Vector2d& target_translation)
      : scaling_factor_(scaling_factor),
        x_(target_translation.x()),
        y_(target_translation.y()) {
    
    }

需要注意的是，该构造函数被设置为 private，所以只能通过工厂函数 CreateAutoDiffCostFunction() 实例化 TranslationDeltaCostFunctor2D 对象。该构造函数需要传递两个两个参数：①scaling_factor→缩放因子，直接理解残差的权重即可。②target_translation→优化的目标，也就是优化之后的结构越靠近该数值，则表示效果越好。

3、operator()→仿函数实现，残差核心部分

  // 平移量残差的计算, (pose[0] - x_)的平方ceres会自动加上
  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;
  }

优化的最终目的就是希望 pose[0]与x_ 相等，pose[1]与y_相等。即 residual[0] 与 residual[1] 都为零，不过通常情况下达不到。这里的 pose 实际上就是传入进行来的，实际上 CeresScanMatcher2D::Match() 函数中定义的变量 ceres_pose_estimate。

4、总结

了解 TranslationDeltaCostFunctor2D 类之后，回过头来再看一下 CeresScanMatcher2D::Match() 函数中的如下代码：

  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation), // 平移的目标值, 没有使用校准后的平移
      nullptr /* loss function */, ceres_pose_estimate);      // 平移的初值

添加一个平移的残差块，只需要调用 TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction() 函数，传入优化目标 target_translation，初始位姿 ceres_pose_estimate 以及权重参数 translation_weight 即可。这里的损失函数设置为 nullprt，即表示不是用 loss function。

$\color{red}疑问1$ : 一个比较奇怪的地方是，这里为什么使用推断器推断、或者经过相关性扫描匹配优化后的位置作为优化目标。这样看起来，似乎都没有优化的必要了？直接使用 target_translation 不是更加准确吗？该疑问在后面进行解答！

五、RotationDeltaCostFunctor2D

了解 TranslationDeltaCostFunctor2D 之后，再来看 RotationDeltaCostFunctor2D 就比较简单了，因为他们是类似的，其实现于 src/cartographer/cartographer/mapping/internal/2d/scan_matching/rotation_delta_cost_functor_2d.h 文件中，同样存在一个静态工厂函数 CreateAutoDiffCostFunction。

该工厂首先调用私有构造函数，构建一个 RotationDeltaCostFunctor2D 实例，然后利用该实例创建一个如下：

ceres::AutoDiffCostFunction<
        RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>;

对象。易知该是一个模板类，第一个模板参数为可调用对象 RotationDeltaCostFunctor2D，其重载了 operator()。因为只对角度进行残差，其是1维的，所以第二个非类型模板参数是1。这里的第三个非类型模板参数是3，因为传入的初始位姿 ceres_pose_estimate 是3三维的，可以理解为输入数据是3维。

与 RotationDeltaCostFunctor2D 一样，重载了 operator()，该函数十分简单：

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

其目标就是希望 pose[2] 与 angle_ 相等，angle_ 表示优化的目标，通过构造函数传递进来，pose 表示优化之后的结果，实际上就是 ceres_pose_estimate() 函数中创建的 ceres_pose_estimate 变量(3维)。

$\color{red}疑问2$ 为什么在 CeresScanMatcher2D::Match() 函数中，角度优化的目标是 ceres_pose_estimate[2]？

六、结语

TranslationDeltaCostFunctor2D 与 RotationDeltaCostFunctor2D 这两个代价仿函数类中，都实现工厂函数 CreateAutoDiffCostFunction。该工厂函数的目的都是构建一个如下对象返回：

ceres::AutoDiffCostFunction<
        代价函数类型, 残差维度 /* residuals */, 输入数据维度/* pose variables */>

其为模板类，第一个参数为代价函数，或者说仿函数类型。第二个非类型模板参数表示进行残差数据的维度，第二个非类型模板参数表示输入数据的维度。另外，在该片博客中，遗留了如下疑问：

$\color{red}疑问1$ 与 $\color{red}疑问2$ 整合：CeresScanMatcher2D::Match()函数中，为什么平移残差优化的目标是 target_translation(由推断器或者暴力匹配获得)，为什么旋转的残差目标值设置为 ceres_pose_estimate[2]。这两个疑问看下后续分析过程中是否能找到答案。