cartographer 代码思想解读（11）-顶层map_bulider类解析

最新推荐文章于 2023-07-19 14:40:12 发布

jiajiading

最新推荐文章于 2023-07-19 14:40:12 发布

阅读量920

点赞数 1

分类专栏： slam

本文链接：https://blog.csdn.net/jiajiading/article/details/109722837

版权

slam 专栏收录该内容

29 篇文章 91 订阅

订阅专栏

cartographer 代码思想解读（11）- 顶层map_bulider类解析

map_builder成员
MapBuilder构建函数
关键核心函数AddTrajectoryBuilder
FinishTrajectory 完成并结束一条轨迹
其他函数接口
总结

从本博客开始分析cartographer中的后端代码，无闭环的SLAM的算法代码和思想已基本分析完成。本节开始首先引入顶层接口类map_bulider类，实际上SLAM的顶层接口，并由cartographer_ros调用，不详细分析，主要是可说明cartographer的主要成员，包括前面分析的local slam。

map_builder成员

  const proto::MapBuilderOptions options_;   // 配置信息
  common::ThreadPool thread_pool_;           // 线程

  std::unique_ptr<PoseGraph> pose_graph_;  // 用于闭环， 包括ID，位置， 约束， 即每个submap的相关信息，所有submap一起进行闭环

  std::unique_ptr<sensor::CollatorInterface> sensor_collator_;             //Sensor 收集器
  std::vector<std::unique_ptr<mapping::TrajectoryBuilderInterface>>
      trajectory_builders_;                                    // trajectory build 序列， 每个元素可认为是维护了一个submap
                                                               // 即整个vector则维护了所有的submap
  std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>
      all_trajectory_builder_options_;                         // 每个trajectory 对应的 配置信息

all_trajectory_builder_options_：所有轨迹信息的配置信息
sensor_collator_：用于收集传感器数据的。
trajectory_builders_：前端核心类，即local slam的维护类，其维护了前端SLAM中的轨迹。这里需要指出不一定仅有一条轨迹，cartographer的高明之处。但一般情况下一次SLAM即仅有一条轨迹。
pose_graph_: 后端核心关键类；用于后端闭环检测和优化实现类。
thread_pool_：是线程池，可采用多线程并行处理，主要在后端中使用。

其中trajectory_builders_即是前10部分分析的前端slam内容。pose_graph_则为另一个最为重要的类，后端优化类。

MapBuilder构建函数

// 构建函数
// 包含两个全局类
// pose_graph_: 用于闭环； 
// trajectory_builders_: 用于local slam
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), thread_pool_(options.num_background_threads()) {
  CHECK(options.use_trajectory_builder_2d() ^
        options.use_trajectory_builder_3d());
  //创建2d地图优化器，用于构建全局地图和闭环操作
  if (options.use_trajectory_builder_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_);
  }
  if (options.use_trajectory_builder_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_);
  }
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
}

MapBuilder构建函数中主要构建了pose_graph_和sensor_collator_两个类，其中pose_graph_分为2d和3d两种配置。sensor_collator_是用来收集所需要传感器类型的的封装类，用来存储关心的传感器类型数据并村放入queue缓存。

其中sensor_collator_也包括两类，根据配置进行选择，但是cartographer_ros的configurtion中没有进行指示定义，在cartographer 中的configurtion_file中的mapbuilder.lua配置如下：

MAP_BUILDER = {
  use_trajectory_builder_2d = false,
  use_trajectory_builder_3d = false,
  num_background_threads = 4,
  pose_graph = POSE_GRAPH,
  collate_by_trajectory = false,
}

表明默认为false。

关键核心函数AddTrajectoryBuilder

AddTrajectoryBuilder是mapbuilder中最为关键的接口函数，其主要功能是创建一个新的轨迹，即SLAM中维持的一条的连续轨迹，依据此轨迹进行记录相关信息（牛逼的地方，可创建多条，暂时未用过，估计可用于地图扩展）。即用于localslam。

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback)

接口包含三个内容，
expected_sensor_ids：期望的传感器种类，即仅处理期望的sensor data，如laser、imu等，其他的不关心；
trajectory_options：轨迹的相关配置信息
local_slam_result_callback：local slam结果返回回调函数

  // 3d
  if (options_.use_trajectory_builder_3d()) {
  // 省略
  } else {
    // 2d 处理, 构建local规划器，没有闭环能力，实际应该是每个submap的local slam
    // 仅选择距离传感器描述信息，用于slam
    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(),
          SelectRangeSensorIds(expected_sensor_ids));
    }
    // 定义闭环节点
    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));
    // 将每个局部规划器，放入队列中， 包含规划器描述，需要校准的sensor， 当前id， 期望的sensor类型
    // 每个局部规划器需create 全局规划器，局部slam采用回调方法
    trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
        trajectory_options, sensor_collator_.get(), trajectory_id,
        expected_sensor_ids,
        CreateGlobalTrajectoryBuilder2D(
            std::move(local_trajectory_builder), trajectory_id,
            static_cast<PoseGraph2D*>(pose_graph_.get()),
            local_slam_result_callback)));
  }

根据2d和3d slam分成两部分处理，这里仅分析2d情况。
其核心便是构建一个LocalTrajectoryBuilder2D即轨迹跟踪器，前面10节分析的内容均是LocalTrajectoryBuilder2D功能，包括位置估计、相关匹配、优化匹配、submap构建以及更新等，即完成了除闭环优化外的所有功能。从代码可看出，即接口仅是配置信息和传感器信息。
而trajectory_builders_则是维护和存储所有轨迹类，包括配置信息，trajectory_id，传感器类型，和全局轨迹跟踪器。其中CreateGlobalTrajectoryBuilder2D为local slam与后端优化结合的桥梁，下节将详细分析。

  //判断是否为纯定位模式，如果是，内部可以不用闭环处理
  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());

cartographer也支持纯定位模式，因此如果是纯定位模式模式，后端处理经会有一定区别，即无需闭环处理。

  if (trajectory_options.has_initial_trajectory_pose()) {
    //获取配置中的初始位置
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();

    // 闭环pose_graph_设置初始位置，即当前ID，与初始ID的位置约束
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }

如果该轨迹有初始pose，应当获取给定的初始位置，初始位置主要用于建图和闭环中作为初始固定位置使用。而slam中其所有节点以及所有submap的位置均是相对初始位置的相对位置，而优化也是将相对位置进行优化。

  // 定义所有期望的即设置的 sensor种类配置转换成proto
  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;

将传感器及其配置信息进行序列化，进行存储，最后返回新建立trajectory_id。由代码可看出，轨迹可能不止一个，因此每个轨迹需要存储自身的配置信息，故创建一个新的轨迹时，判断下配置信息和轨迹的个数是否相等。

FinishTrajectory 完成并结束一条轨迹

void MapBuilder::FinishTrajectory(const int trajectory_id) {
  sensor_collator_->FinishTrajectory(trajectory_id);
  pose_graph_->FinishTrajectory(trajectory_id);
}

此方法是针对某一轨迹实现轨迹结束命令，即trajectory_id的轨迹不再增加的新的节点，也就是不再继续更新，也可以认为是此轨迹的slam结束。如此此轨迹的不再接收新的传感器信息，同时后端节点也将停止增加。