讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证{\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 为这个新轨迹 添加一个SensorBridgesensor_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(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(trajectory_options, sensor_collator_.get(), trajectory_id,expected_sensor_ids,// 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder), trajectory_id,static_cast(pose_graph_.get()),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位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder2D(std::move(local_trajectory_builder), trajectory_id,static_cast(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)>;

其上表示用 LocalSlamResultCallback 表示一个回调函数，该回调函数无返回值，需要传入五个参数。