Cartographer 源码解析（一）：系统总入口与传感器数据如何进入 Cartographer

一、文章简介

Cartographer 的完整 SLAM 系统并不是"订阅到一帧激光点云后，直接进行扫描匹配"这么简单。

从 ROS 角度看，用户只需要发布 /scan、/points_raw、/imu、/odom 等 Topic；但从 Cartographer Core 的角度看，这些消息还必须经历 ROS 消息转换、TF 坐标变换、时间排序、不同传感器类型分发、局部 SLAM 处理以及后端 Node 创建等流程，最终才能进入后续的 Submap、回环检测和全局优化模块。

这一篇主要解决以下问题：

MapBuilder 到底是什么，它负责创建哪些对象？
PoseGraph2D、OptimizationProblem2D、LocalTrajectoryBuilder2D 分别什么时候创建？
为什么外部传感器数据不是直接进入 LocalTrajectoryBuilder2D？
CollatedTrajectoryBuilder 到底负责什么？
GlobalTrajectoryBuilder 为什么名字里有 Global，却不是 PoseGraph 后端？
一帧激光什么时候会真正变成 PoseGraph 中的 Node？
CollatedTrajectoryBuilder 与 RangeDataCollator 有什么区别？

本文不深入扫描匹配、Submap 概率栅格更新、回环检测与 Ceres 位姿图优化的内部算法，而是先把系统入口、对象关系和传感器数据链路讲清楚。Cartographer 的 MapBuilder 确实负责创建 PoseGraph 与 trajectory builder 链路，而 ROS 数据通常经由 SensorBridge 转成 Cartographer 内部数据再送入 trajectory builder。

二、先看总处理链：一帧传感器数据到底走到哪里

先把完整逻辑固定下来，后面所有源码分析都围绕这条链展开。

复制代码

ROS 传感器数据
    │
    ├── /scan、/points_raw
    ├── /imu
    ├── /odom
    └── /fix、Landmark 等
    ↓
cartographer_ros::Node
    ↓
MapBuilderBridge
    ↓
SensorBridge
    ↓
TrajectoryBuilderInterface::AddSensorData(...)
    ↓
CollatedTrajectoryBuilder
    ↓
Collator / TrajectoryCollator
    ↓
GlobalTrajectoryBuilder
    ├── LocalTrajectoryBuilder2D
    │       ↓
    │   PoseExtrapolator
    │       ↓
    │   Scan Matching
    │       ↓
    │   ActiveSubmaps2D
    │
    └── PoseGraph2D
            ↓
        Node / Submap / Constraint
            ↓
        回环检测与全局优化

这条链里最需要先记住的一点是：

ROS 层传感器数据并不会直接进入 LocalTrajectoryBuilder2D。

真正的顺序是：

复制代码

ROS 回调
    ↓
SensorBridge
    ↓
CollatedTrajectoryBuilder
    ↓
GlobalTrajectoryBuilder
    ↓
LocalTrajectoryBuilder2D

也就是说，LocalTrajectoryBuilder2D 是 Local SLAM 前端的核心，但它并不是 ROS Topic 的直接接收者。

三、Cartographer 的核心对象关系

Cartographer 中最容易混淆的是几个名字很像的对象。先从"谁持有谁"的角度看。

复制代码

MapBuilder
│
├── ThreadPool
│
├── PoseGraph2D
│   └── OptimizationProblem2D
│
├── Collator / TrajectoryCollator
│
└── trajectory_builders_[trajectory_id]
    └── CollatedTrajectoryBuilder
        └── GlobalTrajectoryBuilder
            ├── LocalTrajectoryBuilder2D
            └── PoseGraph2D*

这张关系图非常重要。

MapBuilder 是总装配器。它创建整个系统共享的对象，例如 PoseGraph、线程池和传感器排序器。

每条 trajectory 会有一个最外层 CollatedTrajectoryBuilder。这个对象负责接收外部传感器数据，并保证不同传感器消息尽量按时间顺序进入后续模块。

CollatedTrajectoryBuilder 内部包装 GlobalTrajectoryBuilder。而 GlobalTrajectoryBuilder 又持有 Local SLAM 前端对象 LocalTrajectoryBuilder2D，同时保存对全局后端 PoseGraph2D 的引用。

所以可以把这些对象理解为：

对象	核心职责
`MapBuilder`	创建并管理整套 Cartographer SLAM 系统
`PoseGraph2D`	管理 Node、Submap、局部约束、回环约束与全局优化
`OptimizationProblem2D`	保存优化变量、约束残差，调用 Ceres 求解
`CollatedTrajectoryBuilder`	对不同类型传感器数据进行时间排序
`GlobalTrajectoryBuilder`	把 Local SLAM 输出交给 PoseGraph
`LocalTrajectoryBuilder2D`	实时位姿预测、扫描匹配、局部 Submap 更新
`RangeDataCollator`	多颗 LiDAR 点云时间同步与融合

官方 Cartographer 源码中，MapBuilder 创建 PoseGraph，AddTrajectoryBuilder() 创建并包装轨迹构建器；cartographer_ros 则提供 ROS 接入层。

四、MapBuilder：Cartographer 的总装配器

4.1 MapBuilder 是什么

MapBuilder 可以理解为 Cartographer Core 的系统管理器。

它不负责处理某一帧激光，也不负责执行某一次扫描匹配。它更像是整个 SLAM 系统的"总工厂"，负责根据配置文件创建一套可运行的 Cartographer 系统。

它主要做四类工作：

第一，创建后台线程池。Cartographer 前端需要实时运行，而回环检测、约束构建、位姿图优化等任务比较耗时，通常放在后台线程中异步执行。

第二，创建 PoseGraph。对于 2D 模式，创建 PoseGraph2D；对于 3D 模式，创建 PoseGraph3D。

第三，创建全局传感器时间排序器。这个排序器后续会被 CollatedTrajectoryBuilder 使用，用来协调激光、IMU、Odom 等消息的处理顺序。

第四，为每一条 trajectory 创建 Local SLAM、GlobalTrajectoryBuilder 与 CollatedTrajectoryBuilder。

因此，MapBuilder 创建时并不代表"已经开始建图"；它只是先把整个 SLAM 系统的基础框架准备好。

4.2 MapBuilder 构造函数

下面是 MapBuilder::MapBuilder() 的核心逻辑简化版。

复制代码

MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
    : options_(options),
      thread_pool_(options.num_background_threads()) {

  // 当前实例只能使用 2D 或 3D TrajectoryBuilder 之一。
  CHECK(options.use_trajectory_builder_2d() ^
        options.use_trajectory_builder_3d());

  // 2D 模式：创建全局 PoseGraph2D。
  if (options.use_trajectory_builder_2d()) {
    pose_graph_ = absl::make_unique<PoseGraph2D>(
        options_.pose_graph_options(),

        // PoseGraph2D 内部持有优化问题。
        absl::make_unique<optimization::OptimizationProblem2D>(
            options_.pose_graph_options()
                .optimization_problem_options()),

        // 回环、约束构建、优化等后台任务使用线程池。
        &thread_pool_);
  }

  // 创建传感器时间排序器。
  if (options.collate_by_trajectory()) {
    sensor_collator_ =
        absl::make_unique<sensor::TrajectoryCollator>();
  } else {
    sensor_collator_ =
        absl::make_unique<sensor::Collator>();
  }
}

这段代码中有几个关键点。

第一，PoseGraph2D 在 MapBuilder 初始化时创建，不是在第一帧激光到来时创建。也就是说，后端图优化模块在系统启动时就已经存在。

第二，OptimizationProblem2D 被传给 PoseGraph2D。后续进行全局优化时，PoseGraph2D 负责管理哪些 Node、Submap 和 Constraint 应参与优化；OptimizationProblem2D 则负责将它们组织成真正的 Ceres 优化问题。

第三，ThreadPool 是全局后台线程池。Local SLAM 前端通常由传感器回调推动，强调实时性；PoseGraph 的回环搜索、约束构建与全局优化则可以异步执行，避免阻塞前端。

第四，构造函数末尾的 sensor_collator_ 不是多雷达融合器，而是全局"多传感器时间排序器"。它处理激光、IMU、Odom 等不同类型数据的时间关系。

4.3 为什么 2D 和 3D 只能二选一

源码中的这一句：

复制代码

CHECK(options.use_trajectory_builder_2d() ^
      options.use_trajectory_builder_3d());

使用的是异或运算。

复制代码

2D = true， 3D = false：合法
2D = false，3D = true ：合法
2D = true， 3D = true ：非法
2D = false，3D = false：非法

原因是 2D 与 3D 的核心结构不同。

2D 中使用的是：

复制代码

LocalTrajectoryBuilder2D
PoseGraph2D
Submap2D
ActiveSubmaps2D
OptimizationProblem2D

3D 中则使用对应的 3D 版本。二者的状态变量维度、地图表示、扫描匹配方式、重力处理方式和优化过程都有区别，因此同一个 MapBuilder 不允许同时运行 2D 与 3D trajectory builder。

五、Trajectory：不是一条轨迹线，而是一套 SLAM 任务

很多人看到 trajectory_id 时，会以为它只是 RViz 中显示路径的编号。实际上它更像"一次连续 SLAM 任务"的编号。

例如：

复制代码

trajectory 0：机器人第一次在线建图
trajectory 1：加载旧地图后重新定位
trajectory 2：第二台机器人开始建图
trajectory 3：离线播放另一段 rosbag

每条 trajectory 都有自己的 Local SLAM 状态，包括：

自己的 LocalTrajectoryBuilder2D
自己的 IMU 缓存
自己的 Odom 缓存
自己的 PoseExtrapolator
自己的一组 Active Submaps
自己生成的 Node
自己的局部轨迹

但这些 trajectory 可以共享同一个 PoseGraph2D。因此，多机器人、多个建图片段或多个定位轨迹，最终仍然可以通过后端约束放进同一个全局地图中。

trajectory_id 的生成方式很简单：

复制代码

const int trajectory_id = trajectory_builders_.size();

例如当前还没有 trajectory：

复制代码

trajectory_builders_.size() = 0

第一次创建时：

复制代码

trajectory_id = 0

第二次创建时：

复制代码

trajectory_id = 1

所以 trajectory_id 是 Cartographer 内部的轨迹编号，不一定等于机器人编号，也不一定等于 Topic 名。

六、MapBuilder::AddTrajectoryBuilder()：一整条 SLAM 链路在这里创建

MapBuilder::AddTrajectoryBuilder() 是本文最关键的函数之一。

它并不是简单创建一个对象，而是一次性创建当前 trajectory 对应的完整数据处理链。

复制代码

int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback callback) {

  // 1. 分配当前轨迹编号。
  const int trajectory_id = trajectory_builders_.size();

  // 2. 创建当前 trajectory 专属的 Local SLAM 前端。
  std::unique_ptr<LocalTrajectoryBuilder2D> local_builder;

  if (trajectory_options.has_trajectory_builder_2d_options()) {
    local_builder = absl::make_unique<LocalTrajectoryBuilder2D>(
        trajectory_options.trajectory_builder_2d_options(),
        SelectRangeSensorIds(expected_sensor_ids));
  }

  // 3. 创建前后端连接器。
  auto global_builder = CreateGlobalTrajectoryBuilder2D(
      std::move(local_builder),
      trajectory_id,
      static_cast<PoseGraph2D*>(pose_graph_.get()),
      callback,
      pose_graph_odometry_motion_filter);

  // 4. 在最外层套上时间排序包装器。
  trajectory_builders_.push_back(
      absl::make_unique<CollatedTrajectoryBuilder>(
          trajectory_options,
          sensor_collator_.get(),
          trajectory_id,
          expected_sensor_ids,
          std::move(global_builder)));

  return trajectory_id;
}

这段代码的创建顺序是：

复制代码

MapBuilder::AddTrajectoryBuilder()
    ↓
创建 LocalTrajectoryBuilder2D
    ↓
创建 GlobalTrajectoryBuilder
    ↓
创建 CollatedTrajectoryBuilder
    ↓
保存到 trajectory_builders_
    ↓
返回 trajectory_id

最终对象包装关系为：

复制代码

CollatedTrajectoryBuilder
    ↓ 包装
GlobalTrajectoryBuilder
    ↓ 持有
LocalTrajectoryBuilder2D

并且 GlobalTrajectoryBuilder 还持有一个指向全局 PoseGraph2D 的指针。

这里最重要的一点是：ROS 层最后拿到的不是 Local SLAM 对象，而是最外层的 CollatedTrajectoryBuilder。因此，所有传感器数据首先都会进入 CollatedTrajectoryBuilder。

官方源码中的 MapBuilder::AddTrajectoryBuilder() 正是在此处创建 LocalTrajectoryBuilder、GlobalTrajectoryBuilder，并将其交给 CollatedTrajectoryBuilder 包装。

