(02)Cartographer源码无死角解析-(53) 2D后端优化→位姿图优化理论(SPA)讲解、核型函数调用流程

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

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

一、前言

通过前面的一系列博客，已经完成了2D点云扫描匹配：含相关性暴力搜索匹配以及ceres扫描匹配的讲解。总得来说，关于Cartographer前端数据处理以及前端位姿优化的相关代码已经深入解析。那么接下来，就是对后端部分代码讲解，这里暂时还是以2D后端优化为例，后续再向3D扩展。后端优化的的代码主要集中在如下文件：

src/cartographer/cartographer/mapping/internal/2d/pose_graph_2d.cc

源码中的实现，主要参考SPA论文：Efficient Sparse Pose Adjustment for 2D Mapping
论文翻译：https://blog.csdn.net/u014527548/article/details/106238658

二、位姿图理论

Cartographer 后端优化是使用位姿图的方式，如果了解g2o的朋友应该知道，图的核心在于点(节点)与边的建立，边主要起到约束作用，Cartographer 位姿图中包含了两种约束： $\color{red}子图内$ 与 $\color{red}子图间$ 约束，另外在回环检测中会计算子图间约束。

1、什么是图

由节点和边组成的一种数据结构, 节点之间的关系可以是任意的, 图中任意两节点之间都可能相关(存在边)。

2、什么是位姿图

SPA 论文中对位姿图的定义: 位姿图是一组通过非线性约束连接的机器人位姿, 这些非线性约束是从对附近位姿共有的特征的观察中获得的。位姿图是一种图, 节点代表位姿, 边代表 2 个位姿间的相对坐标变换(也叫约束)，如下三角形表示机器人位姿, 三角形之间的连线表示约束(坐标变换)：
在这里插入图片描述
如果对于图优化比较陌生的朋友可以参考本人博客：史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)

3.什么是优化

由于前端里程计会有累计误差, 那有没有一种方法可以将这种累计误差减小甚至消除掉呢?这就是优化的目的与作用。
在这里插入图片描述
左边表示没有优化之前、中间表示优化之后、右边表示真实地图。可以明显看到优化之后的精度提升较大。

三、残差项构建

通过前面博客的了解，可以知道优化问题核心就是在于残差项的构建，这里粘贴一下源码中的图示：
在这里插入图片描述
总的来说，可以分为如下几个步骤

$\color{blue} 第一步：$ 确定 2 个节点在 global 坐标系下的相对位姿变换。
$\color{blue} 第二步：$ 通过其他方式再次获取这 2 个节点的相对位姿变换
$\color{blue} 第三步：$ 对这 2 个相对位姿变换的差的最小二乘问题进行求解
$\color{blue} 第四步：$ 进行求解之后会得到一个增量 ?? , 将当前位姿加上这个增量后就得到了优
化后的位姿。
在这里插入图片描述
其上的存在疑问的是第二步，如何通过其他方式再次获取这 2 个节点的相对位姿变换，该部分内容再后续部分结合源码进行详细讲解。

四、约束(位姿变换)

cartographer 中是使用 ceres 进行位姿图优化，ceres求解的是残差和，至少有两个约束才能够进行 ceres 的位姿图优化。如果觉得约束这个词不好理解，那么直接认为是位姿变换即可(或许有些出入，但是问题不大)。很明显，如果只有一个约束(位姿变换)是没有办法进行优化的，至少需要两个位姿变换然后做残差，简单理解就是一个为待优化位姿，一个为目标位姿。

第一个约束就是节点 global 坐标系下的相对坐标变换，那么问题来了，第二个约束如何求得呢？源码中包含了如下类型约束，这里大致看一下即可，后续会进行详细分析。

$\color{blue} (01):$ 将节点(tracking 的位姿)与节点(子图原点位姿)在 global 坐标系下的相对位姿与约束(包含子图内约束与子图间约束) 的差值作为残差项。

$\color{blue} (02):$ landmark 数据与通过 2 个节点位姿插值出来的相对位姿的差值作为残差项

$\color{blue} (03):$ 节点与节点间在 global 坐标系下的相对坐标变换与通过里程计数据插值出的相对坐标变换的差值作为残差项

$\color{blue} (04):$ 节点与节点间在 global 坐标系下的相对坐标变换与相邻 2 个节点在local 坐标系下的相对坐标变换的差值作为残差项

$\color{blue} (05):$ 节点与 gps 坐标系原点在 global 坐标系下的相对坐标变换与通过 gps 数据进行插值得到的相对坐标变换的差值作为残差项

