cartographer源码阅读四-MapBuilder

一、前言

回顾一下在哪里见过MapBuilder，先回忆一下，这个类的创建在src/cartographer _ros/cartographer_ros/cartographer_ros/node_main.cc 的 run 函数中, 可看到下代码:

1.初始构造过程node_main.cc

// MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

其调用了一个工厂函数，创建一个MapBuilder类对象，该函数的实现于src/cartographer/cartographer/mapping/map builder.cc文件中实现,代码如下:

std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
    const proto::MapBuilderOptions& options) {
  return absl::make_unique<MapBuilder>(options);
}

所谓的工厂函数，简单的说，就是根据传入的参数返回不同的类对象 ，如上面代码就是表示根据传入的options参数，构建一个MapBuilder实例返回其指针，注意，这里返回的指针指向MapBuilder实例，但是返回的类型为其父类std.unique ptr<MapBuilderinterface> ，返回的实例对象指针变量为auto map builder.

然后node_main.cc的run函数,利用map builder变量，构建Node类对象node,代码如下:

// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

2. Node类的构造函数

Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
// step:1 声明需要发布的topic
// step:2 声明发布对应名字的Ros服务，并将服务的发布器放入到vector容器中
// step:3处理之后的点云的发布器
// step:4 进行定时器与函数的绑定，定时发布数据

tf2_ros::Buffer* const tf_buffer用于监听tf,数据预预处理SensorBridge类经常有用到。

const bool collect metrics是否进行评估。对于这两个参数还是很好理解，但是传入的map builder变量，用于构建MapBuilderBridge对象map_builder_bridge_，而并不是直接赋值给Node的成员变量了。

总的来说，auto map_builder参数用于构建MapBuilderBridge对象，其构建的对象为Node中的成员变量MapBuilderBridge map_builder_bridge_。

3.MapBuilderBridge类的构造函数

该构造函数位于 src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中

/**
 * @brief 根据传入的node_options, MapBuilder, 以及tf_buffer 完成三个本地变量的初始化
 * 
 * @param[in] node_options 参数配置
 * @param[in] map_builder 在node_main.cc中传入的MapBuilder
 * @param[in] tf_buffer tf_buffer
 */
MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer) {}

从这里可以看到初始化列表中的map_builder_(std:.move(map_builder))代码。总的来说一路下来map_builder实际被赋值给了Node.map_builder_bridge_.map_builder_变量。

二、MapBuilderInterface

复习了前面的内容。下面看一下MapBuilder，通过map_builder.h文件可以知道其继承于MapBuilderInterface，先学习这里

class MapBuilder : public MapBuilderInterface 

MapBuilderInterface这个类实现于src/cartographer/cartographer/mapping/map_builder_interface.h，从命名上可以明显的看出，这是一个接口类，声明了大量纯虚拟函数，参数后面加上=0是为了告诉编译系统这是纯虚函数，

这里简单描述一下纯虚函数和与函数的区别：1.纯虚函数只有定义，没有实现；虚函数既有定义也有实现的代码。2.报春纯虚函数的类不能定义其对象，而包含虚函数的则可以

另外看到下面代码：

  using LocalSlamResultCallback =TrajectoryBuilderInterface::LocalSlamResultCallback;
  using SensorId = TrajectoryBuilderInterface::SensorId;

这里的using是C++11的新用法，简单理解为取别名，注意这里的LocalSlamResultCallback为一个函数指针，具体定义src/cartographer/cartographer/mapping/trajectory_builder_interface.h里，如下所示，后续在详细讲解

  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time,
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>;

三、proto简介

现在再回到头文件src/cartographer/cartographer/mapping/mapbuilder.h文件,来查看 class MapBuilder,该类包含前端(TrajectoryBuilders,scan to submap)与后端(用于查找回环的PoseGraph)的完整的SLAM。不过该头文件需要讲解的东西太多了，这样一看，不知道从哪里下手，那么就来看map_builder.cc 文件吧.

该头文件开头部分可以看的如下：

Cartographer 工程的命名空间使用是十分规范的，比如之前分析 src/cartographer_ros 下代码的时候，其最外层的命名空间为cartographer_ros，现在分析 src/cartographer 的代码，可以看到其命名空间为cartographer，另外其二级命名空间跟文件夹路径相关，如src/cartographer/cartographer/mapping 下的代码，都是使用

namespace cartographer {
namespace mapping {
}}

作为命名空间，另外这里还使用了匿名空间 ，表示再该命名空间内的变量与函数等只能在本文件中使用。另外，这里提及两个内容:

（1）在src/cartographer/cartographer/mapping/prot目录下，可以看到很多.proto后最的文件，其存储的都是参数信息，如其下的map_builder_options.proto文件，内容如下:

syntax = "proto3"; //定义这个文件的语法是proto3
import "cartographer/mapping/proto/pose_graph_options.proto";  //包含内容
package cartographer.mapping.proto; //命名空间

//参数编写为【字段类型 字段名称 = 字段序号】
message MapBuilderOptions { //类名
  bool use_trajectory_builder_2d = 1; //
  bool use_trajectory_builder_3d = 2;

  // Number of threads to use for background computations.
  int32 num_background_threads = 3;
  PoseGraphOptions pose_graph_options = 4;
  // Sort sensor input independently for each trajectory.
  bool collate_by_trajectory = 5;
}

在编译过程中，其会被生成c++类文件，如该文件对应生成的类文件为build_isolated/cartographer/install/cartographer/mapping/proto/map_builder_options.pb.h, 其为google::protobuf.:Message的派生类，且生成过程中，会为每个.proto定义的参数，自动生成三个函数，举例如下:

