(02)Cartographer源码无死角解析-(22) 传感器数据分发→CollatedTrajectoryBuilder
讲解关于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→官方认证
一、前言
首先对前面的知识做一个回顾，从 node_main.cc 文件中开始;
//根据配置文件，命令行参数与话题重映射，订阅默认话题开始一条轨迹
node.StartTrajectoryWithDefaultTopics(trajectory_options);
AddTrajectory(options);//添加一条新轨迹
// 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
const int trajectory_id =map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
其上的 map_builder_bridge_.AddTrajectory 与 LaunchSubscribers() 是十分重要的两个函数：
( 1 ) ： \color{blue}(1)： (1)： map_builder_bridge_.AddTrajectory 函数主要的核心就是构建CollatedTrajectoryBuilder对象存储于 node::map_builder_bridge_::map_builder_::trajectory_builders_变量之中，然后返回一个 trajectory_id，再根据 trajectory_id 构建一个 SensorBridge对象，创建该对象时代码如下：
// 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
注 意 \color{red}注意 注意 其上的 GetTrajectoryBuilder(trajectory_id) 就是获取 trajectory_builders_ 中的 CollatedTrajectoryBuilder 对象。然后作为参数传送给 SensorBridge 的构造函数。
( 2 ) ： \color{blue}(2)： (2)： LaunchSubscribers() 会根据 trajectory_id 与其对应的配置 TrajectoryOptions& options，进行话题的订阅，同时出注册回调函数。
这里就不在贴代码了，Node::LaunchSubscribers() 主要订阅，注册了如下回调函数：
//位于 src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 文件之中
void Node::LaunchSubscribers(const TrajectoryOptions& options,const int trajectory_id)
&Node::HandleLaserScanMessage//注册的单线雷达回调函数
SensorBridge::HandleLaserScanMessage()//根据采样频率评估是否调用该函数
&Node::HandleMultiEchoLaserScanMessage//注册的多回声雷达回调函数
SensorBridge::HandleMultiEchoLaserScanMessage()//根据采样频率评估是否调用该函数
&Node::HandlePointCloud2Message//注册的多线点云雷达回调函数
SensorBridge::HandlePointCloud2Message()//根据采样频率评估是否调用该函数
&Node::HandleImuMessage//注册的IMU回调函数
SensorBridge::HandleImuMessage()//根据采样频率评估是否调用该函数
......
还有一些回调函数，就不一一在这里列举了，所有的回调函数，根据查询的tf，对数据完成进行坐标系变换(变换到tracking_frame)之后，最终都会调用类似如下的一段代码：
或: trajectory_builder_->AddSensorData(sensor_id,carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
或: trajectory_builder_->AddSensorData(sensor_id,carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
或: trajectory_builder_->AddSensorData(sensor_id, carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>(Rigid3d::Translation(ecef_to_local_frame_.value() *LatLongAltToEcef(msg->latitude, msg->longitude, msg->altitude)))});
或: trajectory_builder_->AddSensorData(sensor_id, landmark_data);
......
上面只列举了一部分，从上面可以看出，trajectory_builder_->AddSensorData() 函数接收了各种各样的数据类型，那么其定然存在很多重载函数。其上的 trajectory_builder_ 就是 CollatedTrajectoryBuilder 的实例对象指针，每个 trajectory_id 都有一个与之对应的 CollatedTrajectoryBuilder 实例对象指针。
二、构造时的传参
根据上面的介绍，可以知道 trajectory_builder_ 就是类CollatedTrajectoryBuilder的实例指针，是在 src/cartographer/cartographer/mapping/map_builder.cc 文件的 MapBuilder::AddTrajectoryBuilder() 函数中实例化，通过上一篇博客了解到，其2D轨迹与3D轨迹的构建过程如下：
// CollatedTrajectoryBuilder初始化
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)));
// 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)));
三、C++多态
CollatedTrajectoryBuilder 是在 src/cartographer/cartographer/mapping/internal/collated_trajectory_builder.cc 文件中定义。这里涉及到一个多态的知识点，创建的实例类型为 CollatedTrajectoryBuilder*，但是在构建 SensorBridge 时， SensorBridge 构造函数需要的类型为 carto::mapping::TrajectoryBuilderInterface*，从 collated_trajectory_builder.h 文件中，可以看到：
class CollatedTrajectoryBuilder : public TrajectoryBuilderInterface
故 CollatedTrajectoryBuilder 是 TrajectoryBuilderInterface 的派生类。TrajectoryBuilderInterface 在 src/cartographer/cartographer/mapping/trajectory_builder_interface.h 文件中被声明。从类名，以及代码可以很明显的看出，其是一个接口类，定义了很多的纯虚函数。一个接口类可以派生出很多类型的子类，构建 SensorBridge 构造函数需要的参数为基类 TrajectoryBuilderInterface，这样有个好处，也就是由 TrajectoryBuilderInterface 派生出来子类，都可以用于 SensorBridge 的构造函数。
CollatedTrajectoryBuilder 的主要作用就是使用 sensor::CollatorInterface 整理传感器数据, 然后将其传递到2D和3D通用的 mapping::TrajectoryBuilderInterface。
另外再介绍一下 c++11中的std::function 与 using 的模板部分具体化
c++11: std::function 通用多态函数封装器
std::function 的实例能存储、复制及调用任何可调用 (Callable) 目标:
如函数、 lambda表达式、 bind表达式或其他函数对象, 还有指向成员函数指针和指向数据成员指针.
它也是对 C++ 中现有的可调用实体的一种类型安全的包裹（相对来说, 函数指针的调用不是类型安全的）
在 trajectory_builder_interface.h 中可以看到如下一段代码：
// A callback which is called after local SLAM processes an accumulated
// 'sensor::RangeData'. If the data was inserted into a submap, reports the
// assigned 'NodeId', otherwise 'nullptr' if the data was filtered out.
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>)>;
其上表示用 LocalSlamResultCallback 表示一个回调函数，该回调函数无返回值，需要传入五个参数。
四、CollatedTrajectoryBuilder.h
现在再来看类 CollatedTrajectoryBuilder，在 collated_trajectory_builder.h 文件中，可以看到 很多函数参数列表后买你都带了 override 关键字，这里的 override 表示重写，也就是说该函数就是重写父类函数，而不是其他自身构建的函数，如果该函数传入的参数或者返回值与父类不一致，则会报错。
那么现在就正式开始讲解，首先在该类中共存在五个 void AddSensorData() 重载函数， 如下所示：
// 处理雷达点云数据
void AddSensorData(const std::string& sensor_id,const sensor::TimedPointCloudData& timed_point_cloud_data)
override {AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));}
// 处理IMU数据
void AddSensorData(const std::string& sensor_id,const sensor::ImuData& imu_data)
override {AddData(sensor::MakeDispatchable(sensor_id, imu_data));}
// 处理里程计数据
void AddSensorData(const std::string& sensor_id,const sensor::OdometryData& odometry_data)
override {AddData(sensor::MakeDispatchable(sensor_id, odometry_data));}
// 根据参数决定gps数据是否需要排序
// AddData与wrapped_trajectory_builder_->AddSensorData只能选一种
// 因为AddData最终调用的就是wrapped_trajectory_builder_->AddSensorData
void AddSensorData(const std::string& sensor_id,const sensor::FixedFramePoseData& fixed_frame_pose_data)
override {if (collate_fixed_frame_) {
AddData(sensor::MakeDispatchable(sensor_id, fixed_frame_pose_data));
return;
}
wrapped_trajectory_builder_->AddSensorData(sensor_id,fixed_frame_pose_data);
}
// 根据参数决定Landmark数据是否需要排序
void AddSensorData(const std::string& sensor_id,const sensor::LandmarkData& landmark_data)
override {
if (collate_landmarks_) {
AddData(sensor::MakeDispatchable(sensor_id, landmark_data));
return;
}
wrapped_trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}
从上面的代码中，可以看到一个共同调用的函数 AddData(sensor::MakeDispatchable(xxx))。处理GPS和Landmark数据时，会对 collate_fixed_frame_ 与 collate_landmarks_ 参数进行判断，然后再决定是否调用 AddData() 函数。这两个参数都是再 src/cartographer/configuration_files/trajectory_builder.lua 中设置。另外其上还可以看到
wrapped_trajectory_builder_->AddSensorData(xxx)
该部分类容后面进行讲解。同时在 collated_trajectory_builder.h 文件中，还存在一个如下函数也是比较重要的的：
void AddData(std::unique_ptr<sensor::Data> data);
// 将local slam 的结果也作为一种传感器数据进行处理
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>local_slam_result_data)
override {AddData(std::move(local_slam_result_data));}
void HandleCollatedSensorData(const std::string& sensor_id,std::unique_ptr<sensor::Data> data);
后续会他们进行详细的分析。
五、CollatedTrajectoryBuilder.cc
在大致了解了头文件之后，在来看看 CollatedTrajectoryBuilder 的构造函数。首先要了解的就是该类实例化时，传入的参数。这里回到前面的讲解的二、构造传参，可以看到如下类似代码:
// CollatedTrajectoryBuilder初始化
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)));
可以看到构建 CollatedTrajectoryBuilder 对象时，需要传入五个参数。特别是第五个参数时比较复杂的，其世为 GlobalTrajectoryBuilder 类对象，注意该类继承于 TrajectoryBuilderInterface 接口。其构造函数的，及其初始化列表如下：
/**
* @brief Construct a new Collated Trajectory Builder:: Collated Trajectory Builder object
*
* @param[in] trajectory_options 轨迹的参数配置
* @param[in] sensor_collator 传入的整理传感器的类,有2种类型
* @param[in] trajectory_id 新生成的轨迹的id
* @param[in] expected_sensor_ids 所有需要的topic的名字的集合
* @param[in] wrapped_trajectory_builder 完整的slam GlobalTrajectoryBuilder
*/
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder)
: sensor_collator_(sensor_collator),
// 以下两个参数在 configuration_files/trajectory_builder.lua 中
// collate_landmarks 为 false, 不要将landmark数据放入到阻塞队列中
collate_landmarks_(trajectory_options.collate_landmarks()),
// collate_fixed_frame 为 true, 将gps数据放入阻塞队列中
collate_fixed_frame_(trajectory_options.collate_fixed_frame()),
trajectory_id_(trajectory_id),
wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)),
last_logging_time_(std::chrono::steady_clock::now()) {
......
}
初步看起来还是挺复杂的，先看看其简单的部分，在初始化列表中，从 trajectory_options 中获取 collate_landmarks 与 collate_fixed_frame 的配置信息，然后赋值给 collate_landmarks_ 与 collate_fixed_frame_。trajectory_id 赋值给成员变量 trajectory_id_。同时还把传入的 CollatedTrajectoryBuilder 实例赋值给了 成员变量 wrapped_trajectory_builder_，另外使用 last_logging_time_ 记录了初始化列表完成的时间。
继续接着往下分析，
( 1 ) : \color{blue}(1): (1): 其首先对参数 expected_sensor_ids(简单理解订阅的话题) 进行遍历，用所有话题的名字构建一个集合 expected_sensor_id_strings。需要注意的是，如果 collate_landmarks_ 与 collate_fixed_frame_ 设置为 false，则 landmark 与 GPS 的话题，不会添加到该集合之中。
( 2 ) : \color{blue}(2): (2): 传入的参数 sensor::CollatorInterface* const sensor_collator调用 sensor_collator_->AddTrajectory() 函数，实际就是对 sensor::Collator 实例对象的初始化。sensor_collator_->AddTrajectory() 函数 需要传入3个参数，分别为 trajectory_id, expected_sensor_id_strings 以及一个 lambda 格式的函数指针。该 lambda 函数体，比较简单，实际就是调用了 CollatedTrajectoryBuilder::HandleCollatedSensorData() 函数。
构造函数体(参数与初始化列表已经在前面讲解过了)代码注释如下：
// 获取topic的名字, 并根据参数配置决定是否加入LANDMARK与gps的topic
absl::flat_hash_set<std::string> expected_sensor_id_strings; //定义一个字符串集合
for (const auto& sensor_id : expected_sensor_ids) {
// collate_landmarks 为 false, sensor_collator_不处理LANDMARK数据
if (sensor_id.type == SensorId::SensorType::LANDMARK &&
!collate_landmarks_) {
continue;
}
// collate_fixed_frame 为 true, sensor_collator_处理gps数据
if (sensor_id.type == SensorId::SensorType::FIXED_FRAME_POSE &&
!collate_fixed_frame_) {
continue;
}
expected_sensor_id_strings.insert(sensor_id.id);
}
// sensor::Collator的初始化
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data));
});
六、成员函数
1、AddData()
分了 CollatedTrajectoryBuilder 构造函数之后，在来分析一下其他的成员函数：
// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
该成员函数，就是前面提到被多个重载 CollatedTrajectoryBuilder::AddSensorData() 函数调用的 AddData 函数。该函数比较简单，就是调用 sensor::Collator 实例对象 sensor_collator_ 的 AddSensorData() 函数。关于 sensor::Collator 的相关内容后续为大家进行详细的讲解。
2、HandleCollatedSensorData()
该函数前面的内容，暂时忽略，在下一篇博客中会对 std::map
// 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
这里的 wrapped_trajectory_builder_ 实际上是 GlobalTrajectoryBuilder，再来看AddToTrajectoryBuilder 代码实现：
// 调用传入的trajectory_builder的AddSensorData()
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}
也就是说最终把数据传送给了 GlobalTrajectoryBuilder(前后端打包的) 的实例对象。
七、结语
如果大家觉得比较懵逼也没有关系，后续了解了其他相关类之后，到时候再进行复盘一下就比较比较简单了，先来对该篇博客做个总结把。
( 1 ) : \color{blue}(1): (1): 首先在开启一条新轨迹的时候，会调用 src/cartographer/cartographer/mapping/map_builder.cc 中的 MapBuilder::AddTrajectoryBuilder() 函数，该函数中会构建一个 CollatedTrajectoryBuilder 对象，存储于 trajectory_builders_ 之中。
( 2 ) : \color{blue}(2): (2): src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中的MapBuilderBridge::AddTrajectory() 函数，会把 SensorBridge 的实例与 CollatedTrajectoryBuilder 对象绑定在一起。
( 3 ) : \color{blue}(3): (3): src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 的 Node::LaunchSubscribers() 函数会订阅话题，然后注册回调函数，所有的回调函数都会执行一句类似 trajectory_builder_->AddSensorData() 的代码。
( 4 ) : \color{blue}(4): (4): 实际上 trajectory_builder_->AddSensorData() 就是调用的就是 CollatedTrajectoryBuilder::AddSensorData() 函数。CollatedTrajectoryBuilder::AddSensorData()又会调用 CollatedTrajectoryBuilder::AddData() 函数。
( 5 ) : \color{blue}(5): (5): CollatedTrajectoryBuilder::AddData() 实际上会调用到CollatedTrajectoryBuilder初始化时传入的 sensor::Collator sensor_collator_ 变量的 sensor_collator_->AddTrajectory() 函数。最终把数据传送给了 GlobalTrajectoryBuilder
绕了一大堆，总的来说，初始注册的回调函数，整理数据之后，最终都会调用到 sensor::Collator::AddTrajectory()，把数据传送给了 GlobalTrajectoryBuilder