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

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

一、前言

在上一篇博客中，简单的介绍了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 对象
注意，其上的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更安全.
     */

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

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

$(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指针对象，添加到trajectory_builders_之中。

$(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指针对象，添加到trajectory_builders_之中。

$(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。