后续的讲解主要围绕以下函数或类型进行讲解(这里简单看一下即可)：

ComputeConstraintsForNode() //计算节点的子图内约束与子图间约束(回环检测)
ConstraintBuilder2D() //回环检测(计算子图间约束)
PrecomputationGridStack2D() //多分辨率地图
FastCorrelativeScanMatcher2D() //基于分支定界算法的粗匹配
OptimizationProblem2D() //优化问题的构建与求解

五、函数调用→PoseGraph2D构建

在对上述函数进行具体分析之前，先来回顾以下之前的内容。首先就是关于2D后端优化的创建位于 src/cartographer/cartographer/mapping/map_builder.cc 文件中的 MapBuilder 构造函数中，可以看到如下代码：

  // 2d位姿图(后端)的初始化根据
  if (options.use_trajectory_builder_2d()) {//如果使用2d追踪
    pose_graph_ = absl::make_unique<PoseGraph2D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem2D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  }

可以知道，其根据配置文件中的 src/cartographer/configuration_files/pose_graph.lua 的 optimization_problem_options 参数构建一个 optimization::OptimizationProblem2D 对象实例指针，然后利用还实例指针与 pose_graph.lua 文件中的 optimization_problem 以及线程池 thread_pool_ 共同构建了一个 PoseGraph2D 实例对象，然后赋值给成员变量 MapBuilder::pose_graph_。

也就是说，MapBuilder::pose_graph_ 与 MapBuilder::pose_graph_::optimization_problem_ 都是在MapBuilder构造函数中完成的，同时在 MapBuilder::AddTrajectoryBuilder()函数中还可找到如下代码：

    // CollatedTrajectoryBuilder初始化
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));

也就是说，没创建一条轨迹都增加一个 CollatedTrajectoryBuilder 对象指针到 MapBuilder::trajectory_builders_ 之中，且 CollatedTrajectoryBuilder 中包含一个 std::unique_ptr<TrajectoryBuilderInterface> 类型的成员变量 wrapped_trajectory_builder_。其上的 CreateGlobalTrajectoryBuilder2D() 函数返回的就是一个 TrajectoryBuilderInterface 类型的智能指针对象。总而言之，上述代码创建了 CollatedTrajectoryBuilder 实例(该实例包含 GlobalTrajectoryBuilder 实例对象), 然后添加至 MapBuilder::trajectory_builders_ 之中。

最终可知另外 GlobalTrajectoryBuilder 中包含了如下两个成员变量：

  PoseGraph* const pose_graph_;     // 模板参数, 可以指向PoseGraph2D也可以指向PoseGraph3D
  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;  // 模板参数

六、函数调用→PoseGraph2D使用

通过上面的分析，知道 GlobalTrajectoryBuilder 中包含成员变量 PoseGraph* const pose_graph_。GlobalTrajectoryBuilder 与 PoseGraph 的交互，即调用关系是在 src/cartographer/cartographer/mapping/internal/global_trajectory_builder.cc 文件中的 AddSensorData() 函数中体现的，基本每个AddSensorData() 重载函数都调用的类似 pose_graph_->Addxxxx() 的函数。首先来看一下关于雷达数据AddSensorData() 重载：

  void AddSensorData(
      const std::string& sensor_id, //订阅的话题
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
      ......
  }

该函数在前面已经分析过，这里再简单重复一下，其主要执行了如下部分代码：

1、扫描匹配

    // 通过前端进行扫描匹配, 然后返回匹配后的结果
    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
        matching_result = local_trajectory_builder_->AddRangeData(
            sensor_id, timed_point_cloud_data);

把点云数据送入前端进行扫描匹配，并且返回结果。该结果 matching_result 是十分重要的，起到前后端交互作用。这里依旧以2D为例，故其类型定义在 src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h 文件中：

  // 将点云插入到地图后的result
  struct InsertionResult {
    std::shared_ptr<const TrajectoryNode::Data> constant_data; //与子图相关，且扫描匹配之后不再改变的信息，也就是一个节点的数据。
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 最多只有2个子图的指针
  };
  // 扫描匹配的result
  struct MatchingResult {
    common::Time time; //点云时间戳
    transform::Rigid3d local_pose; //扫描匹配获得Robot在local地图下的位姿
    sensor::RangeData range_data_in_local; // 经过扫描匹配之后位姿校准之后的雷达数据,且以经过重力校正
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result; //存储子图子信息
  };

