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

讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人 联系方式, 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式,点击本人照片即可显示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个形参,如下所示:

1const Eigen::Vector2d& target_translation: 优化的目标位置,传入的实参为推断器或者经过相关性扫描匹配优化后的位置
2const transform::Rigid2d& initial_pose_estimate: 初始位姿,推断器推断出来的位置,没有经过相关性扫描匹配
3const sensor::PointCloud& point_cloud: 基于local坐标系,但是经过重力矫正的点云数据。
4const 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

该函数的实现步骤比较标准,如下:

( 1 ) \color{blue}(1) (1) 创建Problem,对应于源码 ceres::Problem problem。
( 2 ) \color{blue}(2) (2) 创建残差块,其主要有三个残差块,分别用于优化地图、平移与旋转。
( 3 ) \color{blue}(3) (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。

疑问 1 \color{red}疑问1 疑问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维)。

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

六、结语

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

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

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

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

 
 
 

你可能感兴趣的:(#,机器人,Cartographer,增强现实,无人机,自动驾驶)