(02)Cartographer源码无死角解析-(28) GlobalTrajectoryBuilder构建过程与整体分析

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

一、前言

通过上一篇博客的复盘，已经可以很清晰的知道数据流动过程，总的来说，会依照时间的先后顺序，把数据传送给 GlobalTrajectoryBuilder。当然，对于 GPS 与 landmark 数据可以通过参数 collate_fixed_frame_，collate_landmarks_ 进行控制，是否对数据进行排序之后，再添加到 GlobalTrajectoryBuilder，默认配置 trajectory_builder.lua 中：

  collate_fixed_frame = true,  --是否将GPS数据放入阻塞队列中，按时间排序再进行分发
  collate_landmarks = false,  --是否将landmarks数据放入阻塞队列中，按时间排序再进行分发
1
2

在对 GlobalTrajectoryBuilder 进行深入分析之前，先来看看其构建过程。
 

二、GlobalTrajectoryBuilder实例

在 src/cartographer/cartographer/mapping/map_builder.cc 文件中的 MapBuilder::AddTrajectoryBuilde() 函数

	3D追踪
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        // 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder
        CreateGlobalTrajectoryBuilder3D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph3D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)));
1
2
3
4
5
6
7
8
9

	2D追踪
    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)));
1
2
3
4
5
6
7
8
9

其上可以看到，在对3D或者2D追踪，实例化 CollatedTrajectoryBuilder 对象的时候， 其需要一个 std::unique_ptr wrapped_trajectory_builder 参数，这里以2D为例，也就是如下函数的返回值：

 CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback, pose_graph_odometry_motion_filter)
1
2
3
4

CreateGlobalTrajectoryBuilder2D 是一个工厂函数，在前面提到过，类模板成员函数没有自动推演模板参数的功能，所以单独写了出来，
实现过程如下：

// 2d的完整的slam
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder,
    const int trajectory_id, mapping::PoseGraph2D* const pose_graph,
    const TrajectoryBuilderInterface::LocalSlamResultCallback&
        local_slam_result_callback,
    const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter) {
  return absl::make_unique<
      GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
      std::move(local_trajectory_builder), trajectory_id, pose_graph,
      local_slam_result_callback, pose_graph_odometry_motion_filter);
}
1
2
3
4
5
6
7
8
9
10
11
12

初步看起来比较复杂，其实不然，直接看return，其就是构建一个 GlobalTrajectoryBuilder 类型智能指针返回，共需要五个参数。
 

参数local_trajectory_builder

该参数在 MapBuilder::AddTrajectoryBuilder() 中创建，主要用于前端优化代码如下：

    // local_trajectory_builder(前端)的初始化
    std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_3d_options()) {
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>(
          trajectory_options.trajectory_builder_3d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    } 
1
2
3
4
5
6
7

 

参数 pose_graph_

该参数在 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_);
  }
  // 3d位姿图(后端)的初始化
  if (options.use_trajectory_builder_3d()) {//如果使用3d追踪
    pose_graph_ = absl::make_unique<PoseGraph3D>(
        options_.pose_graph_options(),
        absl::make_unique<optimization::OptimizationProblem3D>(
            options_.pose_graph_options().optimization_problem_options()),
        &thread_pool_);
  } 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

 

参数 local_slam_result_callback

local_slam_result_callback 是一个回调函数，该回调函数是src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 的 MapBuilderBridge::AddTrajectory 函数中的一个lambda 表达式：

      // lambda表达式 local_slam_result_callback_
      [this](const int trajectory_id, 
             const ::cartographer::common::Time time, 
             const Rigid3d local_pose,
             ::cartographer::sensor::RangeData range_data_in_local,
             const std::unique_ptr<
                 const ::cartographer::mapping::TrajectoryBuilderInterface::
                     InsertionResult>) {
        // 保存local slam 的结果数据 5个参数实际只用了4个
        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
      }
1
2
3
4
5
6
7
8
9
10
11

