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

讲解关于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 {
      ......
  }

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

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() 函数进详细分析了。