七、expected_sensor_ids：系统预期会接收哪些数据

AddTrajectoryBuilder() 的第一个参数是：

复制代码

const std::set<SensorId>& expected_sensor_ids

它表示当前 trajectory 预计会收到哪些传感器数据。

例如单雷达 2D SLAM 系统可能是

复制代码

scan
imu
odom

多雷达系统可能是：

复制代码

front_lidar
rear_lidar
imu
odom

Cartographer 的 SensorId 不只是一个字符串，它通常还包含传感器类型。

复制代码

RANGE
IMU
ODOMETRY
FIXED_FRAME_POSE
LANDMARK

各类型含义如下：

类型	含义
`RANGE`	激光雷达、2D LaserScan、3D PointCloud2
`IMU`	角速度、线加速度
`ODOMETRY`	轮速里程计、视觉里程计、外部里程计
`FIXED_FRAME_POSE`	GPS、GNSS、外部绝对位姿
`LANDMARK`	AprilTag、二维码、人工地标等约束

expected_sensor_ids 有两个作用。

第一个作用是告诉 Cartographer：当前轨迹可能会收到哪些输入。

第二个作用是让 Collator 为这些传感器建立等待队列。

例如有三个传感器：

复制代码

scan
imu
odom

那么系统内部概念上会维护：

复制代码

Queue(trajectory 0, scan)
Queue(trajectory 0, imu)
Queue(trajectory 0, odom)

如果你在配置中声明了 imu，但实际 IMU Topic 没有发布，时间排序器可能会等待 IMU 数据，导致激光迟迟不能进入 Local SLAM。

八、SelectRangeSensorIds()：为什么 Local SLAM 只拿 LiDAR ID

expected_sensor_ids 里可能包含 LiDAR、IMU、Odom、GPS、Landmark。

但 LocalTrajectoryBuilder2D 初始化时，需要重点知道的是：

"当前有哪几颗 LiDAR 会往 Local SLAM 输入 range data。"

因此，Cartographer 会筛选 RANGE 类型传感器。

复制代码

std::vector<std::string> SelectRangeSensorIds(
    const std::set<SensorId>& expected_sensor_ids) {

  std::vector<std::string> range_sensor_ids;

  for (const SensorId& sensor_id : expected_sensor_ids) {
    if (sensor_id.type == SensorId::SensorType::RANGE) {
      range_sensor_ids.push_back(sensor_id.id);
    }
  }

  return range_sensor_ids;
}

假设系统传感器为：

复制代码

front_lidar
rear_lidar
imu
odom

那么最终传给 LocalTrajectoryBuilder2D 的 LiDAR 列表为：

复制代码

front_lidar
rear_lidar

这两个 ID 后续会被 Local SLAM 内部的 RangeDataCollator 使用。

也就是说，RangeDataCollator 不关心 IMU 和 Odom；它只关心多颗 LiDAR 的点云如何按时间进行融合。

九、LocalTrajectoryBuilder2D：真正的 Local SLAM 前端

LocalTrajectoryBuilder2D 是 Cartographer 前端 Local SLAM 的核心对象。

它负责当前时刻机器人局部位姿的实时估计，同时将当前激光数据写入局部 Submap。

它内部主要完成以下流程：

复制代码

激光点云输入
    ↓
RangeDataCollator 多雷达同步
    ↓
PoseExtrapolator 位姿预测
    ↓
点云去畸变
    ↓
体素滤波
    ↓
实时扫描匹配
    ↓
运动滤波
    ↓
插入 ActiveSubmaps2D
    ↓
返回 MatchingResult

其中，PoseExtrapolator 会使用 IMU 和 Odom 的历史数据预测当前时刻位姿。

扫描匹配会使用预测位姿作为初值，在当前局部 Submap 中寻找最匹配的机器人姿态。

插入 ActiveSubmaps2D 后，当前点云会成为局部地图的一部分。

但要注意：LocalTrajectoryBuilder2D 不负责回环检测，不负责全局地图纠正，也不负责优化所有历史轨迹。它只解决"当前机器人在局部地图中的实时位姿"和"局部地图更新"问题。

十、GlobalTrajectoryBuilder：前端与后端之间的桥梁

10.1 它不是 PoseGraph 后端

很多人会被名字误导，认为 GlobalTrajectoryBuilder 就是 Global SLAM 后端。

实际上不是。

真正的 Global SLAM 后端是：

复制代码

PoseGraph2D
ConstraintBuilder2D
OptimizationProblem2D

而 GlobalTrajectoryBuilder 更像一个"前后端数据转接器"。

它做的事情是：

激光送进 LocalTrajectoryBuilder2D；
IMU 同时送进 LocalTrajectoryBuilder2D 与 PoseGraph2D；
Odom 同时送进 LocalTrajectoryBuilder2D 与 PoseGraph2D；
GPS、Landmark 等绝对约束送到 PoseGraph2D；
Local SLAM 产生插入结果后，调用 PoseGraph2D 创建 Node；
将 Local SLAM 结果回调给 ROS 层。

因此，可以把它理解为：

复制代码

Local SLAM 与 PoseGraph 之间的连接器

10.2 激光进入 GlobalTrajectoryBuilder 后的处理

复制代码

void GlobalTrajectoryBuilder::AddSensorData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& timed_point_cloud_data) {

  // 激光进入 Local SLAM 前端。
  auto matching_result =
      local_trajectory_builder_->AddRangeData(
          sensor_id,
          timed_point_cloud_data);

  // 当前没有形成有效 Local SLAM 输出。
  if (matching_result == nullptr) {
    return;
  }

  // 只有插入 Submap 时，才会进一步创建 Node。
  if (matching_result->insertion_result != nullptr) {
    const auto node_id = pose_graph_->AddNode(
        matching_result->insertion_result->constant_data,
        trajectory_id_,
        matching_result->insertion_result->insertion_submaps);
  }

  // 将局部 SLAM 结果回调给 ROS 层。
  local_slam_result_callback_(
      trajectory_id_,
      matching_result->time,
      matching_result->local_pose,
      std::move(matching_result->range_data_in_local),
      nullptr);
}

这段代码可以分成三层理解。

第一层，点云被送入 LocalTrajectoryBuilder2D::AddRangeData()。

到这里，才真正进入 Local SLAM 前端。

第二层，Local SLAM 不一定每次都返回结果。

例如以下情况都可能返回空：

复制代码

