(三) MapBuilder类 | 沉默杀手

MapBuilder继承了抽象类MapBuilderInterface，是cartographer算法的核心类.MapBuilder包括了两个部分，其中TrajectoryBuilder用于Local Submap的建立与维护；PoseGraph部分用于Loop Closure.

源码中使用的变量是map_builder_，属于MapBuilder类型，它的赋值在MapBuilderBridge的构造函数；变量map_builder_bridge_的赋值又在Node的构造函数；最终是在node_main.cc中的

1
2
3

auto map_builder =
  cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
      node_options.map_builder_options );

具体怎么实现不用管，知道这是个智能指针就行

成员变量

MapBuilder私有成员变量

const proto::MapBuilderOptions options_;	//MapBuilder的配置项
common::ThreadPool thread_pool_;  //线程池，应该是为每一条trajectory都单独开辟一个线程
// 一个PoseGraph的智能指针
std::unique_ptr<PoseGraph>  pose_graph_; 
//收集传感器数据的智能指针
unique_ptr<sensor::CollatorInterface> sensor_collator_;
//一个向量，管理所有的TrajectoryBuilderInterface;应该是每一个trajectory对应了该向量的一个元素
// 每一个TrajectoryBuilder对应了机器人运行了一圈
vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>> trajectory_builders_;
//与每个 TrajectoryBuilderInterface 相对应的配置项
vector<proto::TrajectoryBuilderOptionsWithSensorIds> all_trajectory_builder_options_;

线程池和pose_graph_在这里。由此可见MapBuilder构造函数里，后端的线程就已经开始跑了

trajectory是机器人跑一圈时的轨迹，在这其中需要记录和维护传感器的数据。根据这个trajectory上传感器收集的数据，我们可以逐步构建出栅格化的地图Submap，但这个submap会随着时间或trajectory的增长而产生误差累积。
PoseGraph是用来进行全局优化，将所有的Submap紧紧tie在一起，构成一个全局的Map，消除误差累积。

构造函数

省略3D的情况：

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) 
{
	// 加载配置，注意 optimization::OptimizationProblem2D 对象
  pose_graph_ = absl::make_unique<PoseGraph2D>(
    options_.pose_graph_options(),
    absl::make_unique<optimization::OptimizationProblem2D>(
        options_.pose_graph_options().optimization_problem_options() ),
    &thread_pool_);
  // 构建修正器
  if (options.collate_by_trajectory())
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  else
    sensor_collator_ = absl::make_unique<sensor::Collator>();
}

在lua文件中找不到collate_by_trajectory这一项，默认没有开启，但在map_builder_options.proto中可以找到，如果我们想要配置这个选项，只需要在lua文件中添加MAP_BUILDER.collate_by_trajectory = true

MapBuilder类在构造时会对线程池, 位姿图进行初始化, 在添加新轨迹时会对前端类的对象进行初始化, 初始化之后才会去判读这个轨迹是否存在初始位姿.

AddTrajectoryBuilder

这个函数可以说是cartographer中最重要的函数之一

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>&  expected_sensor_ids,
    const proto::TrajectoryBuilderOptions&  trajectory_options,
    LocalSlamResultCallback   local_slam_result_callback)
{
  // 当有一个新的trajectory时，以向量的size值作为新的trajectory，加入到trajectory_builders_中
  const int trajectory_id = trajectory_builders_.size();
                                      // 省略 3D 部分
  /*LocalTrajectoryBuilder2D是不带Loop Closure的Local Slam, 包含了Pose Extrapolator, Scan Matching等
  这个类并没有继承TrajectoryBuilderInterface, 并不是一个TrajectoryBuilderInterface的实现
  而只是一个工具类，构造函数全是配置的赋值*/
  std::unique_ptr<LocalTrajectoryBuilder2D>  local_trajectory_builder;
  if (trajectory_options.has_trajectory_builder_2d_options() )
  {
    local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
        trajectory_options.trajectory_builder_2d_options(),
        // 找出expected_sensor_ids中的雷达的话题，比如scan1,scan2
        SelectRangeSensorIds(expected_sensor_ids) );
  }
  DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get())  );
  /*  类CollatedTrajectoryBuilder   继承了接口 TrajectoryBuilderInterface，这个才是真正的
  轨迹跟踪器，使用 sensor::CollatorInterface 接口来收集传感器数据，具体的类在 MapBuilder 构造函数最后
   CollatedTrajectoryBuilder是 sensor_bridges_构造函数中的对应最后一个参数的类型  */
  trajectory_builders_.push_back( absl::make_unique<CollatedTrajectoryBuilder>(
      trajectory_options, sensor_collator_.get(), trajectory_id,
      expected_sensor_ids,
      /*  函数返回类型实际是 std::unique_ptr<GlobalTrajectoryBuilder>，它实际上是一个模板类
      其模板列表中的  LocalTrajectoryBuilder2D 和  PoseGraph2D  分别是前端和后端的两个核心类型
      local_slam_result_callback 前端数据更新后的回调函数  */

      // 将2D前端和位姿图打包传入 CollatedTrajectoryBuilder
      CreateGlobalTrajectoryBuilder2D(
          std::move(local_trajectory_builder), trajectory_id,
          static_cast<PoseGraph2D*>(pose_graph_.get()  ),
          local_slam_result_callback)  )  );

一个MapBuilder的类对应了一次建图过程，在整个建图过程中，用于全局优化的PoseGraph的对象只有一个，即pose_graph_，而这个变量是在构造函数中就生成了。

每生成一个trajectory时都是调用AddTrajectoryBuilder来创建的。一个trajectory对应了机器人运行一圈，trajectory_id取的是trajectory_builders_.size()，所以开始是0.

在图建好后机器人可能多次运行，每一次运行都是新增一条trajectory，trajectory_id就会+1. 因此，需要动态地维护一个trajectory的列表。

  // 如果开启了纯定位模式 
  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get());
  /*  如果开始建图之前已经有了初始位置，将初始位置提供给pose_graph_对象
  比如说，我们检测到了一个Landmark。那么这时，我们可以新增加一条trajectory
  然后根据机器人与Landmark之间的相对位姿推算机器人相对于世界坐标系的相对位姿
  以该位姿作为新增加的trajectory的初始位姿。这样在检测到Landmark时就能有效降低累积误差*/
  if (trajectory_options.has_initial_trajectory_pose())
  {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()) );
  }
  // 将轨迹跟踪器的配置信息和传感器配置信息保存到容器all_trajectory_builder_options_中
  proto::TrajectoryBuilderOptionsWithSensorIds  options_with_sensor_ids_proto;
  for (const auto& sensor_id : expected_sensor_ids)
  {
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;

  all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
  CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());
  return trajectory_id;
}

has_initial_trajectory_pose

其他函数

SerializeState：保存地图到pbstream，序列化当前状态到一个proto流中
LoadState：加载pbstream地图，从一个proto流中加载SLAM状态，逐条trajectory恢复，恢复trajectory上的节点间的约束，恢复Submap
FinishTrajectory：结束指定id的轨迹，调用sensor_collator_->FinishTrajectory和pose_graph_->FinishTrajectory
SubmapToProto: 根据指定的submap_id来查询submap，把结果放到SubmapQuery::Response中。如果出现错误，返回error string; 成功则返回empty string. 对应服务kSubmapQueryServiceName，调用关系如下：

Node::HandleSubmapQuery
|
MapBuilderBridge::HandleSubmapQuery
|
MapBuilder::SubmapToProto