总的来说呢，local_slam_result_callback 等价于函数 MapBuilderBridge::OnLocalSlamResult() 函数，该函数的内容后面再讲解。
 

参数 pose_graph_odometry_motion_filter

参数 pose_graph_odometry_motion_filter 是再 MapBuilder::AddTrajectoryBuilde() 中创建，主要涉及代码如下：

  // 运动过滤器, 运动太小没必要进行更新
  // 配置文件中没有 pose_graph_odometry_motion_filte
  absl::optional<MotionFilter> pose_graph_odometry_motion_filter;

  if (trajectory_options.has_pose_graph_odometry_motion_filter()) {
    LOG(INFO) << "Using a motion filter for adding odometry to the pose graph.";
    pose_graph_odometry_motion_filter.emplace(
        MotionFilter(trajectory_options.pose_graph_odometry_motion_filter()));
  }
1
2
3
4
5
6
7
8
9

其主要功能是一个滤波器。
 

提示

$重点：$这里先不要深究，主要是回顾一下 GlobalTrajectoryBuilder 的构建过程。这里呢，额外介绍一点东西。src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 是 包 cartographer_ros 中的代码，简单的说，map_builder_bridge.cc 负责调用 Cartographer 的算法代码。

MapBuilderBridge 可以理解为 ros 侧封装的代码，其中的函数 MapBuilderBridge::AddTrajectory() 添加一条轨迹的时候会调用到 MapBuilder::AddTrajectoryBuilder() 函数，那么很显然，MapBuilder 也是 Cartographer 暴漏在外面接口，或者说共客户使用的类。

前面我们知道 CollatedTrajectoryBuilder 也是 Cartographer 暴露给客户的接口，那么问题就出现了，CollatedTrajectoryBuilder 与 MapBuilder 是怎么联系起来的？注意，在 MapBuilder 中有一个成员变量

std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>trajectory_builders_;
1

其存储的，就是所有创建轨迹是生成的 CollatedTrajectoryBuilder 实例对象。也就是说，对于 Cartographer 算法的使用，暂时只需要着重这 MapBuilder 与 CollatedTrajectoryBuilder 这两个类就行了，后续其他的在后面也会陆续讲解。
 

三、GlobalTrajectoryBuilder构造函数

GlobalTrajectoryBuilder.h 的头文件，就是声明了两个工厂函数，前面已经进行讲解。再次提及一下 GlobalTrajectoryBuilder 的实例对象，是存储在 CollatedTrajectoryBuilder::wrapped_trajectory_builder_ 之中。

下面来看看 GlobalTrajectoryBuilder 的构造函数，函数所示：

  GlobalTrajectoryBuilder(
      std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder,
      const int trajectory_id, PoseGraph* const pose_graph,
      const LocalSlamResultCallback& local_slam_result_callback,
      const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter)
      : trajectory_id_(trajectory_id),
        pose_graph_(pose_graph),
        local_trajectory_builder_(std::move(local_trajectory_builder)),
        local_slam_result_callback_(local_slam_result_callback), ///主要用于保存结果的回调函数
        pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {}
1
2
3
4
5
6
7
8
9
10

与前面创建实例的时候，传入的参数一一对应的，不过没有做什么复杂的操作，就是一个初始化列表，对如下的几个成员变量进行了赋值：

  const int trajectory_id_; //轨迹id
  PoseGraph* const pose_graph_;     // 模板参数, 可以指向PoseGraph2D也可以指向PoseGraph3D
  std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;  //前端
  LocalSlamResultCallback local_slam_result_callback_; //主要用于保存结果的回调函数
  absl::optional<MotionFilter> pose_graph_odometry_motion_filter_; //里程计过滤器，没有启用
1
2
3
4
5

 

四、AddSensorData() 重载函数

可以很明显的可以看到，实现了多个 AddSensorData() 重载函数，如下所示：

