源码阅读：使用Cartogrpaher进行毫米波雷达栅格SLAM（四）MapBuilder

通过MapBuilderBridge和SensorBridge这两个类的过渡，我们正式从cartographer_ros转到了cartographer部分了。从之前部分我们可知，MapBuilderBridge和SensorBridge主要调用了MapBuilderInterface和TrajectoryBuilderInterface这两个类的成员函数来处理。所以，在cartographer部分，我们将从这两个部分入手。

其中，调用MapBuilderInterface是在 map_builder_bridge.cc的 MapBuilderBridge类的成员变量map_builder，其为cartographer::mapping::MapBuilderInterface的一个实例

而调用TrajectoryBuilderInterface则是在SensorBridge.cc的成员变量trajectory_builder_，其为::cartographer::mapping::TrajectoryBuilderInterface的一个实例

从MapBuilder开始:

1. 接口MapBuilderInterface的定义：

MapBuilderInterface定义在/mapping/map_builder_interface.h中，是一个接口类，其中定义了一系列纯虚函数

创建，并返回id

Serialization是序列化，Deserialization是反序列化。这个函数的作用是从一个序列化的数据中构造出一个trajectory并返回他的index

根据id返回InterFace的指针

MapBuilderInterface这个抽象接口由MapBuilder继承并实现了其方法，MapBuilder定义在/mapping/map_builder.h中。从MapBuilder的注释我们可以看出，MapBuilder是cartographer算法的最顶层设计，MapBuilder包括了两个部分，其中TrajectoryBuilder用于Local Submap的建立与维护；PoseGraph部分用于Loop Closure.

所以，我们在解读完MapBuilder这个类之后，就会先后跳到TrajectoryBuilder和PoseGraph来这两个类。而cartographer算法中的Local Slam和Global Slam也应该是分别在这两个类当中实现的。

接下来，让我们首先来看看MapBuilder对MapBuilderInterface的具体实现：

2. MapBuilder的定义与实现：

MapBuilder是对MapBuilderInterface的继承和实现，MapBuilder中的方法都已经在MapBuilderInterface中定义，所以这里我们只看一下他的私有成员变量：

MapBuilder维护了一个PoseGraph的智能指针，该指针用来做Loop Closure。此外，MapBuilder还维护了一个TrajectoryBuilder的向量列表，每一个TrajectoryBuilder对应了机器人运行了一圈。这个向量列表就管理了整个图中的所有submap。对于其他的一些配置文件，我们暂时不关心。

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

接下来，让我们看一下MapBuilder的构造函数和其他一些方法的实现：

2.1 构造函数

构造函数基本上要做的就是给MapBuilder的配置项赋值的一些操作，比如，根据传入的参数option中的配置要设置是2d建图还是3d建图，2d和3d建图要分别设置不同的PoseGraph。同时，应该能够猜到PoseGraph这个接口应该有种不同的实现，分别是PoseGraph2D和PoseGraph3D

作者是先定义了一个PoseGraphInterface (/mapping/pose_graph_interface.h中)，然后又定义了PoseGraph (/mapping/pose_graph.h)来继承PoseGraphInterface, 但是PoseGraph里依然定义了很多虚函数，分别针对2D和3D的情况，由PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)和PoseGraph3D (/mapping/internal/3d/pose_graph_3d.h)来继承并实现。

特别注意的就是对于sensor_collator的设置。sensor_collator_是一个接口sensor::CollatorInterface的智能指针。可以看到，根据options.collate_by_trajectory()的不同，sensor::CollatorInterface也有两种不同的实现方式，分别是sensor::TrajectoryCollator和sensor::Collator。之前我们尝试从TrajectoryBuilder入手的时候感觉特别混乱就是没有搞清楚不同接口他们之间的实现关系。sensor::CollatorInterface定义在/sensor/collator_interface.h中；sensor::TrajectoryCollator定义在/sensor/internal/collator.h中；sensor::Collator定义在/sensor/internal/collator.h中。

其他几个函数的实现：

2.2 MapBuilder::AddTrajectoryBuilder

