讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面的一系列博客，已经完成了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 位姿图中包含了两种约束：$子图内$ 与 $子图间$ 约束，另外在回环检测中会计算子图间约束。

1、什么是图

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

2、什么是位姿图

SPA 论文中对位姿图的定义: 位姿图是一组通过非线性约束连接的机器人位姿, 这些非线性约束是从对附近位姿共有的特征的观察中获得的。位姿图是一种图, 节点代表位姿, 边代表 2 个位姿间的相对坐标变换(也叫约束)，如下三角形表示机器人位姿, 三角形之间的连线表示约束(坐标变换)：

如果对于图优化比较陌生的朋友可以参考本人博客：史上最简SLAM零基础解读(10.1) - g2o(图优化)→简介环境搭建(slam十四讲第二版为例)

3.什么是优化

由于前端里程计会有累计误差, 那有没有一种方法可以将这种累计误差减小甚至消除掉呢?这就是优化的目的与作用。

左边表示没有优化之前、中间表示优化之后、右边表示真实地图。可以明显看到优化之后的精度提升较大。
 

三、残差项构建

通过前面博客的了解，可以知道优化问题核心就是在于残差项的构建，这里粘贴一下源码中的图示：

总的来说，可以分为如下几个步骤

$第一步：$ 确定 2 个节点在 global 坐标系下的相对位姿变换。
$第二步：$ 通过其他方式再次获取这 2 个节点的相对位姿变换
$第三步：$ 对这 2 个相对位姿变换的差 的最小二乘问题进行求解
$第四步：$ 进行求解之后会得到一个增量 𝛥𝑥 , 将当前位姿加上这个增量后就得到了优
化后的位姿。

其上的存在疑问的是第二步，如何通过其他方式再次获取这 2 个节点的相对位姿变换，该部分内容再后续部分结合源码进行详细讲解。
 

四、约束(位姿变换)

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

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

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

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

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

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

$(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 {