对其上的 constant_data 也是后续一个终点部分，其类型 TrajectoryNode::Data 定义于 src/cartographer/cartographer/mapping/trajectory_node.h 文件中：

  struct Data {
    common::Time time; //点云数据时间戳

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment; //重力校正旋转四元数

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud; //经过滤波之后的点云数据

    // Used for loop closure in 3D.用于3D回环检测，
    sensor::PointCloud high_resolution_point_cloud; //高分辨率点云数据
    sensor::PointCloud low_resolution_point_cloud; //低分辨率点云数据
    Eigen::VectorXf rotational_scan_matcher_histogram; //旋转扫描匹配直方图

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose; //节点相对于local SLAM frame(可以理解为lcoal地图)的位姿，MatchingResult::local_pose是一致的
  };

2、添加节点

经过扫描匹配匹配之后，AddSensorData() 函数会执行如下代码：

    // matching_result->insertion_result 的类型是 LocalTrajectoryBuilder2D::InsertionResult
    // 如果雷达成功插入到地图中
    if (matching_result->insertion_result != nullptr) {
      kLocalSlamInsertionResults->Increment();

      // 将匹配后的结果 当做节点 加入到位姿图中
      auto node_id = pose_graph_->AddNode(
          matching_result->insertion_result->constant_data, trajectory_id_,
          matching_result->insertion_result->insertion_submaps);
          
      CHECK_EQ(node_id.trajectory_id, trajectory_id_);

      // 这里的InsertionResult的类型是 TrajectoryBuilderInterface::InsertionResult
      insertion_result = absl::make_unique<InsertionResult>(InsertionResult{
          node_id, 
          matching_result->insertion_result->constant_data,
          std::vector<std::shared_ptr<const Submap>>(
              matching_result->insertion_result->insertion_submaps.begin(),
              matching_result->insertion_result->insertion_submaps.end())});
    }

其首先对匹配的结果进行判断，如果点云数据插入到子图中，即 matching_result->insertion_result != nullptr 成立，表示匹配成功。例如：经过滤波等操作之后可用点云过少、点云扫描匹配失败都会导致 matching_result->insertion_result == nullptr，即不会把点云数据插入到地图中。

其上的 kLocalSlamMatchingResults 与 kLocalSlamInsertionResults 本人展示没有看太明白。应该是起到计数作用，前者统计扫描匹配的次数，后者统计扫描成功次数。不过比较奇怪的点在于：

static auto* kLocalSlamMatchingResults = metrics::Counter::Null();
static auto* kLocalSlamInsertionResults = metrics::Counter::Null();

从上可以看出，kLocalSlamMatchingResults 为 metrics::Counter::Null() 函数的返回值，该函数实现于 src/cartographer/cartographer/metrics/counter.cc 文件中，但是其调用的：

kLocalSlamMatchingResults->Increment();
kLocalSlamInsertionResults->Increment();

是两个空函数，也就是说实际上什么都没有做。

接着下来AddSensorData() 函数把匹配的结果当作一个节点，调用 PoseGraph2D::AddNode() 函数添加到位姿图中。该函数是后面会进行十分详细的分析，其也是后台优化，或者说位姿图对外的一个接口。随后会把该节点相关的信息，构建成一个 InsertionResult 结构体指针，赋值给 insertion_result 变量。该结构体实现于 src/cartographer/cartographer/mapping/trajectory_builder_interface.h 文件中，如下：

  struct InsertionResult {
    NodeId node_id; //节点id
    std::shared_ptr<const TrajectoryNode::Data> constant_data; //子图相关恒定不变的一些信息，如过滤校正后的点云等
    std::vector<std::shared_ptr<const Submap>> insertion_submaps; //子图
  };

InsertionResult 其表示的是节点插入到位姿图之后，可能经过优化的结果，在把节点插入到位姿图中做优化的时候，有可能会改变子图。所以这里的子图都是以智能指针的方式进行传递的。

最后就是调用回调函数 local_slam_result_callback_() 把数据进行保存，至此，AddSensorData() 函数关于点云数据的处理就完成了。

七、结语

这里再插一张 SPA论文：Efficient Sparse Pose Adjustment for 2D Mapping 的图像：

优化前

在这里插入图片描述

优化后

在这里插入图片描述
可以明显的看到，没有优化之前，在回环部分存在严重的叠图现象，但是通过SPA优化之后，效果好了很多。下一篇博客就是对 PoseGraph2D::AddNode() 函数进详细分析了。

原文链接：https://blog.csdn.net/weixin_43013761/article/details/128833046