前端 4. Ceres scan matcher

原理

在把一帧的scan插入到子图之前，scan 相对子图的位姿是由 ceres scan matcher 优化获得的。由于这是一个实时的局部优化，需要一个好的初始位姿估计。前面的real time CSM就是把位姿估计器传来的预测值更新为一个好的初值，如果没有real time CSM，就还用位姿估计器传来的预测值作初值。Ceres Scan Matcher以初值作为先验，并找到最佳的点，该点就是通过scan match获得的在子地图中的位置，实现方式是 interpolating the submap and sub-pixel aligning the scan. 前端的两个scan matcher其实都是 scan to map 问题，让当前观测和已知环境最匹配

scan matcher 负责在地图中找到一个scan pose，使得在该位姿下所有 hit 点在 local map中的占用概率之和最大，于是转化为一个最小二乘问题：

$arg\ \mathop{min} \limits_{\xi} \sum\limits_{k=1}^{K}(1-M_{smooth}\ \ (T_{\xi} H_k)\ )^2$

式中的 $T_{\xi}$ 就是我们需要求的位姿，它将scan坐标系中的点 (h_k)全部转化到 Submap坐标系下，让点 (h_k)最大概率地匹配到Submap。由于栅格中原本存储的就是空闲的概率，其实从栅格查出来的就是 $1-M_{smooth}\ \ (T_{\xi} H_k)$ 。相对地，real time scan match是占据概率之和最大，其实二者本质差不多，只是CSM换成最小二乘形式了，为了更精确。

在代码里实现的时候，又加上了旋转和平移的残差，防止点云匹配的结果和初值差太多。其实只要初值够好，这两个残差可以去掉。

这种方式的速度很快，计算复杂度高，鲁棒性好，但不能修正比子图分辨率大很多的误差。如果传感器和时间戳是合理的，只使用Ceres Scan Matcher通常是最好的选择。

Msmooth

Msmooth是一个将local子图中的各点概率进行平滑处理的函数，能够提供比栅格分辨率更好的精度。实质是双三次插值算法，该函数的输出结果为(0, 1)以内的数，实际返回了Submap中的占用概率，如果趋近1，那是很好了。

提高Scan matcher的频率可以获得更好优化结果，但计算量加大；或者用IMU来估计 scan match 中的旋转角度

ceres匹配对初值要求相当高，匹配后的结果会考虑其与初始位置偏差进行权重化，说明 cartographer认为其匹配后的值应该与初值相差不大。一旦初值离真实值过远，很容易陷入局部最优解，导致优化失败。这也是为什么ceres对线速度与角速度有限制。

auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
ceres_scan_matcher_.Match(pose_prediction.translation(), 
          initial_ceres_pose,
         filtered_gravity_aligned_point_cloud,
         *matching_submap->grid(),    pose_observation.get(),
         &summary);
// 省略:  统计残差， 观测值 - 预测值
return  pose_observation;

Match 函数

CeresScanMatcher2D类主要函数只有Match：根据初值，将点云hit点和栅格对齐，返回观测值

/*
 input:
 1. 上一个 scan 的位姿 target_translation
 2. 经过 RealTimeCorrelativeScanMatcher2D 处理过的
     当前的 scan 的位姿的初始估计  initial_pose_estimate
 3. 当前 scan 点云（2D）  point_cloud
 4. local map 概率分布栅格图   grid

 output:  更新的位姿估计  pose_estimate
*/
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
{
	// 优化的变量（3维）
  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(
      CreateOccupiedSpaceCostFunction2D(
          options_.occupied_space_weight() /
              std::sqrt(static_cast<double>(point_cloud.size())),
          point_cloud,   grid ),
      nullptr /* loss function */, ceres_pose_estimate );

    // 省略 GridType::TSDF
  CHECK_GT(options_.translation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单   AutoDiff
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.translation_weight(), target_translation),
      nullptr, ceres_pose_estimate);

  CHECK_GT(options_.rotation_weight(), 0.);
  // ceres 的套路都在函数里定义，很简单
  problem.AddResidualBlock(
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          options_.rotation_weight(), ceres_pose_estimate[2]),
      nullptr, ceres_pose_estimate);
   // ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
  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 matcher 将上面的最小二乘问题分解成了三个代价函数：点云与栅格的匹配残差、平移的残差、角度的残差。其中，点云匹配的权重总和为1，平移的权重和旋转的权重都是手动给的。

三个对应的核函数都是 nullptr， ceres的linear_solver_type是DENSE_QR

TranslationDeltaCostFunctor2D

两个Cost Function的实现，就是保证平移和旋转最小的代价函数

// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
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()) {}
  // 省略: 禁止拷贝构造函数 和 赋值运算符
  const double scaling_factor_;
  const double x_;
  const double y_;
};

这个代价函数比较简单，都是套路，RotationDeltaCostFunctor2D也是同理

平移量的残差计算，当前位姿逼近目标位姿 (位姿估计器输出的预测值xy)。目标函数
residual[0] = scaling_factor_ * (pose[0] - x_); residual[1] = scaling_factor_ * (pose[1] - y_);
逼近目标角度，目标函数scaling_factor_ * (pose[2] - angle_)
这里是逼近之前的位姿，所以要求先验位姿比较准确，经试验发现ceres输出的位姿和预测值相差极小，疑似存在问题，可以将这两个ceres优化去掉，但地图匹配必须保留

统计残差

观测值 - 预测值

if (pose_observation)
{
    kCeresScanMatcherCostMetric->Observe(summary.final_cost);
    const double residual_distance =
        (pose_observation->translation() - pose_prediction.translation())
            .norm();
    kScanMatcherResidualDistanceMetric->Observe(residual_distance);

    const double residual_angle =
        std::abs(pose_observation->rotation().angle() -
                 pose_prediction.rotation().angle());
    kScanMatcherResidualAngleMetric->Observe(residual_angle);
}

参考：
无处不在的小土
 jiajiading的分析
 叶落寒蝉的分析
 Ceres的使用