讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文
末
正
下
方
中
心
提
供
了
本
人
联
系
方
式
，
点
击
本
人
照
片
即
可
显
示
W
X
→
官
方
认
证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式，点击本人照片即可显示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
注意，其上的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 ) ： \color{blue} (1)： (1)： 获得轨迹 id，因为每条轨迹都会创建一个 CollatedTrajectoryBuilder 对象存储于trajectory_builders_之中，其size()就可以用作为 trajectory_id。另外，其没有是个设置 pose_graph_odometry_motion_filter 相关参数，所以 MotionFilter() 函数未执行。
(
2
)
：
\color{blue} (2)：
(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
(
3
)
：
\color{blue} (3)：
(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
( 4 ) ： \color{blue} (4)： (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 ) ： \color{blue} (5)： (5)： 如果给了初始位姿，通过 pose_graph_->SetInitialTrajectoryPose 在位姿图中设置初始位姿。
(
6
)
：
\color{blue} (6)：
(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 有了一定了解，但是其涉及的东西太多了，所以这里仅仅是讲解个大概，后续会对每一个函数进行具体的分析。