cartographer源码分析（5）map_builder_interface.h、map_builder.h以及map_builder.cc
通过对cartographer_ros的分析， 我们已经了解到在系统运行之初就构建了一个map_builder对象用于建图。 分析MapBuilderBridge和SensorBridge对象的时候，我们看到cartographer_ros只是用来将ROS系统中各种各样的消息转换之后，传递给Cartographer进行处理，也就是这里所说的map_builder对象。
MapBuilderBridge和SensorBridge主要调用了MapBuilderInterface和TrajectoryBuilderInterface这两个类的成员函数来处理。这里我们先看一下MapBuilderInterface。
1、map_builder_interface.h
MapBuilderInterface定义在/mapping/map_builder_interface.h中，是一个接口类，其中定义了一系列纯虚函数：
#ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_
#define CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_
#include ,
* common::Time,//时间*/,
* transform::Rigid3d /* local pose estimate */,
* sensor::RangeData /* in local frame */,
* std::unique_ptr<const InsertionResult>)>;*/,
*/
// Creates a new trajectory and returns its index. Querying the trajectory
// builder for it will return 'nullptr'.
// Serialization是序列化，Deserialization是反序列化。
// 所以这个函数的作用是从一个序列化的数据中构造出一个trajectory并返回他的index
//AddTrajectoryForDeserialization同样也是一个用于新建轨迹跟踪器的接口，只是它的参数只有一个。这个参数处理记录了轨迹跟踪器的配置还包含传感器的配置。
//这个参数的数据类型，和接口AddTrajectoryBuilder的参数trajectory_options的数据类型，都是通过protobuf根据proto文件生成的
virtual int AddTrajectoryForDeserialization(//只有一个参数，就是序列化的数据
const proto::TrajectoryBuilderOptionsWithSensorIds&
options_with_sensor_ids_proto) = 0;
// Returns the 'TrajectoryBuilderInterface' corresponding to the specified
// 'trajectory_id' or 'nullptr' if the trajectory has no corresponding
// builder.
// 根据trajectory_id返回一个TrajectoryBuilderInterface的指针。
// 如果该trajectory没有一个TrajectoryBuilder，则返回nullptr' corresponding to the specified
virtual mapping::TrajectoryBuilderInterface* GetTrajectoryBuilder(
int trajectory_id) const = 0;
// Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished,
// i.e. no further sensor data is expected.
// 接口FinishTrajectory用于关闭trajectory_id对应的轨迹跟踪器，该跟踪器将不再响应新的传感器数据。
virtual void FinishTrajectory(int trajectory_id) = 0;
// Fills the SubmapQuery::Response corresponding to 'submap_id'. Returns an
// error string on failure, or an empty string on success.
// 根据指定的submap_id来查询submap，把结果放到SubmapQuery::Response中。
// 如果出现错误，返回error string; 成功则返回empty string.
virtual std::string SubmapToProto(const SubmapId& submap_id,
proto::SubmapQuery::Response* response) = 0;
// Serializes the current state to a proto stream. If
// 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
// submaps that have not yet received all rangefinder data insertions, will
// be included in the serialized state.
// 将当前的系统状态转换成一个proto的流，完成序列化。
virtual void SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* writer) = 0;
// Serializes the current state to a proto stream file on the host system. If
// 'include_unfinished_submaps' is set to true, unfinished submaps, i.e.
// submaps that have not yet received all rangefinder data insertions, will
// be included in the serialized state.
// Returns true if the file was successfully written.
//将当前状态序列化为主机系统上的proto文件。
virtual bool SerializeStateToFile(bool include_unfinished_submaps,
const std::string& filename) = 0;
// Loads the SLAM state from a proto stream. Returns the remapping of new
// trajectory_ids.
// 从一个proto流中加载SLAM状态
virtual std::map<int /* trajectory id in proto */, int /* trajectory id */>
LoadState(io::ProtoStreamReaderInterface* reader, bool load_frozen_state) = 0;
// Loads the SLAM state from a pbstream file. Returns the remapping of new
// trajectory_ids.
//从pbstream文件加载SLAM状态。返回新轨迹标识的重新映射。LoadState可以说是SerializeState的逆操作，用于从proto流中加载SLAM状态。
virtual std::map<int /* trajectory id in proto */, int /* trajectory id */>
LoadStateFromFile(const std::string& filename, bool load_frozen_state) = 0;
// 返回系统中当前已有的trajectory_builder的数量
virtual int num_trajectory_builders() const = 0;
// 返回一个PoseGraphInterface的接口指针。后面我们可以看到，PoseGrapher用来进行Loop Closure。
virtual mapping::PoseGraphInterface* pose_graph() = 0;
// 获取所有TrajectoryBuilder的配置项。
virtual const std::vector<proto::TrajectoryBuilderOptionsWithSensorIds>&
GetAllTrajectoryBuilderOptions() const = 0;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_INTERFACE_H_
2、map_builder.h
MapBuilderInterface这个抽象接口由MapBuilder继承并实现了其方法，MapBuilder定义在/mapping/map_builder.h中。从MapBuilder的注释我们可以看出，MapBuilder是cartographer算法的最顶层设计，MapBuilder包括了两个部分，其中TrajectoryBuilder用于Local Submap的建立与维护；PoseGraph部分用于Loop Closure.
MapBuilder是对MapBuilderInterface的继承和实现，MapBuilder中的方法都已经在MapBuilderInterface中定义
MapBuilder维护了一个PoseGraph的智能指针，该指针用来做Loop Closure。此外，MapBuilder还维护了一个TrajectoryBuilder的向量列表，每一个TrajectoryBuilder对应了机器人运行了一圈。这个向量列表就管理了整个图中的所有submap。
trajectory是机器人跑一圈时的轨迹，在这其中需要记录和维护传感器的数据。根据这个trajectory上传感器收集的数据，我们可以逐步构建出栅格化的地图Submap，但这个submap会随着时间或trajectory的增长而产生误差累积，但trajectory增长到超过一个阈值，则会新增一个submap。而PoseGraph是用来进行全局优化，将所有的Submap紧紧tie在一起，构成一个全局的Map，消除误差累积。
MapBuilder中的其他方法都是对pose_graph_和向量列表trajectory_builders_的管理和维护。
#ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
#define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_
#include 3、map_builder.cc
MapBuilder::AddTrajectoryBuilder这个函数可以说是cartographer中最重要的一个函数：
3.1、
首先，用于Local Slam的TrajectoryBuilderInterface相关的各个类、接口等的相互继承关系的梳理：
可以看到，根据是2d建图还是3d建图分为了两种情况。 针对两种不同的情况，首先建立一个LocalTrajectoryBuilder2D或LocalTrajectoryBuilder3D的变量local_trajectory_builder；它几乎完成了一个局部SLAM的所有功能，包括位姿估计、扫描匹配等，就是没有闭环检测。
但注意，这两个类并没有继承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来创建的。
3.2、
其次，对于用于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)来继承并实现。
3.3、
第三，比较有用的关于Landmark的信息
初看，这里似乎并没有包含跟Landmark相关的信息。但是给算法中添加Landmark提供了便利。那就是其中trajectory_options.has_initial_trajectory_pose()参数的设置，所以后面我们需要关注一下这些参数如何设置。
该参数对应的含义是说如果要添加的轨迹有初始pose该如何处理——即，该轨迹及其建立起来的子图在全局中的变换矩阵是有初始值的。 这对应的情况就是比如说，我们检测到了一个Landmark。那么这时，我们可以新增加一条trajectory，增加新的trajectory时设置has.initial_trajectory_pose为真，然后根据机器人与Landmark之间的相对位姿推算机器人相对于世界坐标系的相对位姿。 以该位姿作为新增加的trajectory的初始位姿。这样情况下，在检测到Landmark时就能有效降低累积误差。
3.4、最后，是关于另一个参数pure_localization()
可能cartographer还是保留了纯定位的功能的。
pure_localization()的配置文件在/src/cartographer/configuration_files/trajectory_builder.lua中定义，可以看到，默认情况下，该值为false。
#include "cartographer/mapping/map_builder.h"
#include "absl/memory/memory.h"
#include "cartographer/common/time.h"
#include "cartographer/io/internal/mapping_state_serialization.h"
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/io/serialization_format_migration.h"
#include "cartographer/mapping/internal/2d/local_trajectory_builder_2d.h"
#include "cartographer/mapping/internal/2d/pose_graph_2d.h"
#include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h"
#include "cartographer/mapping/internal/3d/pose_graph_3d.h"
#include "cartographer/mapping/internal/collated_trajectory_builder.h"
#include "cartographer/mapping/internal/global_trajectory_builder.h"
#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h"
#include "cartographer/sensor/internal/collator.h"
#include "cartographer/sensor/internal/trajectory_collator.h"
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping {
namespace {
using mapping::proto::SerializedData;
std::vector<std::string> SelectRangeSensorIds(
const std::set<MapBuilder::SensorId>& expected_sensor_ids) {
std::vector<std::string> range_sensor_ids;
for (const MapBuilder::SensorId& sensor_id : expected_sensor_ids) {
if (sensor_id.type == MapBuilder::SensorId::SensorType::RANGE) {
range_sensor_ids.push_back(sensor_id.id);
}
}
return range_sensor_ids;
}
void MaybeAddPureLocalizationTrimmer(
const int trajectory_id,
const proto::TrajectoryBuilderOptions& trajectory_options,
PoseGraph* pose_graph) {
if (trajectory_options.pure_localization()) {
LOG(WARNING)
<< "'TrajectoryBuilderOptions::pure_localization' field is deprecated. "
"Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id, 3 /* max_submaps_to_keep */));
return;
}
if (trajectory_options.has_pure_localization_trimmer()) {
pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
trajectory_id,
trajectory_options.pure_localization_trimmer().max_submaps_to_keep()));
}
}
} // namespace
proto::MapBuilderOptions CreateMapBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::MapBuilderOptions options;
options.set_use_trajectory_builder_2d(
parameter_dictionary->GetBool("use_trajectory_builder_2d"));
options.set_use_trajectory_builder_3d(
parameter_dictionary->GetBool("use_trajectory_builder_3d"));
options.set_num_background_threads(
parameter_dictionary->GetNonNegativeInt("num_background_threads"));
options.set_collate_by_trajectory(
parameter_dictionary->GetBool("collate_by_trajectory"));
*options.mutable_pose_graph_options() = CreatePoseGraphOptions(
parameter_dictionary->GetDictionary("pose_graph").get());
CHECK_NE(options.use_trajectory_builder_2d(),
options.use_trajectory_builder_3d());
return options;
}
//构造函数基本上要做的就是给MapBuilder的配置项赋值的一些操作，比如，根据传入的参数option中的配置要设置是2d建图还是3d建图，2d和3d建图要分别设置不同的PoseGraph。
//同时，应该能够猜到PoseGraph这个接口应该有种不同的实现，分别是PoseGraph2D和PoseGraph3D. 对于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)来继承并实现。
//另外需要提醒读者特别注意的就是对于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中。
//下面是对象map_builder的构造函数的代码片段。在构造map_builder对象的时候，用其成员变量options_保存了配置选项，同时根据配置中关于线程数量的配置完成了线程池thread_pool_的构造。
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) {
//接着检查配置项，确保在2D建图和3D建图之间二选一。
CHECK(options.use_trajectory_builder_2d() ^
options.use_trajectory_builder_3d());
//如果是2D建图，那么我们就构建一个PoseGraph2D的对象记录在pose_graph_下。
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建图，就构建一个PoseGraph3D的对象。
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_);
}
//最后根据配置项collate_by_trajectory来构建修正器。
if (options.collate_by_trajectory()) {
sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
} else {
sensor_collator_ = absl::make_unique<sensor::Collator>();
}
}
//可以看到在构造函数中，我们完成了对options_、thread_pool_、pose_graph_、sensor_collator_的构建工作，而用于建立子图的轨迹跟踪器的对象则需要通过调用接口AddTrajectoryBuilder来完成构建。
//创建一个新的TrajectoryBuilder并返回它的trajectory_id.
//函数AddTrajectoryBuilder的代码片段。它有三个参数，其中expected_sensor_ids中记录了用于建图的所有传感器名称和类型，trajectory_options是新建的轨迹跟踪器的配置，
//最后的local_slam_result_callback则是一个回调函数对象，用于响应局部地图构建完成的事件。
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
//生成trajectory_id，trajectory_builders_是一个向量，存着所有的trajectory。因此，当有一个新的trajectory时，以向量的size值作为新的trajectory，加入到trajectory_builders_中
//在函数的一开始，先通过容器trajectory_builders_获取轨迹跟踪器的数量。如果构建成功，该值将作为新的跟踪器的索引。
const int trajectory_id = trajectory_builders_.size();
/*
* 可以看到，根据是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中。
*/
if (options_.use_trajectory_builder_3d()) {//根据配置参数选择是3d情况还是2d情况
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder;//LocalTrajectoryBuilder3D定义在/mapping/internal/3d/local_trajectory_builder_3d.h中
if (trajectory_options.has_trajectory_builder_3d_options()) {//是否有跟该trajectory相关的参数，如果有就设置一下
//根据参数实例化这个LocalTrajectoryBuilder3D的变量
local_trajectory_builder = absl::make_unique<LocalTrajectoryBuilder3D>(
trajectory_options.trajectory_builder_3d_options(),
SelectRangeSensorIds(expected_sensor_ids));//生成一个local_trajectory_builder
}
// 检查类型转化
DCHECK(dynamic_cast<PoseGraph3D*>(pose_graph_.get()));//dynamic_cast是强制类型转化，把PoseGraphInterface的指针转化为PoseGraph3D
//将生成的CollatedTrajectoryBuilder压入向量列表中
//接下来的语句所构建的CollatedTrajectoryBuilder类型的对象才是所谓的轨迹跟踪器， 它继承自接口类TrajectoryBuilderInterface， 这个接口适用于2D和3D建图的对象，
//如此上层的代码就不必关心具体的建图内核，使用同一个容器trajectory_builders_就可以保存两种local SLAM的对象。构建好的对象就直接被塞进了容器中。
//此外CollatedTrajectoryBuilder类型还使用sensor::CollatorInterface接口来收集传感器数据，所以我们可以在它的构造参数列表中看到对象sensor_collator_。
//在构建轨迹跟踪器的时候， 还通过函数CreateGlobalTrajectoryBuilder2D构建了一个GlobalTrajectoryBuilder类型的对象， 从它的参数列表中来看，
//除了含有刚刚构建的local_trajectory_builder对象之外，还引入了位姿图对象pose_graph_。个人猜测这是一个具有闭环功能的SLAM对象，所以称之为Global的。
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
//真正地创建一个TrajectoryBuilder，其中CollatedTrajectoryBuilder继承了接口TrajectoryBuilder
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()),
local_slam_result_callback)));
} else {//如果是2d的情况，跟3d情况基本相同，不再赘述
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()));
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的情况需要多处理的情况。
//猜测是说如果两个submap有重叠的部分，则调用pose_graph_中的方法来进行全局优化
MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options,
pose_graph_.get());
// 如果该轨迹有初始pose；开始一条轨迹前我们是否已知初始位姿。
// 这对应的情况就是比如说，我们检测到了一个Landmark。那么这时，我们可以新增加一条trajectory，
// 增加新的trajectory时设置has.initial_trajectory_pose为真，
// 然后根据机器人与Landmark之间的相对位姿推算机器人相对于世界坐标系的相对位姿。
// 以该位姿作为新增加的trajectory的初始位姿。这样情况下，在检测到Landmark时就能有效降低累积误差。
if (trajectory_options.has_initial_trajectory_pose()) {
const auto& initial_trajectory_pose =
trajectory_options.initial_trajectory_pose();//调用pose_graph_中的方法，设置初始pose。跟全局相关的事情，都交给pose_graph_来处理。
pose_graph_->SetInitialTrajectoryPose(//设置初始pose
trajectory_id, initial_trajectory_pose.to_trajectory_id(),
transform::ToRigid3(initial_trajectory_pose.relative_pose()),
common::FromUniversal(initial_trajectory_pose.timestamp()));
}
//将一些跟传感器相关的配置项转成proto流，然后统一放到all_trajectory_builder_options_这个向量列表中
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;
}
//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还是保留了纯定位的功能的。pure_localization()的配置文件
//在/src/cartographer/configuration_files/trajectory_builder.lua中定义，可以看到，默认情况下，该值为false。
//从序列化的数据中构造一条trajectory
int MapBuilder::AddTrajectoryForDeserialization(
const proto::TrajectoryBuilderOptionsWithSensorIds&
options_with_sensor_ids_proto) {
const int trajectory_id = trajectory_builders_.size();
trajectory_builders_.emplace_back();//emplace_back和push_back都是向容器内添加数据.
//对于在容器中添加类的对象时, 相比于push_back,emplace_back可以避免额外类的复制和移动操作.
//但是，这里emplace_back里怎么没有参数呢？那把什么加进去了呢
//配置项的添加
all_trajectory_builder_options_.push_back(options_with_sensor_ids_proto);
CHECK_EQ(trajectory_builders_.size(), all_trajectory_builder_options_.size());//检查两者大小是否一致
return trajectory_id;
}
//结束一条轨迹；可以看到，分别调用sensor_collator和pose_graph_的成员函数来Finish一条轨迹
void MapBuilder::FinishTrajectory(const int trajectory_id) {
sensor_collator_->FinishTrajectory(trajectory_id);//现阶段猜测，sensor_collator_->FinishTrajectory应该是做的清楚对传感器的占用等操作
pose_graph_->FinishTrajectory(trajectory_id);//现阶段猜测，pose_graph_->FinishTrajectory应该是完成对刚刚的trajectory的全局优化
}
// 根据指定的submap_id来查询submap，把结果放到SubmapQuery::Response中。
// 如果出现错误，返回error string; 成功则返回empty string.
std::string MapBuilder::SubmapToProto(
const SubmapId& submap_id, proto::SubmapQuery::Response* const response) {
if (submap_id.trajectory_id < 0 ||
submap_id.trajectory_id >= num_trajectory_builders()) {
return "Requested submap from trajectory " +
std::to_string(submap_id.trajectory_id) + " but there are only " +
std::to_string(num_trajectory_builders()) + " trajectories.";
}//指定的submap的id必须合法
//pose_graph_中应该是维护着一张submap的列表。通过pose_graph_获取指定id的子图
const auto submap_data = pose_graph_->GetSubmapData(submap_id);
if (submap_data.submap == nullptr) {
return "Requested submap " + std::to_string(submap_id.submap_index) +
" from trajectory " + std::to_string(submap_id.trajectory_id) +
" but it does not exist: maybe it has been trimmed.";
}//一些子图可能在优化过程中被裁掉。
//正常情况下返回数据
submap_data.submap->ToResponseProto(submap_data.pose, response);
return "";
}
//调用io::WritePbStream工具，保存所有数据
void MapBuilder::SerializeState(bool include_unfinished_submaps,
io::ProtoStreamWriterInterface* const writer) {
io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, writer,
include_unfinished_submaps);
}
// 序列化当前状态到一个proto流中。
bool MapBuilder::SerializeStateToFile(bool include_unfinished_submaps,
const std::string& filename) {
io::ProtoStreamWriter writer(filename);
io::WritePbStream(*pose_graph_, all_trajectory_builder_options_, &writer,
include_unfinished_submaps);
return (writer.Close());
}
//主要功能就是从proto流中构造出当前的状态。
std::map<int, int> MapBuilder::LoadState(// 从一个proto流中加载SLAM状态
io::ProtoStreamReaderInterface* const reader, bool load_frozen_state) {
io::ProtoStreamDeserializer deserializer(reader);//反序列化读取工具
// Create a copy of the pose_graph_proto, such that we can re-write the
// trajectory ids.
proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
const auto& all_builder_options_proto =
deserializer.all_trajectory_builder_options();
std::map<int, int> trajectory_remapping;//逐条trajectory恢复
for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) {
auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i);
const auto& options_with_sensor_ids_proto =
all_builder_options_proto.options_with_sensor_ids(i);
const int new_trajectory_id =
AddTrajectoryForDeserialization(options_with_sensor_ids_proto);
CHECK(trajectory_remapping
.emplace(trajectory_proto.trajectory_id(), new_trajectory_id)
.second)
<< "Duplicate trajectory ID: " << trajectory_proto.trajectory_id();
trajectory_proto.set_trajectory_id(new_trajectory_id);
if (load_frozen_state) {
pose_graph_->FreezeTrajectory(new_trajectory_id);
}
}
//恢复trajectory上的节点间的约束关系
// Apply the calculated remapping to constraints in the pose graph proto.
for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) {
constraint_proto.mutable_submap_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.submap_id().trajectory_id()));
constraint_proto.mutable_node_id()->set_trajectory_id(
trajectory_remapping.at(constraint_proto.node_id().trajectory_id()));
}
//恢复Submap
MapById<SubmapId, transform::Rigid3d> submap_poses;
for (const proto::Trajectory& trajectory_proto :
pose_graph_proto.trajectory()) {
for (const proto::Trajectory::Submap& submap_proto :
trajectory_proto.submap()) {
submap_poses.Insert(SubmapId{trajectory_proto.trajectory_id(),
submap_proto.submap_index()},
transform::ToRigid3(submap_proto.pose()));
}
}
//恢复节点的pose
MapById<NodeId, transform::Rigid3d> node_poses;
for (const proto::Trajectory& trajectory_proto :
pose_graph_proto.trajectory()) {
for (const proto::Trajectory::Node& node_proto : trajectory_proto.node()) {
node_poses.Insert(
NodeId{trajectory_proto.trajectory_id(), node_proto.node_index()},
transform::ToRigid3(node_proto.pose()));
}
}
// 设置Landmark
// Set global poses of landmarks.
for (const auto& landmark : pose_graph_proto.landmark_poses()) {
pose_graph_->SetLandmarkPose(landmark.landmark_id(),
transform::ToRigid3(landmark.global_pose()),
true);
}
MapById<SubmapId, mapping::proto::Submap> submap_id_to_submap;
MapById<NodeId, mapping::proto::Node> node_id_to_node;
//不停读取，直到读完
SerializedData proto;
while (deserializer.ReadNextSerializedData(&proto)) {
switch (proto.data_case()) {
case SerializedData::kPoseGraph:
LOG(ERROR) << "Found multiple serialized `PoseGraph`. Serialized "
"stream likely corrupt!.";
break;
case SerializedData::kAllTrajectoryBuilderOptions:
LOG(ERROR) << "Found multiple serialized "
"`AllTrajectoryBuilderOptions`. Serialized stream likely "
"corrupt!.";
break;
case SerializedData::kSubmap: {
proto.mutable_submap()->mutable_submap_id()->set_trajectory_id(
trajectory_remapping.at(
proto.submap().submap_id().trajectory_id()));
submap_id_to_submap.Insert(
SubmapId{proto.submap().submap_id().trajectory_id(),
proto.submap().submap_id().submap_index()},
proto.submap());
break;
}
case SerializedData::kNode: {
proto.mutable_node()->mutable_node_id()->set_trajectory_id(
trajectory_remapping.at(proto.node().node_id().trajectory_id()));
const NodeId node_id(proto.node().node_id().trajectory_id(),
proto.node().node_id().node_index());
const transform::Rigid3d& node_pose = node_poses.at(node_id);
pose_graph_->AddNodeFromProto(node_pose, proto.node());
node_id_to_node.Insert(node_id, proto.node());
break;
}
case SerializedData::kTrajectoryData: {
proto.mutable_trajectory_data()->set_trajectory_id(
trajectory_remapping.at(proto.trajectory_data().trajectory_id()));
pose_graph_->SetTrajectoryDataFromProto(proto.trajectory_data());
break;
}
case SerializedData::kImuData: {
if (load_frozen_state) break;
pose_graph_->AddImuData(
trajectory_remapping.at(proto.imu_data().trajectory_id()),
sensor::FromProto(proto.imu_data().imu_data()));
break;
}
case SerializedData::kOdometryData: {
if (load_frozen_state) break;
pose_graph_->AddOdometryData(
trajectory_remapping.at(proto.odometry_data().trajectory_id()),
sensor::FromProto(proto.odometry_data().odometry_data()));
break;
}
case SerializedData::kFixedFramePoseData: {
if (load_frozen_state) break;
pose_graph_->AddFixedFramePoseData(
trajectory_remapping.at(
proto.fixed_frame_pose_data().trajectory_id()),
sensor::FromProto(
proto.fixed_frame_pose_data().fixed_frame_pose_data()));
break;
}
case SerializedData::kLandmarkData: {
if (load_frozen_state) break;
pose_graph_->AddLandmarkData(
trajectory_remapping.at(proto.landmark_data().trajectory_id()),
sensor::FromProto(proto.landmark_data().landmark_data()));
break;
}
default:
LOG(WARNING) << "Skipping unknown message type in stream: "
<< proto.GetTypeName();
}
}
// TODO(schwoere): Remove backwards compatibility once the pbstream format
// version 2 is established.
if (deserializer.header().format_version() ==
io::kFormatVersionWithoutSubmapHistograms) {
submap_id_to_submap =
cartographer::io::MigrateSubmapFormatVersion1ToVersion2(
submap_id_to_submap, node_id_to_node, pose_graph_proto);
}
for (const auto& submap_id_submap : submap_id_to_submap) {
pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id),
submap_id_submap.data);
}
if (load_frozen_state) {
// Add information about which nodes belong to which submap.
// Required for 3D pure localization.
for (const proto::PoseGraph::Constraint& constraint_proto :
pose_graph_proto.constraint()) {
if (constraint_proto.tag() !=
proto::PoseGraph::Constraint::INTRA_SUBMAP) {
continue;
}
pose_graph_->AddNodeToSubmap(
NodeId{constraint_proto.node_id().trajectory_id(),
constraint_proto.node_id().node_index()},
SubmapId{constraint_proto.submap_id().trajectory_id(),
constraint_proto.submap_id().submap_index()});
}
} else {
// When loading unfrozen trajectories, 'AddSerializedConstraints' will
// take care of adding information about which nodes belong to which
// submap.
pose_graph_->AddSerializedConstraints(
FromProto(pose_graph_proto.constraint()));
}
CHECK(reader->eof());
return trajectory_remapping;
}
std::map<int, int> MapBuilder::LoadStateFromFile(
const std::string& state_filename, const bool load_frozen_state) {
const std::string suffix = ".pbstream";
if (state_filename.substr(
std::max<int>(state_filename.size() - suffix.size(), 0)) != suffix) {
LOG(WARNING) << "The file containing the state should be a "
".pbstream file.";
}
LOG(INFO) << "Loading saved state '" << state_filename << "'...";
io::ProtoStreamReader stream(state_filename);
return LoadState(&stream, load_frozen_state);
}
} // namespace mapping
} // namespace cartographer
主要参考：
https://zhuanlan.zhihu.com/p/48575070
以及
https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=%E5%88%9D%E8%AF%86map_builder