(02)Cartographer源码无死角解析-(21) MapBuilder→AddTrajectoryBuilder()

南宫浩皛

2023-12-01

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

在上一篇博客中，简单的介绍了MapBuilder的构造函数，该篇博客主要讲解的是 MapBuilder::AddTrajectoryForDeserialization() 函数，首先来回忆一下其是怎么被调用的，

//开始文件
src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc
	node.StartTrajectoryWithDefaultTopics(trajectory_options);
		AddTrajectory(options);
		  const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
		  	//Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
		  	map_builder_->AddTrajectoryBuilder()
		  	absl::make_unique<SensorBridge>()

map_builder_bridge_.AddTrajectory 函数首先调用了 map_builder_->AddTrajectoryBuilder() 创建一条轨迹，然后在调用 absl::make_unique<SensorBridge>() 为该轨迹绑定一个 SensorBridge 对象
注意，其上的map_builder_就是MapBuilder的实例对象, AddTrajectoryBuilder() 函数具体调用代码如下：

  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
    expected_sensor_ids, trajectory_options.trajectory_builder_options,
    // 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);
    });

二、参数介绍

从上面的调用代码，可以知道其需要闯入3个参数，在src/cartographer/cartographer/mapping/map_builder.cc 问文件中，可以看到该函数的定义：

/**
 * @brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id
 * 
 * @param[in] expected_sensor_ids 所有需要的topic的名字的集合
 * @param[in] trajectory_options 轨迹的参数配置
 * @param[in] local_slam_result_callback 需要传入的回调函数
 *        实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数
 * @return int 新生成的轨迹的id
 */
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids, //根据配置参数获得期待的传感器类型，主要为订阅topic名字
    const proto::TrajectoryBuilderOptions& trajectory_options, //追踪的配置参数
    LocalSlamResultCallback local_slam_result_callback) { //回调函数

三、构造函数逻辑分析

在介绍之前，先了解一下c++11的一些知识点：

    /**
     * c++11: static_cast关键字（编译时类型检查）: static_cast < type-id > ( expression )
     * 该运算符把expression转换为type-id类型, 但没有运行时类型检查来保证转换的安全性
      （1）用于基本数据类型之间的转换, 如把int转换为char, 把int转换成enum, 
      （2）把空指针转换成目标类型的空指针
      （3）把任何类型的表达式类型转换成void类型
      （4）用于类层次结构中父类和子类之间指针和引用的转换.

     * c++11: dynamic_cast关键字（运行时类型检查）: dynamic_cast < type-id > ( expression )
        该运算符把 expression 转换成 type-id 类型的对象. Type-id必须是类的指针、类的引用或者void *
        如果type-id是类指针类型, 那么expression也必须是一个指针
        如果type-id是一个引用, 那么expression也必须是一个引用

        dynamic_cast主要用于类层次间的上行转换（子类到父类）和下行转换（父类到子类）, 还可以用于类之间的交叉转换.
        在类层次间进行上行转换时, dynamic_cast和static_cast的效果是一样的；
        在进行下行转换时, dynamic_cast具有类型检查的功能, 比static_cast更安全.
     */

函数的内部主要分为如下几步：

$\color{blue} (1)：$ 获得轨迹 id，因为每条轨迹都会创建一个 CollatedTrajectoryBuilder 对象存储于trajectory_builders_之中，其size()就可以用作为 trajectory_id。另外，其没有是个设置 pose_graph_odometry_motion_filter 相关参数，所以 MotionFilter() 函数未执行。

$\color{blue} (2)：$ 如果使用3d轨迹→
①首先创建一个 LocalTrajectoryBuilder3D(前端) 类型智能指针，其主要未为3D前端的初始化。
②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph3D类型，PoseGraph3D为后端优化。
③利用前端LocalTrajectoryBuilder3D与后端PoseGraph3D通过CreateGlobalTrajectoryBuilder3D函数构建一个TrajectoryBuilderInterface智能指针对象
④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数，构建一个std::unique_ptr<mapping::TrajectoryBuilderInterface>指针对象，添加到trajectory_builders_之中。

$\color{blue} (3)：$ 如果使用3d轨迹→
①首先创建一个 LocalTrajectoryBuilder2D(前端) 类型智能指针，其主要未为3D前端的初始化。
②尝试通过dynamic_cast函数把 pose_graph_ 原 PoseGraph 类型转换成 PoseGraph2D类型，PoseGraph2D为后端优化。
③利用前端LocalTrajectoryBuilder2D与后端PoseGraph2D通过CreateGlobalTrajectoryBuilder2D函数构建一个TrajectoryBuilderInterface智能指针对象
④结合TrajectoryBuilderInterface智能指针对象与trajectory_options、 sensor_collator_.get()、trajectory_id等参数，构建一个std::unique_ptr<mapping::TrajectoryBuilderInterface>指针对象，添加到trajectory_builders_之中。

$\color{blue} (4)：$ 判断是否为纯定位模式，如果是则只保存最近3个submap(老版本默认)，通过参数 pure_localization 参数控制。新版本可以通过设置 src/cartographer/configuration_files/trajectory_builder.lua 文件中的：

--  pure_localization_trimmer = {
--    max_submaps_to_keep = 3,
--  },

参数进行配置，先有 pure_localization_trimmer 这个参数，然后再配置其中的max_submaps_to_keep，默认设置依旧为3。其本质是通过PoseGraph::AddTrimmer() 函数，中的 PureLocalizationTrimmer 的实例对象进行控制的。

$\color{blue} (5)：$ 如果给了初始位姿，通过 pose_graph_->SetInitialTrajectoryPose 在位姿图中设置初始位姿。

$\color{blue} (6)：$ 保存轨迹的配置文件，每条轨迹都对应一个配置文件 proto::TrajectoryBuilderOptionsWithSensorIds 对象。

四、代码注释：

/**
 * @brief 创建一个新的 TrajectoryBuilder 并返回它的 trajectory_id
 * 
 * @param[in] expected_sensor_ids 所有需要的topic的名字的集合
 * @param[in] trajectory_options 轨迹的参数配置
 * @param[in] local_slam_result_callback 需要传入的回调函数
 *        实际上是map_builder_bridge.cc 中的 OnLocalSlamResult() 函数
 * @return int 新生成的轨迹的id
 */
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids, 
    const proto::TrajectoryBuilderOptions& trajectory_options, //追踪的配置参数
    LocalSlamResultCallback local_slam_result_callback) { //回调函数

  // id是从零开始的, 所以新trajectory_id就是trajectory_builders_的size()
  const int trajectory_id = trajectory_builders_.size();

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

  // LOG(INFO) << "pose_graph odometry_motion_filter is " << trajectory_options.has_pose_graph_odometry_motion_filter();
  // 上面会打印出0, 所以没有使用后端的里程计的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()));
  }

  // LocalTrajectoryBuilder 就是前端, 不带 Loop Closure 
  // 包含了 Pose Extrapolator, Scan Matching, 生成submap 等

  // 3d的轨迹
  if (options_.use_trajectory_builder_3d()) {
    // 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));
    } 

    /**
     * c++11: static_cast关键字（编译时类型检查）: static_cast < type-id > ( expression )
     * 该运算符把expression转换为type-id类型, 但没有运行时类型检查来保证转换的安全性
      （1）用于基本数据类型之间的转换, 如把int转换为char, 把int转换成enum, 
      （2）把空指针转换成目标类型的空指针
      （3）把任何类型的表达式类型转换成void类型
      （4）用于类层次结构中父类和子类之间指针和引用的转换.

     * c++11: dynamic_cast关键字（运行时类型检查）: dynamic_cast < type-id > ( expression )
        该运算符把 expression 转换成 type-id 类型的对象. Type-id必须是类的指针、类的引用或者void *
        如果type-id是类指针类型, 那么expression也必须是一个指针
        如果type-id是一个引用, 那么expression也必须是一个引用

        dynamic_cast主要用于类层次间的上行转换（子类到父类）和下行转换（父类到子类）, 还可以用于类之间的交叉转换.
        在类层次间进行上行转换时, dynamic_cast和static_cast的效果是一样的；
        在进行下行转换时, dynamic_cast具有类型检查的功能, 比static_cast更安全.
     */
    DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));

    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)));
  } 
  // 2d的轨迹
  else {
    std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder;
    if (trajectory_options.has_trajectory_builder_2d_options()) {
      // local_trajectory_builder(前端)的初始化
      local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
          trajectory_options.trajectory_builder_2d_options(),
          SelectRangeSensorIds(expected_sensor_ids));
    }

    DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph_.get()));

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

  // 是否是纯定位模式, 如果是则只保存最近3个submap
  MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
                                  pose_graph_.get());

  // 如果给了初始位姿
  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()));
  }

  // 保存轨迹的配置文件
  proto::TrajectoryBuilderOptionsWithSensorIds options_with_sensor_ids_proto;
  //对订阅的话题名字名字集合，或者说期待的传感器字段进行遍历
  for (const auto& sensor_id : expected_sensor_ids) {
    //把sensor_id转换成proto类型变量，然后添加到options_with_sensor_ids_proto之中
    *options_with_sensor_ids_proto.add_sensor_id() = ToProto(sensor_id);
  }
  //理解为对trajectory_builder_options变量进行赋值即可
  *options_with_sensor_ids_proto.mutable_trajectory_builder_options() =
      trajectory_options;
  //all_trajectory_builder_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;
}