clear_num_background_threads() //恢复变量num_background_threads默认设置
num_background_threads() //获取变量num_background_threads内容
set_num_background_threads(::google::protobuf::int32 value)//设置变量num_background_threads内容

总的来说，就是把配置文件使用.proto文件编写，只需要配置变量名，然后通过指令就可以生成c++(java,python)的类文件了，其包含了对于配置参数的一些基本操作。这里只做一个简单的介绍，有兴趣深入了解的朋友，可以查阅相关的一些资料深入了解。

三、MapBuilder类的构造函数

MapBuilder构造函数实现于 src/cartographer/cartographer/mapping/map_builder.cc 文件中,注释下:

/**
 * @brief 保存配置参数, 根据给定的参数初始化线程池, 并且初始化pose_graph_与sensor_collator_
 * 
 * @param[in] options proto::MapBuilderOptions格式的 map_builder参数
 */
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options), 
    // 根据num_background_threads参数构建一个线程池
    thread_pool_(options.num_background_threads()) { // param: num_background_threads
  // 异或操作，相同为1，不同为1:表示2d或者3d追踪有且只有一个启动
  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_);
  }
  // 3d位姿图(后端)的初始化
  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_);
  } 

  // 在 cartographer/configuration_files/map_builder.lua 中设置
  // param: MAP_BUILDER.collate_by_trajectory 默认为false
  if (options.collate_by_trajectory()) {
    sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    // sensor_collator_初始化, 实际使用这个
    sensor_collator_ = absl::make_unique<sensor::Collator>();
  }
}

该构造函数首先进行了线程池thread_pool_的创建，然后对位姿图(后端)初始化，最后返回一个sensor:.Collator类型指针，因为collate_by_trajectory参数默认为false。

四、线程池

构建线程池的代码为 thread_pool_(options.num_background_threads(),

在头文件src\cartographer\cartographer\mapping\map_builder.h里thread_pool_的定义

private:
  const proto::MapBuilderOptions options_;
  common::ThreadPool thread_pool_; // 线程池

thread_pool_为ThreadPool的实例化,ThreadPool成员函数定义在 src/cartographer/cartographer/common/thread_pool.cc 文件中，其构造函数如下

// 根据传入的数字, 进行线程池的构造, DoWork()函数开始了一个始终执行的for循环
ThreadPool::ThreadPool(int num_threads) {
  CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!";
  absl::MutexLock locker(&mutex_);
  for (int i = 0; i != num_threads; ++i) {
    pool_.emplace_back([this]() { ThreadPool::DoWork(); });
  }
}

其中的pool_：
// 线程池
std::vector<std::thread> pool_ GUARDED_BY(mutex_);  

代码比较简单，就是根据num_threads创建线程，存储于pool_变量之中，这里需要注意ThreadPool::DoWork()内部为一个循环函数，也就是说线程被创建出来之后，就已经再运行了，当检检测到标志位为 cartographer::common::ThreadPool::running_=false, 则会跳出循环。

这里的pool_变量的定义分为两部分

基础部分：std::vector<std::thread> pool_，表明pool_是vector容器，里面存储着thread类的线程对象。

注释部分GUARDED_BY(mutex_):GUARDED_BY注解关键字，表达「受... 保护」的语义,mutex_是保护 pool_ 的互斥锁

线程池的 pool_ 是多线程共享的变量（比如：主线程可能添加线程、工作线程可能被析构函数 join），如果不加锁直接访问。

本篇小节

这里再提及一个MapBuilder 的构造函数，是在执行node_main.cc文件中如下代码:

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

构建的，因为这里创建了一个MapBuilder对象，也就是同时完成了线程池的初始化。接下来会讲解

-下，Cartographer 的追踪时如何开始的。

---

MapBuilder::AddTrajectoryBuilder()

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

在src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc创建完map_builder 之后，

// 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

这里StartTrajectoryWithDefaultTopics函数在src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc里

// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

int Node::AddTrajectory(const TrajectoryOptions& options) {

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

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);
........................................................................

在这里map_builder_bridge_.AddTrajectory 函数首先调用了 map_builder_->AddTrajectoryBuilder() 创建一条轨迹

我们回顾下Node类的map_builder_bridge_在哪里来的

// Node类的构造函数声明（参数列表）
Node::Node(
    const NodeOptions& node_options,  // 选项参数（const引用，避免拷贝）
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,  // 独占指针（所有权需要转移）
    tf2_ros::Buffer* const tf_buffer,  // 指针参数（const指针，指向的对象不可通过该指针修改）
    const bool collect_metrics)  // 布尔参数（是否收集指标）
    // 构造函数初始化列表
    : node_options_(node_options),  // 初始化node_options_成员
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {  // 初始化map_builder_bridge_成员
    ----------
}

用构造函数的参数 node_options（const NodeOptions& 类型）初始化 Node 类的成员变量 node_options_

map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)，用三个参数（node_options_、std::move(map_builder)、tf_buffer）初始化 Node 类的成员变量 map_builder_bridge_（推测是 MapBuilderBridge 类的对象）。

---

然后看一下map_builder_->AddTrajectoryBuilder()

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

(3)： 如果使用2d轨迹

①首先创建一个 LocalTrajectoryBuilder2D(前端) 类型智能指针，其主要为2D前端的初始化。

②尝试通过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_之中。

(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 的实例对象进行控制的。

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

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