1、imu数据处理

  // imu数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_,一个是进入后端pose_graph_
  void AddSensorData(const std::string& sensor_id,
                     const sensor::ImuData& imu_data) override {
    if (local_trajectory_builder_) { //如果存在前端
      local_trajectory_builder_->AddImuData(imu_data); //把数据发送给前端
    }
    pose_graph_->AddImuData(trajectory_id_, imu_data); //把数据发送给后端
  }

1
2
3
4
5
6
7
8
9

 

2、里程计数据处理

  // 里程计数据的处理, 数据走向有两个,一个是进入前端local_trajectory_builder_, 一个是进入后端pose_graph_
  // 加入到后端之前, 先做一个距离的计算, 只有时间,移动距离,角度 变换量大于阈值才加入到后端中
  void AddSensorData(const std::string& sensor_id,
                     const sensor::OdometryData& odometry_data) override {
    //判断数据是否有值，如果没有值，则报错，且输出里程计数据                 
    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
    if (local_trajectory_builder_) { //如果存在前端
      local_trajectory_builder_->AddOdometryData(odometry_data);
    }
    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
    // odometry could be marginalized between nodes in the pose graph.
    // Related issue: cartographer-project/cartographer/#1768
    //如果设置则进行过滤
    if (pose_graph_odometry_motion_filter_.has_value() &&
        pose_graph_odometry_motion_filter_.value().IsSimilar(
            odometry_data.time, odometry_data.pose)) {
      return;
    }
    //把数据分发给后端
    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

 

3、GPS数据处理

  // gps数据只在后端中使用
  void AddSensorData(
      const std::string& sensor_id,
      const sensor::FixedFramePoseData& fixed_frame_pose) override {
    if (fixed_frame_pose.pose.has_value()) {
      CHECK(fixed_frame_pose.pose.value().IsValid())
          << fixed_frame_pose.pose.value();
    }
    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
  }
1
2
3
4
5
6
7
8
9
10

 

4、Landmark数据处理

  // Landmark的数据只在后端中使用
  void AddSensorData(const std::string& sensor_id,
                     const sensor::LandmarkData& landmark_data) override {
    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
  }

1
2
3
4
5
6

 

5、local slam 数据处理

  // 将local slam的结果加入到后端中, 作为位姿图的一个节点
  void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
                                  local_slam_result_data) override {
    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
                                         "local_trajectory_builder_ present.";
    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
  }
1
2
3
4
5
6
7

 

五、雷达数据处理

下面来重点讲解一下雷达数据的处理，代码如下：

 /**
   * @brief 点云数据的处理, 先进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中
   * 
   * @param[in] sensor_id topic名字
   * @param[in] timed_point_cloud_data 点云数据
   */
  void AddSensorData(
      const std::string& sensor_id, //订阅的话题
      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
    CHECK(local_trajectory_builder_)//检测是否存在前端，没有前端则报错
        << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";

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

    if (matching_result == nullptr) { //如果失败了，直接返回
      // The range data has not been fully accumulated yet.
      return;
    }
    kLocalSlamMatchingResults->Increment();
    std::unique_ptr<InsertionResult> insertion_result;

    // 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())});
    }

    // 将结果数据传入回调函数中, 进行保存
    if (local_slam_result_callback_) {
      local_slam_result_callback_(
          trajectory_id_, matching_result->time, matching_result->local_pose,
          std::move(matching_result->range_data_in_local),
          std::move(insertion_result));
    }

  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

如果看得不是是明白没有关系，下一篇博客会针对该函数进行十分详细的讲解。

六、结语

通过篇博客，可以知道，GlobalTrajectoryBuilder 中除了对雷达数据做了比较复杂的处理，对于其他的数据集，基本就是做了个转发工作。值得注意的是，GlobalTrajectoryBuilder::local_slam_result_callback_ 回调函数，是一个十分主要点，其主要起到保存结果的作用，具体的内容后面再进行讲解。