五、总结

最后，这里在提及一个点，找到 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 中的如下函数：

// 开始一条新轨迹
int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {

  // Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      // 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);
      });
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  // Step: 2 为这个新轨迹 添加一个SensorBridge
  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan,
      trajectory_options.tracking_frame,
      node_options_.lookup_transform_timeout_sec, 
      tf_buffer_,
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder

其首先调用 map_builder_->AddTrajectoryBuilder() 添加一条新的轨迹，并且获得该轨迹的 trajectory_id。然后再根据 trajectory_id 为其绑定一个 SensorBridge 对象，注意上面再实例化 SensorBridge 对象的时候传入了一个这样的参数：

map_builder_->GetTrajectoryBuilder(trajectory_id)

这里的 map_builder_ 就是 MapBuilder 的实例，调用其中的 GetTrajectoryBuilder(trajectory_id) 函数：

  该函数声明于 src/cartographer/cartographer/mapping/map_builder.h 
  // 返回指向CollatedTrajectoryBuilder的指针
  mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
      int trajectory_id) const override {
    return trajectory_builders_.at(trajectory_id).get();
  }

从这里可以看到，GetTrajectoryBuilder() 函数，实际上就是根据 trajectory_id 返回一个 mapping::TrajectoryBuilderInterface 的普通指针。trajectory_builders_变量是不是有点熟悉，就是 MapBuilder::AddTrajectoryBuilder() 中将前端与位姿图打包在一起的 CollatedTrajectoryBuilder 对象指针。

通过该篇博客，对 MapBuilder 有了一定了解，但是其涉及的东西太多了，所以这里仅仅是讲解个大概，后续会对每一个函数进行具体的分析。

(02)Cartographer源码无死角解析-(21) MapBuilder→AddTrajectoryBuilder()

一、前言

二、参数介绍

三、构造函数逻辑分析

四、代码注释：

五、总结

相关阅读

相关文章

相关问答

相关文档