(02)Cartographer源码无死角解析-(22) 传感器数据分发→总体分析

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

首先对前面的知识做一个回顾，从 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);
1
2
3
4
5
6
7

其上的 map_builder_bridge_.AddTrajectory 与 LaunchSubscribers() 是十分重要的两个函数：

$(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
1
2
3
4
5
6
7

$注意$ 其上的 GetTrajectoryBuilder(trajectory_id) 就是获取 trajectory_builders_ 中的 CollatedTrajectoryBuilder 对象。然后作为参数传送给 SensorBridge 的构造函数。

$(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()//根据采样频率评估是否调用该函数
	......
1
2
3
4
5
6
7
8
9
10
11

还有一些回调函数，就不一一在这里列举了，所有的回调函数，根据查询的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);
	......
1
2
3
4
5

上面只列举了一部分，从上面可以看出，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)));
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

 

三、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 
1

故 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++ 中现有的可调用实体的一种类型安全的包裹（相对来说, 函数指针的调用不是类型安全的）
1
2
3
4

在 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>)>;
1
2
3
4
5
6
7
8

其上表示用 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);
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

从上面的代码中，可以看到一个共同调用的函数 AddData(sensor::MakeDispatchable(xxx))。处理GPS和Landmark数据时，会对 collate_fixed_frame_ 与 collate_landmarks_ 参数进行判断，然后再决定是否调用 AddData() 函数。这两个参数都是再 src/cartographer/configuration_files/trajectory_builder.lua 中设置。另外其上还可以看到

 wrapped_trajectory_builder_->AddSensorData(xxx)
1

该部分类容后面进行讲解。同时在 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);
1
2
3
4
5
6
7

后续会他们进行详细的分析。
 

五、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)));
1
2
3
4
5
6
7
8
9

可以看到构建 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()) {
	......
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

初步看起来还是挺复杂的，先看看其简单的部分，在初始化列表中，从 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() 函数。

构造函数体(参数与初始化列表已经在前面讲解过了)代码注释如下：

  // 获取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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

 

六、成员函数

1、AddData()

分了 CollatedTrajectoryBuilder 构造函数之后，在来分析一下其他的成员函数：

// 将数据传入sensor_collator_的AddSensorData进行排序
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
  sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}
1
2
3
4

该成员函数，就是前面提到被多个重载 CollatedTrajectoryBuilder::AddSensorData() 函数调用的 AddData 函数。该函数比较简单，就是调用 sensor::Collator 实例对象 sensor_collator_ 的 AddSensorData() 函数。关于 sensor::Collator 的相关内容后续为大家进行详细的讲解。
 

2、HandleCollatedSensorData()

该函数前面的内容，暂时忽略，在下一篇博客中会对 std::map> rate_timers_; 进行详细的讲解，该函数最后就是将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用，执行如下代码：

  // 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用
  data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
1
2

这里的 wrapped_trajectory_builder_ 实际上是 GlobalTrajectoryBuilder，再来看AddToTrajectoryBuilder 代码实现：

  // 调用传入的trajectory_builder的AddSensorData()
  void AddToTrajectoryBuilder(
      mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
    trajectory_builder->AddSensorData(sensor_id_, data_);
  }
1
2
3
4
5

也就是说最终把数据传送给了 GlobalTrajectoryBuilder(前后端打包的) 的实例对象。
 

七、结语

如果大家觉得比较懵逼也没有关系，后续了解了其他相关类之后，到时候再进行复盘一下就比较比较简单了，先来对该篇博客做个总结把。

$(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