(02)Cartographer源码无死角解析-(15) Node::AddTrajectory()→回调函数之数据流向分析-CSDN博客

本文链接：https://blog.csdn.net/weixin_43013761/article/details/127686479

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过上一篇博客，可以知道 Node::AddTrajectory() 会为每个轨迹 id，根据传感器种类与数量订阅话题，同时注册如下回调函数→但是注意，同一类型的位姿传感器共用一个回调函数。如：

Node::HandleLaserScanMessage() //所有单线雷达topic回调函数
Node::HandleMultiEchoLaserScanMessage() //所有回声波雷达topoc回调函数
Node::HandlePointCloud2Message() //所有多线雷达topic回调函数
Node::HandleImuMessage() //所有IMU topic回调函数
Node::HandleOdometryMessage() //所有里程计 topic回调函数
Node::HandleNavSatFixMessage() //GPS topic回调函数
Node::HandleLandmarkMessage()//Landmar 回调函数

上述函数其实是个中转站，执行这些函数的时候，都是进去先上锁，然后根据参数再真正的调用对应的数据处理函数。如 Node::HandleLaserScanMessage() 函数实现如下：

// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
                                  const std::string& sensor_id,
                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  // 根据配置,是否将传感器数据跳过
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

可以看到，首先判断一下该传感器数据是否需要跳过(根据采样频率→前面的博客构建采样器的时候有讲解)。然后调用 map_builder_bridge_ 中的 HandleLaserScanMessage() 函数。这里的 map_builder_bridge_ 实际上是在 node_main.cc 文件中创建的 map_builder，如下所示

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

二、HandleOdometryMessage()

先来讨论一下 Node::HandleOdometryMessage() 这个函数，其首先上锁，然后判断一下当前采样到的数据是否使用，如果不使用则直接返回。

如果对该数据进行使用，则获得其轨迹对应的 SensorBridge 类对象 sensor_bridge_ptr 指针，通过其调用 sensor_bridge_ptr->ToOdometryData(msg) 函数，把 msg 转换成 ::cartographer::sensor::OdometryData(里程计数据) 格式，转换完成之后，数据有两个流向：①位姿估估算器。②sensor_bridge_ptr 的 HandleOdometryMessage() 函数，代码注释如下：

/**
 * @brief 处理里程计数据,里程计的数据走向有2个
 * 第1个是传入PoseExtrapolator,用于位姿预测
 * 第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] sensor_id 里程计的topic名字
 * @param[in] msg 里程计的ros格式的数据
 */
void Node::HandleOdometryMessage(const int trajectory_id,
                                 const std::string& sensor_id,
                                 const nav_msgs::Odometry::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
  // extrapolators_使用里程计数据进行位姿预测
  if (odometry_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
  }
  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}

推测器 extrapolators_ 是为融合里程计然后推断出一个位姿。

三、Node::HandleImuMessage()

代码逻辑基本比较一致，上锁，判断这这一次轨迹下的IMU数是否需要处理，如果需要处理通过：

  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);

把数据转换成 cartographer::sensor::ImuData 格式。然后也有两个数据流向，第1个是传入PoseExtrapolator,用于位姿预测；第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理。注释如下：

/**
 * @brief 处理imu数据,imu的数据走向有2个
 * 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定
 * 第2个是传入SensorBridge,使用其传感器处理函数进行imu数据处理
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] sensor_id imu的topic名字
 * @param[in] msg imu的ros格式的数据
 */
void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  // extrapolators_使用里程计数据进行位姿推测
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

推测器 extrapolators_ 是为了融合IMU与里程计数据然后推断出一个位姿。

四、共性回调函数讲解

对于 Node::HandleNavSatFixMessage() 、Node::HandleLandmarkMessage()
Node::HandleLaserScanMessage()Node::HandleMultiEchoLaserScanMessage() 、Node::HandlePointCloud2Message() 这五个函数都是类似的，而且比较简单，这里就略过了。

五、MapBuilderBridge

上述的7各回调函数中转站，都是通过 map_builder_bridge_.sensor_bridge(trajectory_id) 去调用其实际的回调函数。先来看看 map_builder_bridge_ 这个变量，其在 node.h 文件中有定义如下：

  // c++11: GUARDED_BY 是在Clang Thread Safety Analysis（线程安全分析）中定义的属性
  // GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护.
  // 对数据的读操作需要共享访问, 而写操作则需要互斥访问.
  // 官方介绍文档: https://clang.llvm.org/docs/ThreadSafetyAnalysis.html
  MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

这个声明表示，在使用这个变量的时候，必须对 mutex_ 进行加锁，如果没有加锁就使用，则会报错。map_builder_bridge_ 初始化，是在 Node 类构造函数初始化列表中完成的，在构造 Node 对象的时候，需要传递一个 map_builder 类对象。Node 类对象的构造位于 node_main.cc 文件中，代码如下所示：

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  
  // c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象, 
  // 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..
  // 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象, 
  // 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
  // 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

可以知道，构建 MapBuilderBridge 对象 map_builder_bridge_ 其构造函数需要传递三个参数，那就是 node_options_、std::move(map_builder)、 tf_buffer。MapBuilderBridge 实现于
src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 文件中。

先来看看其头文件 map_builder_bridge.h，其中有有很多函数或者成员变量带有 local 关键字，这里对两个 local 与 global 做下简单的介绍：

note: local frame 与 global frame

carographer中存在两个地图坐标系, 分别为global frame与local frame

local frame
是前端(扫描匹配)的坐标系, 这个坐标系与坐标系下的机器人的坐标, 一经扫描匹配之后就不会再被改变，
所以其容易产生累计误差。后端的坐标系, 回环检测与后端位姿图优化都不会对前端的任何坐标产生作用.
 
