讲解关于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
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
或: trajectory_builder_->AddSensorData(sensor_id, carto::sensor::FixedFramePoseData{time, absl::optional
或: 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
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast
local_slam_result_callback, pose_graph_odometry_motion_filter)));
// CollatedTrajectoryBuilder初始化
trajectory_builders_.push_back(absl::make_unique
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast
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 transform::Rigid3d /* local pose estimate */, sensor::RangeData /* in local frame */, std::unique_ptr 其上表示用 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 // 将local slam 的结果也作为一种传感器数据进行处理 void AddLocalSlamResultData(std::unique_ptr override {AddData(std::move(local_slam_result_data));} void HandleCollatedSensorData(const std::string& sensor_id,std::unique_ptr 后续会他们进行详细的分析。 五、CollatedTrajectoryBuilder.cc 在大致了解了头文件之后，在来看看 CollatedTrajectoryBuilder 的构造函数。首先要了解的就是该类实例化时，传入的参数。这里回到前面的讲解的二、构造传参，可以看到如下类似代码: // CollatedTrajectoryBuilder初始化 trajectory_builders_.push_back(absl::make_unique trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids, // 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast 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 std::unique_ptr : 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 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 HandleCollatedSensorData(sensor_id, std::move(data)); }); 六、成员函数 1、AddData() 分了 CollatedTrajectoryBuilder 构造函数之后，在来分析一下其他的成员函数： // 将数据传入sensor_collator_的AddSensorData进行排序 void CollatedTrajectoryBuilder::AddData(std::unique_ptr 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 推荐文章
您阅读本篇文章共花了：
发表评论