MapBuilder::AddTrajectoryBuilder这个函数可以说是cartographer中最重要的一个函数

这里再进行一下简单总结：

》首先，用于Local Slam的TrajectoryBuilderInterface相关的各个类、接口等的相互继承关系的梳理：

可以看到，根据是2d建图还是3d建图分为了两种情况。针对两种不同的情况，首先建立一个LocalTrajectoryBuilder2D或LocalTrajectoryBuilder3D的变量local_trajectory_builder；这个类是不带Loop Closure的Local Slam, 包含了Pose Extrapolator, Scan Matching等；

* 但注意，这两个类并没有继承TrajectoryBuilder,并不是一个TrajectoryBuilder的实现，而只是一个工具类

真正地创建一个TrajectoryBuilder是在后面，trajectory_builders_的push_back函数里面。其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder;而前面生成的local_trajectory_builder 则用于CreateGlobalTrajectoryBuilder2D函数的第一个参数，用于生成一个CollatedTrajectoryBuilder的智能指针。 CreateGlobalTrajectoryBuilder2D函数定义在/mapping/internal/global_trajectory_builder.h中。

一个MapBuilder的类对应了一次建图过程，在整个建图过程中，用于全局优化的PoseGraph的对象只有一个，即pose_graph_，而这个变量是在构造函数中就生成了。在AddTrajectorybuilder函数中只需要检查一下pose_graph_是否符合PoseGraph2D或PoseGraph3D的情况。而一个trajectory对应了机器人运行一圈。在图建好后机器人可能多次运行。每一次运行都是新增一条trajectory，因此，需要动态地维护一个trajectory的列表。每生成一个trajectory时都是调用AddTrajectoryBuilder来创建的。

》其次，对于用于Global Slam的PoseGraph之间的关系梳理：

对于PoseGraph这块儿我还不知道为啥这么设计，但是作者是先定义了一个PoseGraphInterface (/mapping/pose_graph_interface.h中)，然后又定义了PoseGraph (/mapping/pose_graph.h)来继承PoseGraphInterface, 但是PoseGraph里依然定义了很多虚函数，分别针对2D和3D的情况，由PoseGraph2D (/mapping/internal/2d/pose_graph_2d.h)和PoseGraph3D (/mapping/internal/3d/pose_graph_3d.h)来继承并实现。

》第三，比较有用的关于Landmark的信息

初看，这里似乎并没有包含跟Landmark相关的信息。但是给算法中添加Landmark提供了便利。那就是其中trajectory_options.has_initial_trajectory_pose()参数的设置，所以后面我们需要关注一下这些参数如何设置。

该参数对应的含义是说如果要添加的轨迹有初始pose该如何处理——即，该轨迹及其建立起来的子图在全局中的变换矩阵是有初始值的。这对应的情况就是比如说，我们检测到了一个Landmark。那么这时，我们可以新增加一条trajectory，增加新的trajectory时设置has.initial_trajectory_pose为真，然后根据机器人与Landmark之间的相对位姿推算机器人相对于世界坐标系的相对位姿。以该位姿作为新增加的trajectory的初始位姿。这样情况下，在检测到Landmark时就能有效降低累积误差。

》最后，是关于另一个参数pure_localization()

前面的一篇文章中有朋友留言，说“如果用cartographer定位，如何修改”。当时我还不知道该怎么修改，看到这里我觉得可能cartographer还是保留了纯定位的功能的。

pure_localization()的配置文件在/src/cartographer/configuration_files/trajectory_builder.lua中定义，可以看到，默认情况下，该值为false。

2.3 AddTrajectoryForDeserialization、FinishTrajectory、SubmapToProto和SerializeState

2.4 LoadState

最后一个函数比较长，但是不是特别重要，主要功能就是从proto流中构造出当前的状态

接下来，我们需要分成两个支线去看代码，一个是Local SLAM相关的TrajectoryBuilder，另一个是用于Loop Closure的PoseGraph部分。

下一篇文章我们首先从用于Local SLAM的TrajectoryBuilder入手。