global frame
是后端的坐标系, 是表达被回环检测与位姿图优化所更改后的坐标系.当优化之后, 
这个坐标系下的节点坐标(包括点云的位姿, submap 的位姿)会发生变化.但坐标系本身不会发生变化

如果觉得不好理解也没有关系，后续会进行比较详细的讲解。先来看看其构造函数：

/**
 * @brief 根据传入的node_options, MapBuilder, 以及tf_buffer 完成三个本地变量的初始化
 * 
 * @param[in] node_options 参数配置
 * @param[in] map_builder 在node_main.cc中传入的MapBuilder
 * @param[in] tf_buffer tf_buffer
 */
MapBuilderBridge::MapBuilderBridge(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
    tf2_ros::Buffer* const tf_buffer)
    : node_options_(node_options),
      map_builder_(std::move(map_builder)),
      tf_buffer_(tf_buffer) {}

代码比较单调，简单的初始化列表赋值操作。需要注意的是存在函数 MapBuilderBridge::AddTrajectory() 是比较重要的，如果有印象的朋友可以回忆起来其是在 Node::AddTrajectory() 函数中，没有进行话题订阅之前调用的。其返回值是 trajectory_id。

MapBuilderBridge::AddTrajectory() 函数需要两个参数，分别为 SensorId 的集合与 TrajectoryOptions 类型。SensorI 存储的是传感器类型与话题topic名称，TrajectoryOptions 为追踪的相关配置参数。该函数主要有如下三个步骤：

$\color{blue}(1)$ 调用 map_builder_->AddTrajectoryBuilder() 函数添加一条新的轨迹。该函数需要传递三个参数，前两个分别为 expected_sensor_ids 与 trajectory_options.trajectory_builder_options。第三个参数为使用 lambda 表达式实现了一个函数:

1.该函数需要五个个参数，分别为
	const int trajectory_id, 
	const ::cartographer::common::Time time,
	const Rigid3d local_pose,
	::cartographer::sensor::RangeData range_data_in_local,
	const std::unique_ptr<
	const ::cartographer::mapping::TrajectoryBuilderInterface::InsertionResult

2.函数内容就是调用下面函数
    // 保存local slam 的结果数据 5个参数实际只用了4个
    OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);

$\color{blue}(2)$ 为这个新轨迹添加一个SensorBridge。构造 SensorBridge 需要传入如下五个参数：

  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
      trajectory_options.num_subdivisions_per_laser_scan, //一帧数据分成几次处理，一般设置为1
      trajectory_options.tracking_frame, //把所有数据都转换到该坐标系下 
      node_options_.lookup_transform_timeout_sec, //查找tf时的超时时间
      tf_buffer_, //用于存储 tf
      map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder 轨迹构建器

$\color{blue}(3)$ 保存轨迹的参数，然后返回 trajectory_id

  // Step: 3 保存轨迹的参数配置
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);
  CHECK(emplace_result.second == true);
  return trajectory_id;

总的来说，通过 map_builder_->AddTrajectoryBuilder() 函数完成了一条轨迹的新建，轨迹新建会绑定 OnLocalSlamResult() 函数，同时对应一个SensorBridge，并且会把该轨迹的参数保存在 MapBuilderBridge::trajectory_options_变量之中。

六、SensorBridge

再次会到前面的回调函数 Node::HandleOdometryMessage() 与 Node::HandleImuMessage()，找到如下代码：

auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);

其就是根据 trajectory_id 获得与之对应的 SensorBridge 成员对象指针。然后还会调用如下函数进行数据转换

auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);

以及还会调用真实回调函数，如下：

sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
map_builder_bridge_.sensor_bridge(trajectory_id)->HandleNavSatFixMessage(sensor_id, msg);
map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLandmarkMessage(sensor_id, msg);
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage(sensor_id, msg);
map_builder_bridge_.sensor_bridge(trajectory_id)->HandleMultiEchoLaserScanMessage(sensor_id, msg);
map_builder_bridge_.sensor_bridge(trajectory_id)->HandlePointCloud2Message(sensor_id, msg);

从这里可以看到 sensor_bridge_ptr 是十分重要的，所有传感器数据的处理都集中在 sensor_bridge 之中。

结语

$\color{red}需要注意的是：$ Node::HandleOdometryMessage() 、 Node::HandleImuMessage() 中的位姿推测器 extrapolators_ 与前端优化过程中的位姿推测器不是同一个同一个推测器。他们是同一类的不同对象。extrapolators_ 这个位置优化器，通过参数 use_pose_extrapolator 参数控制是否启用。如果设置为 Ture，Node 类在发送 tf 数据的时候，就会使用位姿推测器，推测出来的位姿。一般设置为 false，除非 IMU 与里程计的数据十分的精准。

$\color{red}总结：$ Node 中的成员变量 MapBuilderBridge map_builder_bridge_，其包含了一个 std::unique_ptr<cartographer::mapping::MapBuilderInterface> 对象，该对象在 node_main.cc 中构建，map_builder_bridge_ 中可以存在多条轨迹，每条轨迹都对应一个 SensorBridge 与一个 trajectory_options 变量。分别存储于 MapBuilderBridge 中的 sensor_bridges_ 与 trajectory_options_ 之中。同时每条轨迹都会与函数：

OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);

绑定在一起。那么到这里，对于Node::AddTrajectory()的回调函数，以及新轨迹的添加有了一个大致的了解。知道了数据最终流向 SensorBridge 的处理函数。