传感器数据分发CollatedTrajectoryBuilder
一、前言
首先对前面的知识做一个回顾，从 node_main.cc 文件中开始;
//根据配置文件，命令行参数与话题重映射，订阅默认话题开始一条轨迹
node.StartTrajectoryWithDefaultTopics(trajectory_options);
----------------------
在node.cc里面
// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
CHECK(ValidateTrajectoryOptions(options));
// 添加一条轨迹
AddTrajectory(options);
}
--------------------------
在node.cc
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);
.................................
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);其上的 map_builder_bridge_.AddTrajectory 与 LaunchSubscribers() 是十分重要的两个函数
（1）：map_builder_bridge_.AddTrajectory 函数主要的核心就是构建CollatedTrajectoryBuilder对象存储于 node::map_builder_bridge_::map_builder_::trajectory_builders_变量之中，然后返回一个 trajectory_id，再根据 trajectory_id 构建一个 SensorBridge对象，创建该对象时代码如下：
// 在src\cartographer_ros\cartographer_ros\cartographer_ros\map_builder_bridge.cc
// 的int MapBuilderBridge::AddTrajectory函数里
// 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
注意 其上的 GetTrajectoryBuilder(trajectory_id) 就是获取 trajectory_builders_ 中的 CollatedTrajectoryBuilder 对象。然后作为参数传送给 SensorBridge 的构造函数
2)： LaunchSubscribers() 会根据 trajectory_id 与其对应的配置 TrajectoryOptions& options，进行话题的订阅，同时注册回调函数。所有的回调函数（实现在src\cartographer_ros\cartographer_ros\cartographer_ros\sensor_bridge.cc），根据查询的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 文件中，还存在一个如下函数也是比较重要的的：
// 将local slam 的结果也作为一种传感器数据进行处理
void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData>
local_slam_result_data) override {
AddData(std::move(local_slam_result_data));
}
private:
void AddData(std::unique_ptr<sensor::Data> data);
void HandleCollatedSensorData(const std::string& sensor_id,
std::unique_ptr<sensor::Data> data);后续会对他们进行详细的分析。
五、CollatedTrajectoryBuilder.cc
在大致了解了头文件之后，在来看看 CollatedTrajectoryBuilder 的构造函数。首先要了解的就是该类实例化时，传入的参数。这里回到前面的讲解的二、构造传参在src\cartographer\cartographer\mapping\map_builder.cc，可以看到如下类似代码:
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 类对象，
// 第一部分：3D 全局轨迹构建器创建函数
// 3d的完整的slam
// 工厂函数，专门创建 3D 版本的全局轨迹构建器，把3D 前端和3D 后端绑定在一起
std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
// 参数1 3D局部SLAM前端，负责实时算位姿、建子图
std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder,
// 参数2 轨迹id
const int trajectory_id,
// 参数3 3D 后端位姿图，负责全局优化、回环闭合
mapping::PoseGraph3D* const pose_graph,
// 参数4 局部SLAM结果回调
const TrajectoryBuilderInterface::LocalSlamResultCallback&
local_slam_result_callback,
// 参数5 运动滤波器
const absl::optional<MotionFilter>& pose_graph_odometry_motion_filter)
{
// 创建并返回3D全局轨迹构建器
return absl::make_unique<
GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
std::move(local_trajectory_builder), trajectory_id, pose_graph,
local_slam_result_callback, pose_graph_odometry_motion_filter);
}注意该类继承于 TrajectoryBuilderInterface 接口。
class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface {CollatedTrajectoryBuilder其构造函数及其初始化列表如下：
src\cartographer\cartographer\mapping\internal\collated_trajectory_builder.cc
/**
* @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()) {
// 获取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));
});
}
// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}初步看起来还是挺复杂的，先看看其简单的部分，在初始化列表中，从 trajectory_options 中获取 collate_landmarks 与 collate_fixed_frame 的配置信息，然后赋值给 collate_landmarks_ 与 collate_fixed_frame_。trajectory_id 赋值给成员变量 trajectory_id_。
同时还把传入的 CollatedTrajectoryBuilder 实例赋值给了 成员变量 wrapped_trajectory_builder_，另外使用 last_logging_time_ 记录了初始化列表完成的时间。
继续接着往下分析，
(1):其首先对参数 expected_sensor_ids(简单理解订阅的话题) 进行遍历，用所有话题的名字构建一个集合 expected_sensor_id_strings。需要注意的是，如果 collate_landmarks_ 与 collate_fixed_frame_ 设置为 false，则 landmark 与 GPS 的话题，不会添加到该集合之中。
(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() 函数。这个在下面讲解成员函数会有
六、成员函数
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<std::string, common::RateTimer<>> rate_timers_; 进行详细的讲解，该函数最后就是将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用，执行如下代码：
// 将排序好的数据送入 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_);
}
七、结语
如果大家觉得比较懵逼也没有关系，后续了解了其他相关类之后，到时候再进行复盘一下就比较比较简单了，先来对该篇博客做个总结把。
1):首先在开启一条新轨迹的时候，会调用 src/cartographer/cartographer/mapping/map_builder.cc 中的 MapBuilder::AddTrajectoryBuilder() 函数，该函数中会构建一个 CollatedTrajectoryBuilder 对象，存储于 trajectory_builders_ 之中。
2): src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中的MapBuilderBridge::AddTrajectory() 函数，会把 SensorBridge 的实例与 CollatedTrajectoryBuilder 对象绑定在一起。
3):src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 的 Node::LaunchSubscribers() 函数会订阅话题，然后注册回调函数，所有的回调函数都会执行一句类似 trajectory_builder_->AddSensorData() 的代码。
4): 实际上 trajectory_builder_->AddSensorData() 就是调用的就是 CollatedTrajectoryBuilder::AddSensorData() 函数。CollatedTrajectoryBuilder::AddSensorData()又会调用 CollatedTrajectoryBuilder::AddData() 函数。
5):CollatedTrajectoryBuilder::AddData() 实际上会调用到CollatedTrajectoryBuilder初始化时传入的 sensor::Collator sensor_collator_ 变量的 sensor_collator_->AddTrajectory() 函数。最终把数据传送给了 GlobalTrajectoryBuilder
绕了一大堆，总的来说，初始注册的回调函数，整理数据之后，最终都会调用到 sensor::Collator::AddTrajectory()，把数据传送给了 GlobalTrajectoryBuilder
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
9
0- 0
已为社区贡献1条内容
所有评论(0)