多颗 LiDAR 的同步数据尚未准备好
当前点云累计数量还不够
IMU 初始化尚未完成
数据时间没有覆盖当前处理区间
当前 range data 不满足有效条件

因此：

复制代码

matching_result == nullptr

并不一定意味着系统出错，只表示"当前还无法形成一帧完整的 Local SLAM 输入"。

第三层，只有当前帧成功插入 ActiveSubmaps2D 时，才会有 insertion_result。

之后才调用：

复制代码

pose_graph_->AddNode(...)

这也意味着：

复制代码

不是每帧 LiDAR 都生成 Node。

Cartographer 的 GlobalTrajectoryBuilder 会把 range data 送入 LocalTrajectoryBuilder；当 Local SLAM 返回插入结果时，再向 PoseGraph 添加 Node。

十一、MatchingResult 与 InsertionResult 的区别

这两个对象特别容易混淆。

MatchingResult 表示：

"Local SLAM 已经对当前批次点云完成了位姿估计。"

它一般包含：

复制代码

当前处理时刻
当前 local pose
局部坐标系下的点云
扫描匹配结果
是否插入 Submap 的结果

而 InsertionResult 表示：

"当前帧不仅完成了扫描匹配，而且真的被插入到了 ActiveSubmaps2D。"

它包含：

复制代码

当前帧不可变点云数据
当前帧插入的 Submap 列表
后续创建 PoseGraph Node 所需的数据

关系如下：

复制代码

有效点云
    ↓
LocalTrajectoryBuilder2D
    ↓
MatchingResult
    ↓
是否通过 MotionFilter？
    ├── 否：不插入 Submap，不生成 Node
    └── 是：生成 InsertionResult
                 ↓
             PoseGraph2D::AddNode()

所以更准确的说法是：

复制代码

一帧有效 Local SLAM 结果，不一定对应一个 PoseGraph Node。

只有真正插入 Submap 的帧，才会变成 Node。

十二、CollatedTrajectoryBuilder：外部传感器的第一站

12.1 为什么不能直接处理激光

现实系统中，IMU、Odom、LiDAR 不会严格同步到达。

例如：

复制代码

10.000 s：IMU
10.005 s：IMU
10.010 s：Odom
10.020 s：LiDAR
10.021 s：IMU
10.030 s：Odom

如果 LiDAR 在 10.020 秒到达时立刻进入 Local SLAM，但对应时刻附近的 IMU 或 Odom 数据还未进入系统，那么 PoseExtrapolator 就可能无法获得完整的预测信息。

这会造成：

复制代码

当前帧预测位姿不准确
扫描匹配初值变差
局部匹配更容易陷入错误位置
地图出现错位
轨迹发生抖动

因此，Cartographer 在外层使用 CollatedTrajectoryBuilder 先对不同类型传感器数据进行时间排序。

它的目标不是让所有数据物理上完全同步，而是尽量保证：

复制代码

较早时间的数据先处理；
避免明显的时间倒序；
避免关键传感器数据尚未到达时过早处理激光。

12.2 CollatedTrajectoryBuilder 的内部对象关系

复制代码

CollatedTrajectoryBuilder
    │
    ├── trajectory_id
    ├── sensor_collator_
    ├── expected_sensor_ids
    └── wrapped_trajectory_builder_
            ↓
        GlobalTrajectoryBuilder

它内部最重要的成员是：

复制代码

std::unique_ptr<TrajectoryBuilderInterface>
    wrapped_trajectory_builder_;

这个 wrapped_trajectory_builder_ 实际上就是 GlobalTrajectoryBuilder。

因此外部传感器数据先进入：

复制代码

CollatedTrajectoryBuilder

排序完成后，才被转发给：

复制代码

GlobalTrajectoryBuilder

12.3 CollatedTrajectoryBuilder 构造函数

复制代码

CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
    const proto::TrajectoryBuilderOptions& trajectory_options,
    sensor::CollatorInterface* sensor_collator,
    const int trajectory_id,
    const std::set<SensorId>& expected_sensor_ids,
    std::unique_ptr<TrajectoryBuilderInterface>
        wrapped_trajectory_builder)
    : sensor_collator_(sensor_collator),
      trajectory_id_(trajectory_id),
      wrapped_trajectory_builder_(
          std::move(wrapped_trajectory_builder)) {

  absl::flat_hash_set<std::string>
      expected_sensor_id_strings;

  for (const auto& sensor_id : expected_sensor_ids) {
    expected_sensor_id_strings.insert(sensor_id.id);
  }

  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_->AddTrajectory(...)

这句代码是在告诉 Collator：

"当前 trajectory 预计会收到哪些传感器的数据；当某条数据可以安全放行时，就调用 HandleCollatedSensorData()。"

假设当前 trajectory 需要：

复制代码

scan
imu
odom

那么排序器会维护类似如下的队列结构：

复制代码

trajectory 0:
    scan queue
    imu queue
    odom queue

每个 Topic 到来的消息，都先进入对应队列。

等排序器确认时间顺序合理后，才会继续向后分发。

12.4 传感器数据如何进入 Collator

复制代码

void CollatedTrajectoryBuilder::AddData(
    std::unique_ptr<sensor::Data> data) {

  sensor_collator_->AddSensorData(
      trajectory_id_,
      std::move(data));
}

这里没有扫描匹配，没有 TF 查询，也没有地图更新。

它只做一件事：

复制代码

把当前数据放进时间排序器。

从外部角度看，路径是：

复制代码

AddSensorData(...)
    ↓
封装为 sensor::Data
    ↓
CollatedTrajectoryBuilder::AddData(...)
    ↓
Collator::AddSensorData(...)
    ↓
进入对应传感器队列

12.5 排序完成后如何转发

当 Collator 判定某条数据可以放行后，会触发回调：

复制代码

void CollatedTrajectoryBuilder::HandleCollatedSensorData(
    const std::string& sensor_id,
    std::unique_ptr<sensor::Data> data) {

  // 统计当前传感器数据频率。
  UpdateRateStatistic(sensor_id, data->GetTime());

  // 根据真实数据类型，调用内部 GlobalTrajectoryBuilder。
  data->AddToTrajectoryBuilder(
      wrapped_trajectory_builder_.get());
}

关键是：

复制代码

data->AddToTrajectoryBuilder(
    wrapped_trajectory_builder_.get());

这是 C++ 多态分发。

如果 data 实际是激光点云：

复制代码

TimedPointCloudData

它会调用：

复制代码

GlobalTrajectoryBuilder::AddSensorData(
    sensor_id,
    TimedPointCloudData)

如果是 IMU：

复制代码

ImuData

它会调用：

复制代码

GlobalTrajectoryBuilder::AddSensorData(
    sensor_id,
    ImuData)

如果是 Odom：

复制代码

OdometryData

它会调用：

复制代码

GlobalTrajectoryBuilder::AddSensorData(
    sensor_id,
    OdometryData)

这就是 Cartographer 使用统一传感器接口处理多种消息类型的方式。CollatedTrajectoryBuilder 会向 Collator 注册 trajectory 的预期传感器与回调，并在数据被排序后将其分发到内部 TrajectoryBuilder。

十三、Collator 与 TrajectoryCollator 的区别

MapBuilder 创建排序器时，可能使用：

复制代码

sensor::Collator

或者：

复制代码

sensor::TrajectoryCollator

二者目标相同：让传感器数据尽量按正确时间顺序进入系统。

区别在于它们管理队列的范围不同。

Collator 使用一个全局排序器。不同 trajectory 的传感器队列可能都在同一个全局队列结构中。

复制代码

trajectory 0:
    scan / imu / odom

trajectory 1:
    scan / imu / odom

所有队列共同参与全局排序

TrajectoryCollator 则为每条 trajectory 分配独立排序器。

复制代码

trajectory 0：
    独立 scan / imu / odom 排序器

trajectory 1：
    独立 scan / imu / odom 排序器

对于单机器人、单 trajectory 场景，两者通常不会带来特别明显的区别。

但在多机器人、多轨迹、离线加载多个建图任务时，TrajectoryCollator 的隔离性更好：trajectory 0 某个传感器卡住，不一定会影响 trajectory 1。

十四、为什么 Collator 会等待数据

假设机器人传感器频率如下：

复制代码

IMU：100 Hz
Odom：50 Hz
LiDAR：10 Hz

某个时刻三个队列的队头可能是：

复制代码

IMU  队头：10.020 s
Odom 队头：10.018 s
LiDAR 队头：10.030 s

此时 LiDAR 的 10.030 秒数据不能简单立刻处理，因为系统还需要确认：IMU 或 Odom 是否会补来一条更早的消息，例如 10.025 秒。

可以将"当前安全可放行时间"的概念理解为：

复制代码

t_{\text{dispatch}}
=
\min
\left(
t_{\text{head}}^{\text{imu}},
t_{\text{head}}^{\text{odom}},
t_{\text{head}}^{\text{lidar}},
\ldots
\right)

这个公式表达的是一个直观概念：系统不能只看 LiDAR 自己的时间，而要看所有参与排序的传感器队列状态。

实际的 OrderedMultiQueue 还会处理空队列、阻塞队列、传感器结束、未知时间戳等复杂情况，但从工程理解上，可以把它视为：

复制代码

确认当前数据前面不会再有更早数据插进来，
再允许它进入后续 SLAM 模块。

因此，如果你配置了 IMU 或 Odom，但对应 Topic 没有数据，Cartographer 可能表现为：

复制代码

LiDAR 已经有数据
但 Local SLAM 没有输出
轨迹不更新
地图不增长
系统像卡住一样

此时首先应该检查的不是扫描匹配，而是：

复制代码

IMU Topic 是否正常发布
Odom Topic 是否正常发布
传感器时间戳是否单调递增
传感器配置名称是否与实际 Topic 对应
Collator 是否在等待某个 expected sensor

十五、ROS 数据如何真正进入 Cartographer

Cartographer Core 不直接认识 ROS 的：

复制代码

sensor_msgs::LaserScan
sensor_msgs::PointCloud2
sensor_msgs::Imu
nav_msgs::Odometry

ROS 数据需要由 cartographer_ros 中的桥接层转换。

核心对象是：

复制代码

MapBuilderBridge
SensorBridge

MapBuilderBridge 负责 ROS 系统和 Cartographer Core 之间的整体管理。

SensorBridge 负责把具体 ROS 消息转换成 Cartographer 内部数据。

15.1 MapBuilderBridge 创建 SensorBridge

复制代码

int MapBuilderBridge::AddTrajectory(
    const std::set<SensorId>& expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {

  const int trajectory_id =
      map_builder_->AddTrajectoryBuilder(
          expected_sensor_ids,
          trajectory_options.trajectory_builder_options,
          local_slam_result_callback);

  sensor_bridges_[trajectory_id] =
      absl::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.ignore_out_of_order_messages,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec,
          tf_buffer_,
          map_builder_->GetTrajectoryBuilder(trajectory_id));

  return trajectory_id;
}

这段代码说明：

SensorBridge 内部保存的 trajectory builder 指针，来自：

复制代码

map_builder_->GetTrajectoryBuilder(trajectory_id)

而这个对象就是最外层的：

复制代码

CollatedTrajectoryBuilder

所以 ROS 数据路径为：

复制代码

ROS 回调
    ↓
SensorBridge
    ↓
CollatedTrajectoryBuilder

不是：

复制代码

ROS 回调
    ↓
LocalTrajectoryBuilder2D

Cartographer ROS 的 MapBuilderBridge 负责创建 trajectory，并把对应 trajectory builder 交给 SensorBridge 作为 ROS 传感器数据的输入目标。

十六、PointCloud2 如何进入 Cartographer

16.1 ROS 点云首先被转换

复制代码

void SensorBridge::HandlePointCloud2Message(
    const std::string& sensor_id,
    const sensor_msgs::PointCloud2::ConstPtr& msg) {

  PointCloudWithIntensities point_cloud;
  common::Time time;

  std::tie(point_cloud, time) =
      ToPointCloudWithIntensities(*msg);

  HandleRangefinder(
      sensor_id,
      time,
      msg->header.frame_id,
      point_cloud.points);
}

这段代码主要做两件事。

第一，将 ROS PointCloud2 转成 Cartographer 内部点云格式。

第二，调用 HandleRangefinder()，准备进行 TF 变换并构造 TimedPointCloudData。

16.2 为什么点云中每个点都需要带时间

一帧 LiDAR 点云不是同一瞬间采集完成的。

例如 10 Hz 雷达，一圈扫描周期约为 0.1 秒。机器人在这 0.1 秒内可能前进、转弯或晃动。

因此，早采到的点与晚采到的点对应不同机器人姿态。

设整帧点云结束时刻为：

复制代码

t_{\text{end}}

第 iii 个点相对结束时刻的偏移为：

复制代码

\Delta t_i \leq 0

那么该点真实采样时刻为：

复制代码

t_i
=
t_{\text{end}}
+
\Delta t_i

其中：

最后一个采样点一般满足 Δti≈0\Delta t_i \approx 0Δti≈0；
早期点一般满足 Δti<0\Delta t_i < 0Δti<0；
Local SLAM 后续会利用 tit_iti 查询或预测机器人当时的位姿；
这样才能把一帧扫描中的点变换到统一时刻，减少运动畸变。

这就是点云去畸变的时间基础。

16.3 SensorBridge 查询 TF 并变换点云

LiDAR 数据原本处于自身坐标系。

例如：

复制代码

laser_link
velodyne
front_lidar_link
base_scan

而 Cartographer Local SLAM 通常围绕 tracking_frame 工作。

例如：

复制代码

base_link
imu_link
base_imu_link

因此，SensorBridge 需要查询：

复制代码

sensor frame → tracking frame

核心逻辑如下：

复制代码

const auto sensor_to_tracking =
    tf_bridge_.LookupToTracking(
        time,
        CheckNoLeadingSlash(frame_id));

trajectory_builder_->AddSensorData(
    sensor_id,
    sensor::TimedPointCloudData{
        time,

        // 雷达原点转换到 tracking frame。
        sensor_to_tracking->translation().cast<float>(),

        // 全部点云转换到 tracking frame。
        sensor::TransformTimedPointCloud(
            ranges,
            sensor_to_tracking->cast<float>())
    });

坐标变换可以表示为：

复制代码

\mathbf{p}_{T}
=
\mathbf{T}_{T \leftarrow S}
\mathbf{p}_{S}

其中：

pS\mathbf{p}_{S}pS：激光点在雷达坐标系中的坐标；
TT←S\mathbf{T}_{T \leftarrow S}TT←S：从雷达坐标系到 tracking frame 的刚体变换；
pT\mathbf{p}_{T}pT：转换后的点在 tracking frame 中的坐标。

展开成旋转和平移形式：

复制代码

\mathbf{p}_{T}
=
\mathbf{R}_{T \leftarrow S}
\mathbf{p}_{S}
+
\mathbf{t}_{T \leftarrow S}

如果雷达到 tracking frame 的外参错误，例如雷达 yaw 写反、平移长度写错、TF 父子关系反了，那么后续扫描匹配即使正常运行，地图也会出现扭曲、双层墙、轨迹漂移或点云错位。

Cartographer ROS 的 SensorBridge 会将 ROS 点云转换为内部点云数据，并查询传感器坐标系到 tracking frame 的 TF 后再送入 trajectory builder。

十七、IMU 如何进入 Cartographer

IMU 通常提供：

复制代码

三轴角速度
三轴线加速度
可选姿态四元数
协方差

Cartographer 重点使用的是：

复制代码

angular_velocity
linear_acceleration

IMU 原始数据也必须先转换到 tracking frame。

复制代码

sensor::ImuData{
    time,

    // 加速度旋转到 tracking frame。
    sensor_to_tracking->rotation() *
        ToEigen(msg->linear_acceleration),

    // 角速度旋转到 tracking frame。
    sensor_to_tracking->rotation() *
        ToEigen(msg->angular_velocity)
}

对于向量，只需要旋转，不需要平移。

复制代码

\mathbf{a}_{T}
=
\mathbf{R}_{T \leftarrow I}
\mathbf{a}_{I}

\boldsymbol{\omega}_{T}
=
\mathbf{R}_{T \leftarrow I}
\boldsymbol{\omega}_{I}

其中：

aI\mathbf{a}_{I}aI：IMU 坐标系中的线加速度；
ωI\boldsymbol{\omega}_{I}ωI：IMU 坐标系中的角速度；
RT←I\mathbf{R}_{T \leftarrow I}RT←I：IMU 到 tracking frame 的旋转矩阵；
aT\mathbf{a}{T}aT、ωT\boldsymbol{\omega}{T}ωT：转换到 tracking frame 后的数据。

17.1 IMU 进入 GlobalTrajectoryBuilder 后走两条路

复制代码

void GlobalTrajectoryBuilder::AddSensorData(
    const std::string& sensor_id,
    const sensor::ImuData& imu_data) {

  // Local SLAM：用于当前时刻位姿预测。
  if (local_trajectory_builder_) {
    local_trajectory_builder_->AddImuData(imu_data);
  }

  // PoseGraph：保存后端优化需要的 IMU 数据。
  pose_graph_->AddImuData(
      trajectory_id_,
      imu_data);
}

这段代码说明 IMU 同时服务于前端和后端。

送入 LocalTrajectoryBuilder2D：

用于 PoseExtrapolator。它帮助预测当前激光帧到来时机器人可能的姿态，尤其是 yaw、roll、pitch 等旋转状态。

送入 PoseGraph2D：

用于后端保存 IMU 相关数据，后续全局优化会综合激光局部约束、回环约束、里程计、GPS 与 IMU 信息。

因此，IMU 不是只服务于去畸变，也不是只服务于后端，而是同时影响前端预测与后端约束。

十八、Odom 如何进入 Cartographer

轮速里程计或外部里程计通常使用 ROS 消息：

复制代码

nav_msgs::Odometry

它通常表达：

复制代码

机器人 child_frame 在 odom frame 下的位姿

例如：

复制代码

odom → base_link

但 Cartographer 的 tracking frame 不一定是 base_link，也可能是 imu_link。

因此，SensorBridge 需要将 Odom 转换成：

复制代码

tracking_frame 在 odom frame 下的位姿

sensor::OdometryData{
    time,

    ToRigid3d(msg->pose.pose) *
        sensor_to_tracking->inverse()
}

可表示为：

复制代码

\mathbf{T}_{O \leftarrow T}
=
\mathbf{T}_{O \leftarrow B}
\mathbf{T}_{B \leftarrow T}

其中：

OOO：odom frame；
BBB：base_link；
TTT：tracking frame；
TO←T\mathbf{T}_{O \leftarrow T}TO←T：tracking frame 在 odom 坐标系下的位姿。

18.1 Odom 同样会分两路

复制代码

void GlobalTrajectoryBuilder::AddSensorData(
    const std::string& sensor_id,
    const sensor::OdometryData& odometry_data) {

  // 前端：辅助短时间位姿预测。
  if (local_trajectory_builder_) {
    local_trajectory_builder_->AddOdometryData(
        odometry_data);
  }

  // 后端：保存 Odom 约束。
  pose_graph_->AddOdometryData(
      trajectory_id_,
      odometry_data);
}

Odom 对 Local SLAM 的作用是提供较稳定的短时间运动先验。

例如机器人从上一帧到当前帧大约移动了 0.2 米、旋转了 3 度，Odom 可以给扫描匹配提供一个初始猜测。

但 Odom 不会直接覆盖 Cartographer 位姿。

更准确的关系是：

复制代码

Odom：提供预测初值
    ↓
LiDAR 扫描匹配：修正局部位姿
    ↓
PoseGraph 回环优化：修正全局位姿

因此，最终 Cartographer 轨迹一般不是纯 Odom 轨迹，也不是纯 LiDAR 轨迹，而是多种约束融合后的结果。

十九、RangeDataCollator：多颗 LiDAR 的同步器

这里必须把 RangeDataCollator 与 CollatedTrajectoryBuilder 区分开。

CollatedTrajectoryBuilder 处理的是不同传感器类型：

复制代码

LiDAR
IMU
Odom
GPS
Landmark

它解决的问题是：

复制代码

不同类型传感器的消息应该按什么时间顺序进入系统。

而 RangeDataCollator 处理的是多个 LiDAR：

复制代码

front_lidar
rear_lidar
left_lidar
right_lidar

它解决的问题是：

复制代码

多颗 LiDAR 的点云如何融合成一批时间一致的 range data。

二者关系如下：

复制代码

CollatedTrajectoryBuilder：
系统级时间排序
    ↓
GlobalTrajectoryBuilder
    ↓
LocalTrajectoryBuilder2D
    ↓
RangeDataCollator：
多 LiDAR 点云同步、裁剪、合并

19.1 RangeDataCollator 的入口

复制代码

auto synchronized_data =
    range_data_collator_.AddRangeData(
        sensor_id,
        unsynchronized_data);

if (synchronized_data.ranges.empty()) {
  return nullptr;
}

假设机器人有两颗雷达：

复制代码

front_lidar
rear_lidar

当前先收到：

复制代码

front_lidar：10.000 s

后方雷达对应数据还没到时，系统可能无法形成完整同步范围，因此返回空。

等收到：

复制代码

rear_lidar：10.005 s

之后，RangeDataCollator 才会根据两颗雷达的时间信息，取出一个共同的处理窗口，并将两颗雷达在该时间范围内的点融合。

19.2 多雷达时间窗口

设当前融合时间窗口为：

复制代码

I_k
=
\left(
t_{\text{start}},
t_{\text{end}}
\right]

只有点采样时刻满足：

复制代码

t_{\text{start}}
<
t_i
\leq
t_{\text{end}}

的点才会保留。

其中每个点真实采样时刻为：

复制代码

t_i
=
t_{\text{cloud}}
+
\Delta t_i

这里：

tcloudt_{\text{cloud}}tcloud：当前点云结束时刻；
Δti\Delta t_iΔti：点相对结束时间；
tit_iti：点真实采样时刻。

随后，来自不同雷达的点会被合并，并统一到相同时间参考下。

因此，RangeDataCollator 输出的并不是"前雷达一帧点云"或"后雷达一帧点云"，而是一批融合后的、时间一致的 range data。

二十、从 PointCloud2 到 PoseGraph Node 的完整追踪

下面以一帧 /points_raw 为例，将完整过程串起来。

复制代码

1. ROS 收到 sensor_msgs::PointCloud2
        ↓
2. SensorBridge::HandlePointCloud2Message()
        ↓
3. PointCloud2 转为 PointCloudWithIntensities
        ↓
4. 查询 sensor frame → tracking frame 的 TF
        ↓
5. 构造 TimedPointCloudData
        ↓
6. trajectory_builder_->AddSensorData(...)
        ↓
7. CollatedTrajectoryBuilder 接收数据
        ↓
8. Collator / TrajectoryCollator 时间排序
        ↓
9. HandleCollatedSensorData(...)
        ↓
10. GlobalTrajectoryBuilder::AddSensorData(...)
        ↓
11. LocalTrajectoryBuilder2D::AddRangeData(...)
        ↓
12. RangeDataCollator 多雷达同步与融合
        ↓
13. PoseExtrapolator 位姿预测
        ↓
14. 点云去畸变、滤波
        ↓
15. 实时扫描匹配
        ↓
16. ActiveSubmaps2D 插入
        ↓
17. MatchingResult
        ↓
18. InsertionResult
        ↓
19. PoseGraph2D::AddNode(...)
        ↓
20. 创建 Node 与 Submap 局部约束

这条调用链是后续分析 Cartographer 所有模块的基础。

后续第 2 篇会从第 11 步开始，重点分析：

复制代码

LocalTrajectoryBuilder2D
    ↓
PoseExtrapolator
    ↓
RangeDataCollator
    ↓
实时扫描匹配
    ↓
局部位姿输出

二十一、总结

Cartographer 的系统入口并不是单一函数，也不是"订阅到一帧激光后立刻进行扫描匹配"这样简单的过程。它实际上是一条层次明确、职责分离的数据处理链。只有先理解这条链，后续阅读 LocalTrajectoryBuilder、Submap、ConstraintBuilder、PoseGraph 和 OptimizationProblem 时，才不会把不同模块的职责混在一起。

整个系统最上层的核心对象是 MapBuilder。MapBuilder 可以理解为 Cartographer 的总装配器。它本身不负责处理一帧激光，也不直接执行扫描匹配或回环检测，而是根据配置创建整个 SLAM 系统所需的公共对象。对于 2D 模式，MapBuilder 会创建 PoseGraph2D、OptimizationProblem2D、后台线程池以及传感器时间排序器。PoseGraph2D 是后端管理器，它保存 Node、Submap、局部约束、回环约束，并在合适时机触发全局优化。OptimizationProblem2D 则是更底层的优化问题组织器，它负责把 Node 位姿、Submap 位姿、各种 Constraint 转换为可交给 Ceres 求解器处理的优化变量和残差项。线程池的存在说明 Cartographer 前端与后端并不是完全串行的关系：前端 Local SLAM 尽量实时运行，而回环检测、约束构建和全局优化通常放在后台异步执行。

当系统需要开始一段建图、定位或 rosbag 回放任务时，会创建一条 trajectory。trajectory 并不只是 RViz 中显示的一条路径线，而是一整套连续运行的 SLAM 任务状态。每条 trajectory 都有自己的 LocalTrajectoryBuilder2D、自己的 IMU 与 Odom 缓存、自己的 PoseExtrapolator、自己的 ActiveSubmaps2D 和自己的 Node 序列。不同 trajectory 可以代表不同机器人、不同建图阶段，或者不同 rosbag 数据段。尽管它们的 Local SLAM 状态独立，但多条 trajectory 可以共享一个 PoseGraph2D，因此后端可以通过约束将不同 trajectory 对齐到统一全局地图中。

MapBuilder 的 AddTrajectoryBuilder() 是创建一条 trajectory 的核心函数。这个函数首先创建 LocalTrajectoryBuilder2D。LocalTrajectoryBuilder2D 是 Cartographer 的 Local SLAM 前端，负责实时位姿预测、点云去畸变、点云滤波、实时扫描匹配和局部 Submap 更新。之后，系统创建 GlobalTrajectoryBuilder。GlobalTrajectoryBuilder 并不是 Global SLAM 后端，而是前端与后端之间的连接器。它负责将 LiDAR 数据送入 LocalTrajectoryBuilder2D，同时把 IMU 和 Odom 同时送给 Local SLAM 与 PoseGraph；当 Local SLAM 处理完当前帧并插入 Submap 后，它又会调用 PoseGraph2D::AddNode()，将当前帧正式加入后端图优化系统。

但系统对外真正暴露的对象并不是 LocalTrajectoryBuilder2D，也不是 GlobalTrajectoryBuilder，而是 CollatedTrajectoryBuilder。这个设计非常重要。真实机器人上的 LiDAR、IMU、Odom 不会严格同步到达，ROS 通信、驱动延迟和回调调度都可能造成消息先后顺序不一致。如果 LiDAR 一到就直接执行扫描匹配，而相关时刻的 IMU 或 Odom 尚未进入系统，那么 PoseExtrapolator 就可能缺少足够数据，导致预测位姿不可靠，进而影响扫描匹配初值。因此，Cartographer 在最外层使用 CollatedTrajectoryBuilder，对不同类型传感器数据进行缓存和时间排序。它会将数据交给 Collator 或 TrajectoryCollator，在确认不会出现明显早到或晚到的数据插队后，再将消息转发给内部的 GlobalTrajectoryBuilder。

这也是为什么配置中声明某个传感器后，必须保证该传感器 Topic 真正稳定发布。如果配置里存在 IMU，但 IMU Topic 没有数据，或者时间戳不递增，那么 Collator 可能会一直等待，导致看起来 LiDAR 已经发布但地图不更新。很多 Cartographer"卡住"的问题，根源并不一定是扫描匹配，而可能是时间排序器正在等待 expected sensor。工程调试时，应该同时检查 LiDAR、IMU、Odom 的频率、时间戳、TF、Topic 名和传感器配置，而不能只盯着激光数据。

ROS 数据进入 Cartographer Core 前，还需要经过 SensorBridge。SensorBridge 是 ROS 消息与 Cartographer 内部数据结构之间的转换器。对于 PointCloud2 或 LaserScan，SensorBridge 会先转换为 Cartographer 内部点云格式，再查询传感器坐标系到 tracking frame 的 TF，将点云变换到统一坐标系，最后构造 TimedPointCloudData。TimedPointCloudData 不仅包含点云坐标，还包含每个点的相对采样时间。这一点非常关键，因为 LiDAR 一帧扫描往往持续几十毫秒甚至上百毫秒，机器人在扫描期间可能已经发生平移和旋转。如果不知道每个点真实采样时刻，就无法准确估计每个点对应的机器人姿态，也难以完成有效的点云去畸变。

IMU 与 Odom 的处理也遵循类似思路。IMU 中的线加速度和角速度需要转换到 tracking frame，之后会同时进入 LocalTrajectoryBuilder2D 和 PoseGraph2D。Local SLAM 使用 IMU 来帮助 PoseExtrapolator 预测当前激光帧的姿态和运动趋势；PoseGraph 则保存 IMU 数据，为后续全局优化提供约束。Odom 也是如此：前端利用 Odom 给扫描匹配提供运动初值，后端则把它作为外部运动约束之一。需要明确的是，IMU 和 Odom 都不会直接覆盖 Cartographer 的最终位姿。Local SLAM 的最终局部位姿仍由扫描匹配修正，而全局位姿还会在回环检测和 PoseGraph 优化之后发生调整。

此外，Cartographer 中还有一个容易与 CollatedTrajectoryBuilder 混淆的模块：RangeDataCollator。二者名称相近，但处理层级完全不同。CollatedTrajectoryBuilder 负责 LiDAR、IMU、Odom、GPS、Landmark 等不同类型传感器之间的时间排序；RangeDataCollator 则只处理多颗 LiDAR 的点云融合。例如机器人同时安装前向雷达和后向雷达时，两个雷达消息到达时间可能略有不同，RangeDataCollator 会在 LocalTrajectoryBuilder2D 内部缓存它们、按共同时间窗口裁剪点、统一相对时间并合并为一批同步 range data。之后，这一批融合点云才会进入去畸变、扫描匹配和 Submap 插入流程。

最后，还必须理解 Node 的生成机制。并不是每帧 LiDAR 都会生成一个 PoseGraph Node。每一批有效 range data 经 LocalTrajectoryBuilder2D 处理后，可能产生 MatchingResult；MatchingResult 表示当前批次点云已经得到局部位姿估计。但若机器人移动很小、旋转很小，或者当前帧被 MotionFilter 判断为信息增量不足，那么这一帧不会插入 ActiveSubmaps2D，也不会产生 InsertionResult。只有当前帧真正插入 Submap 时，GlobalTrajectoryBuilder 才会调用 PoseGraph2D::AddNode()，将该帧变成后端图优化中的 Node，并与当前 Submap 建立局部约束。

因此，Cartographer 的完整入口可以概括为：MapBuilder 创建 SLAM 系统；MapBuilderBridge 管理 ROS 与 Core 的关系；SensorBridge 负责 ROS 消息转换与 TF 变换；CollatedTrajectoryBuilder 负责多传感器时间排序；GlobalTrajectoryBuilder 负责连接 Local SLAM 与 PoseGraph；LocalTrajectoryBuilder2D 负责实时定位和局部建图；PoseGraph2D 负责保存 Node、Submap、局部约束、回环约束并进行全局优化。只要明确当前函数属于哪一层，处理的是传感器数据、局部位姿、Submap、Node 还是全局约束，Cartographer 的源码结构就会清晰很多。