第 3 篇结束后，当前激光点云已经完成 Local SLAM：去畸变、位姿预测、扫描匹配、MotionFilter 判断，以及 Active Submap 插入。接下来 Global SLAM 要做的不是重新把每帧点云建一遍图，而是把前端认可的局部观测整理成 Node，把局部地图整理成 Submap，再不断建立它们之间的约束，最后统一优化。
这一篇最容易混淆的点有四个：Trajectory 和 Node 是什么关系；Node 里具体存什么；Node 与 Submap 到底怎样建立关系；以及 Cartographer 后端除了局部约束和回环约束外，还有哪些约束参与最终优化。把这些问题弄清楚后，PoseGraph2D 的代码结构就顺了。
整条主线可以先固定为：
Trajectory（一条独立 SLAM 运行实例）
↓
Local SLAM 持续输出 MatchingResult
↓
当前扫描真正插入 Submap
↓
生成 InsertionResult
↓
GlobalTrajectoryBuilder 调用 PoseGraph2D::AddNode(...)
↓
产生 TrajectoryNode
↓
Node ↔ 当前插入 Submap：INTRA_SUBMAP
↓
Node ↔ 历史完成 Submap：尝试 INTER_SUBMAP
↓
Fast Correlative 粗搜索 + Ceres 精修
↓
PoseGraph / OptimizationProblem2D 全局优化需要先纠正一个常见说法：Cartographer 的显式 Constraint 对象 确实主要是 Node-Submap 关系，标签只有 INTRA_SUBMAP 和 INTER_SUBMAP 两类；但真正进入 Ceres 全局优化时，还会额外加入相邻 Node 之间的 Local SLAM 相对位姿残差、Odom 残差、可选的固定坐标系残差和 Landmark 残差。所以可以说 Cartographer 的主图是 Node-Submap 图，但它的最终优化问题并不只有 Node-Submap 两类残差。
一、Trajectory、Node、Submap 的层级关系
Trajectory（轨迹） 不是 RViz 里那条折线，而是一条完整 SLAM 任务的运行容器。一次建图、一次定位、一次重新启动后的独立 SLAM 流，通常都会对应一个 Trajectory。它内部持续维护当前传感器流、Local SLAM 状态、Node、Submap、PoseGraph 状态以及相关后端数据。
因此可以把一条 Trajectory 看成一条"时间主线"，Node 和 Submap 都是在这条主线上不断生成的对象：
Trajectory 0
├── Node 0
├── Node 1
├── Node 2
├── Node 3
├── ...
│
├── Submap 0
├── Submap 1
├── Submap 2
└── ...Node 的编号不是单一整数，而是：
NodeId = [trajectory_id, node_index]
SubmapId = [trajectory_id, submap_index]例如 NodeId{0, 35} 表示第 0 条 Trajectory 中的第 35 个 Node；SubmapId{0, 8} 表示该 Trajectory 的第 8 张局部子图。这样多机器人、多次建图或多 Trajectory 场景中，即使不同轨迹都有 "Node 35"，系统也不会混淆。PoseGraph2D 在追加 Node 时，会将其加入对应 Trajectory，并在发现一个此前未登记的新 Submap 时为其创建对应 SubmapId。
Trajectory 是"整条 SLAM 生命周期"；Node 是这条轨迹上的关键局部观测；Submap 是一段时间内由多个 Node 相关点云共同累积出的局部地图块。它们不是同一层级：Trajectory 包含 Node 和 Submap，Node 与 Submap 则通过约束连接。
二、Node 怎么产生：它不是每一帧 LiDAR
在 Cartographer 2D 中，一帧 LiDAR（激光雷达）点云进入系统后，并不会立刻变成 Node（轨迹节点，后端长期保存并参与全局优化的一次关键观测）。它必须先经过 Local SLAM（局部 SLAM 前端）的位姿预测、点云去畸变、实时扫描匹配和运动筛选；只有这一帧真正被允许写入 ActiveSubmaps2D（当前正在构建的二维子地图）后，才会形成 InsertionResult（插图结果），并进一步交给 PoseGraph（位姿图后端）创建 Node。
整个过程可以概括为：
原始 LiDAR 点云
↓
LocalTrajectoryBuilder2D::AddRangeData(...)
↓
多雷达数据整理 + 位姿预测 + 去畸变 + 扫描匹配
↓
MatchingResult（当前帧局部匹配结果）
↓
MotionFilter（运动过滤器）判断是否需要插图
↓
通过
↓
插入 ActiveSubmaps2D（活动子地图）
↓
InsertionResult（插图结果）
↓
PoseGraph::AddNode(...)
↓
TrajectoryNode（后端轨迹节点）这里最重要的一点是：MatchingResult（扫描匹配结果）只表示"当前帧的局部位姿已经计算出来了"；而 InsertionResult（插图结果）才表示"当前帧真的参与了建图，并且会进入后端成为 Node"。
1．当前帧首先进入 LocalTrajectoryBuilder2D
当前 LiDAR 点云通常会进入：
LocalTrajectoryBuilder2D::AddRangeData(...)LocalTrajectoryBuilder2D（二维局部轨迹构建器）是 Cartographer 2D 前端的核心类，主要负责处理当前帧点云，并估计机器人此刻在局部地图中的位姿。
AddRangeData(...)（添加距离观测数据）并不是简单把一帧点云塞进去，而是把当前雷达观测交给 Local SLAM 进行完整处理。处理后的数据通常会以 RangeData（距离观测数据）的形式存在，其中包含激光原点、命中点、未命中射线以及时间相关信息。
如果机器人装有多个 LiDAR，例如前雷达、后雷达或左右雷达，那么 Cartographer 还会先经过 RangeDataCollator（多雷达数据整理器）。它会缓存不同雷达的数据，并按照时间戳进行整理，使多路雷达数据能够以较合理的时间顺序进入 Local SLAM。
需要注意，RangeDataCollator 做的是软件层面的时间整理，并不代表多个雷达一定在硬件层面同时发射或同时采样。
2．PoseExtrapolator 先预测当前机器人在哪里
在扫描匹配之前，系统必须先给当前帧一个初始位姿。这个初始位姿由 PoseExtrapolator（位姿外推器、位姿预测器）提供。
所谓机器人位姿 pose（位置和朝向），在二维 SLAM 中通常可以写成：
pose = (x, y, yaw)其中，x 和 y 表示机器人在平面地图中的位置；yaw（航向角）表示机器人绕竖直方向旋转的角度，也就是机器人朝向。
PoseExtrapolator 会利用历史 Local SLAM 位姿、IMU（惯性测量单元）、Odometry（轮速里程计或外部里程计）等数据，预测当前时刻机器人的大概位置和朝向。这个预测结果通常可以理解为：
pose_prediction(t)其中，pose_prediction（预测位姿）只是扫描匹配的初始猜测，不是最终结果。
例如，上一帧机器人位于 (10.00 m, 5.00 m, 0°)，轮速里程计显示机器人向前移动了约 0.10 m，那么系统可能先预测当前位姿为：
x = 10.10 m
y = 5.00 m
yaw = 0°随后扫描匹配会以这个预测结果为起点，结合当前 LiDAR 点云和局部地图进一步修正。
3．逐点去畸变：同一帧点云并不是同一时刻采到的
LiDAR 的一帧扫描不是瞬间完成的。假设雷达频率为 10 Hz，那么一帧扫描大约持续 0.1 s。机器人在这一段时间内可能正在前进、转弯或轻微晃动，因此同一帧中不同激光点的真实采样时刻不同。
某个点的采样时刻可以抽象写成：
t_point = t_scan + Δt其中，t_scan（当前帧扫描基准时间）表示这帧点云的参考时刻；Δt（相对时间偏移）表示当前点相对于基准时刻的时间差；t_point（该点真实采样时间）则表示这个激光点真正被测量到的时间。
如果机器人扫描过程中正在运动，而系统不做处理，就会出现墙面弯曲、障碍物边缘拉伸、走廊轮廓错位等问题，这就是点云运动畸变。
因此 Cartographer 会利用 PoseExtrapolator 在各个 t_point 时刻预测机器人位姿，再把不同时间采到的点转换到统一参考时刻或统一局部坐标系中。这个过程通常称为逐点去畸变。
去畸变后的点云，才适合参与后续的局部地图匹配。
4．实时扫描匹配后得到 MatchingResult
完成位姿预测和去畸变后，Cartographer 会进行实时扫描匹配。扫描匹配的目标是：寻找一个更准确的机器人位姿，使当前帧点云投影到局部地图后，能够尽可能和已有墙体、障碍物边缘、走廊结构和空闲区域重合。
其核心思想可以抽象为：
T* = argmin Σ residual(当前帧点云, 局部子地图)其中，T*（最优坐标变换）表示扫描匹配最终求出的机器人位姿；residual（残差、匹配误差）表示当前帧点云与局部地图之间的不一致程度；argmin 表示寻找总误差最小的那组位姿参数。
扫描匹配完成后，Local SLAM 会输出 MatchingResult（扫描匹配结果）。它通常包含当前时间戳、局部位姿、已经转换到局部坐标系的点云，以及是否存在插图结果。
可以概念性理解为：
MatchingResult {
time; // 当前观测时间戳
local_pose; // 当前帧在局部坐标系中的估计位姿
range_data_in_local; // 转到局部坐标系后的点云
insertion_result; // 插图结果，可能为空
}其中，local_pose（局部位姿）表示当前帧经过扫描匹配后得到的位置和朝向；range_data_in_local（局部坐标系下的距离数据）表示已经完成坐标变换和去畸变的当前帧点云。
这一步只说明：当前机器人此刻大概在哪里，已经被 Local SLAM 算出来了。
但它还不能说明：这一帧一定会成为 Node。
5．MotionFilter 决定这一帧是否值得插图
即使扫描匹配成功，机器人也可能几乎没有运动。例如机器人静止时，LiDAR 仍然会持续发布点云，系统仍然会持续计算当前位姿，但连续多帧点云几乎完全相同。
例如：
上一次有效插图：
x = 12.350 m
y = 8.720 m
yaw = 91.0°
当前扫描匹配结果：
x = 12.354 m
y = 8.721 m
yaw = 91.1°从定位角度看，当前帧仍然有效，因为系统知道机器人还在原来的位置附近；但从建图角度看，这一帧几乎没有新增环境信息。如果每帧都写入地图，就会不断重复更新同一批栅格，也会不断生成重复 Node，导致后端计算量快速增加。
因此 Cartographer 会通过 MotionFilter（运动过滤器）判断当前帧是否值得插图。
它通常会检查当前帧相对于上一次有效插图之间的三个变化量：
Δt = t_current - t_last_insertion
Δd = √((x_current - x_last)² + (y_current - y_last)²)
Δθ = |yaw_current - yaw_last|其中，Δt（时间差）表示当前帧距离上一次有效插图过去了多久；Δd（平移距离差）表示机器人相对上一次插图移动了多远；Δθ（角度差）表示机器人相对上一次插图旋转了多大角度。
x_current、y_current 和 yaw_current 来自当前扫描匹配得到的 local_pose；x_last、y_last 和 yaw_last 则来自上一次真正插入 Submap 时记录的位姿。
MotionFilter 常见的配置参数包括：
max_time_seconds
max_distance_meters
max_angle_radiansmax_time_seconds（最大时间阈值）表示最长允许多久不插图；max_distance_meters（最大距离阈值）表示机器人至少移动多远才需要插图；max_angle_radians（最大角度阈值）表示机器人至少旋转多大角度才需要插图。
如果当前帧满足"时间很近、移动很小、转角也很小"，MotionFilter 就会认为当前帧和上次插图高度重复，于是过滤掉当前帧。
此时会发生的情况是：
扫描匹配成功
↓
得到当前 local_pose
↓
MotionFilter 认为运动不明显
↓
当前帧不插入 Submap
当前帧不生成 InsertionResult
当前帧不创建 Node但是，当前的 local_pose 仍然可以继续输出给上层定位模块、RViz 或控制模块使用。
所以要注意：
被 MotionFilter 过滤的帧，不是"定位失败帧"，而是"定位结果存在，但不值得重复建图的帧"。
6．真正插入 ActiveSubmaps2D 后，才会产生 InsertionResult
若当前帧通过 MotionFilter，系统就会把当前点云插入 ActiveSubmaps2D（活动二维子地图集合）。
Cartographer 不会只维护一张无限扩大的大地图，而是把环境划分为多个 Submap（子地图）。旧 Submap 在积累足够多的点云后会完成并冻结，不再接收新观测；新的 Submap 则继续接收当前帧数据。
通常系统会同时维护两个 Active Submap，使相邻子地图之间存在重叠区域。例如当前帧可能同时插入：
Submap 5
Submap 6这样做的目的，是让相邻子地图共享一部分环境观测，后续 PoseGraph 才能更稳定地建立局部约束和全局约束。
当前帧写入 Submap 时，更新的并不是"原始点云文件"，而是栅格地图中的占据概率。激光击中的位置会更新为 occupied cells（占据栅格，可能存在墙体、障碍物或货架）；激光射线经过但没有击中的区域会更新为 free-space cells（空闲栅格，通常表示可通行空间）。
因此，Submap 保存的是环境的空间结构，而不是简单叠加一帧一帧的原始点云。
当当前帧真正插入 ActiveSubmaps2D 后，系统会生成 InsertionResult（插图结果）。它可以理解为：本次关键插图已经完成，当前帧具备进入后端的条件。
它通常包含两类核心数据：
InsertionResult {
constant_data; // 当前观测的长期保存数据
insertion_submaps; // 当前帧插入过哪些 Submap
}其中，constant_data（不可变观测数据）通常包含当前帧时间戳、局部位姿、重力方向信息、降采样后的点云等。它不是原始 LiDAR 消息的完整拷贝，而是后端进行回环检测、约束构建和全局优化真正需要保留的数据。
insertion_submaps（插入子地图集合）表示当前帧被写入了哪些子地图。例如：
insertion_submaps = {Submap 5, Submap 6}表示当前帧同时更新了第 5 张和第 6 张子地图。后端后续会根据这个关系，建立当前 Node 与这些 Submap 之间的局部约束。
7．PoseGraph::AddNode 才是 Node 正式创建的位置
Local SLAM 返回 MatchingResult 后，GlobalTrajectoryBuilder（全局轨迹构建器）会检查当前帧是否存在 insertion_result。
核心代码逻辑可以概括为：
matching_result =
local_trajectory_builder_->AddRangeData(...);
if (matching_result->insertion_result != nullptr) {
pose_graph_->AddNode(
matching_result->insertion_result->constant_data,
trajectory_id,
matching_result->insertion_result->insertion_submaps);
}local_trajectory_builder_（当前轨迹对应的 Local SLAM 前端对象）负责处理当前帧点云；matching_result（当前帧扫描匹配结果）保存前端输出。
下面这句最关键：
matching_result->insertion_result != nullptrnullptr（空指针）表示当前帧没有产生插图结果。也就是说，当前帧虽然可能完成了扫描匹配，但它没有真正插入 Submap，因此不能进入后端创建 Node。
只有 insertion_result 非空时，才会调用：
pose_graph_->AddNode(...)pose_graph_（PoseGraph 后端对象）负责维护 Node、Submap、局部约束、回环约束和全局优化关系。AddNode(...) 表示：将当前这一帧关键观测正式加入后端位姿图。
传入的三个主要参数分别是：
constant_dataconstant_data（当前节点的长期观测数据）包含当前帧时间、局部位姿、处理后的点云等信息，后续回环检测和约束构建会使用这些数据。
trajectory_idtrajectory_id（轨迹编号）表示当前 Node 属于哪一条轨迹。单机器人一次建图时通常只有一条轨迹；多机器人、多次建图或重新定位场景中，可能同时存在多条轨迹，因此需要编号区分。
insertion_submapsinsertion_submaps（当前节点插入过的子地图集合）表示该 Node 与哪些 Submap 存在直接观测关系。后端会根据这些关系建立 Node 与 Submap 之间的局部约束。
例如，Node 120 插入了 Submap 5 和 Submap 6，那么后端会记录：
Node 120 ↔ Submap 5
Node 120 ↔ Submap 6这些约束会参与后续 PoseGraph 优化。即使后面发生回环检测、地图整体被拉回或修正，Node 和 Submap 之间的局部关系仍然能够帮助系统保持地图局部结构的一致性。
8．Node 保存的是什么，和栅格地图有什么区别
Node 不是某个栅格，也不是一帧原始 LiDAR 点云的简单副本。
Node 更准确地说是：
机器人在某一个关键时刻，利用一帧有效观测完成局部定位并参与建图后，后端保存下来的一次关键局部观测记录。
一个 Node 通常会关联当前帧的时间戳、局部位姿、过滤后的点云、重力方向、所属轨迹编号以及插入的 Submap 信息。
而 Submap 保存的是环境结构，例如墙体、障碍物、走廊边界和空闲区域。
两者的区别可以这样理解：
Submap：
保存"环境长什么样"。
Node：
保存"机器人什么时候、在哪个位置、看到了什么"。当前帧通过 MotionFilter 后，插入 Submap 和创建 Node 通常发生在同一次有效插图事件中，但它们不是同一个对象。
当前帧有效点云
↓
插入 ActiveSubmaps2D
├── 更新栅格地图
└── 生成 InsertionResult
↓
PoseGraph::AddNode(...)
↓
创建 TrajectoryNode因此不能理解为"栅格地图里的点后来变成了 Node"。
更准确的说法是：
当前帧点云在插入 Submap 的同时，系统把这次有效观测对应的位姿、点云和子地图关系交给 PoseGraph，从而创建一个 Node。
9．原始帧、MatchingResult、InsertionResult 和 Node 的数量关系
正常情况下，数量关系可以写成：
原始 LiDAR 帧数量
≥ MatchingResult 数量
≥ InsertionResult 数量
= Node 生成数量原始 LiDAR 帧数量表示传感器实际发布了多少次点云。部分原始帧可能因为多雷达数据尚未凑齐、时间戳不合适、点云为空、系统初始化尚未完成等原因，没有形成有效的 MatchingResult。
MatchingResult 数量表示成功完成 Local SLAM 位姿估计的次数。即使某帧被 MotionFilter 过滤，通常仍然可以有 MatchingResult，因为系统已经完成了扫描匹配并得到了局部位姿。
InsertionResult 数量表示真正插入 ActiveSubmaps2D 的次数。MotionFilter 会过滤掉大量重复帧，因此它通常明显少于 MatchingResult 数量。
在标准链路中，只要生成一个 InsertionResult，后端就会调用一次 PoseGraph::AddNode(...)，因此：
InsertionResult 数量 = Node 生成数量这里说的是"Node 的生成总数"。如果后续启用了 PoseGraph Trimmer（位姿图裁剪器），旧 Node 或旧 Submap 可能被裁剪，因此运行过程中当前仍保留的 Node 数量可能会小于历史生成总数。
例如，10 Hz LiDAR 连续运行 10 秒，理论上有约 100 帧原始点云；机器人前 3 秒静止，中间 5 秒持续前进并转弯，最后 2 秒静止，那么可能出现：
原始 LiDAR 帧：100
MatchingResult：95
InsertionResult：23
Node 生成数量：23没有成为Node的那些帧不是完全无用，它们仍然可以参与实时扫描匹配、帮助维持连续局部位姿；只是它们没有被长期写入 Submap 和 PoseGraph。
最后可以把这一部分总结成一句话：
Node 不是每一帧 LiDAR 的副本，而是当前帧经过 Local SLAM 位姿估计、通过 MotionFilter 筛选、成功写入 ActiveSubmaps2D 后，由 PoseGraph::AddNode(...) 创建的一次关键局部观测。
三、Node 的 local_pose 与 global_pose：刚插入时是否相同、局部漂移如何被后端修正
先把前面那句话修正得更准确：
constant_data里的local_pose，就是当前帧点云经过位姿预测、去畸变、实时扫描匹配后，前端最终得到的局部位姿；当前帧通过 MotionFilter（运动过滤器）并插入 ActiveSubmaps2D（活动子地图）后，这个匹配结果会被固定写入 Node。
所以，它不是单纯的 IMU / 轮速预测值，也不是原始 LiDAR 的位置，而是当前帧点云与活动 Submap 匹配后的 pose_estimate（前端估计位姿）。TrajectoryNode::Data 中保存的 local_pose 属于不可变观测数据；Node 外部的 global_pose 则表示它在 Global SLAM 全局地图中的位置，后续会被后端持续更新。
当前 Node 的两种位姿可以写成：
T_L←N = local_pose
T_G←N = global_pose其中，L（Local SLAM 坐标系）表示前端持续维护的局部坐标系，G（Global SLAM 坐标系）表示后端全局优化后的地图坐标系，N 表示当前 Node 所在的机器人观测时刻。T_A←B 的含义是：把 B 坐标系中的点转换到 A 坐标系。
两者之间的核心关系是：
T_G←N = T_G←L × T_L←N其中，T_L←N 是前端扫描匹配得到的 local_pose；T_G←L 是当前 Local SLAM 坐标系映射到 Global SLAM 坐标系的修正关系；两者相乘后得到 Node 的初始 global_pose。注意，T_G←L 更适合被理解成当前时刻的"局部轨迹到全局地图的校正关系"，它不一定是一个永久不变、对所有历史 Node 都完全适用的固定变换。
1．刚插入的 Node，local_pose 和 global_pose 是否一样
不保证一样。 新 Node 刚进入 PoseGraph 时，源码会先计算：
optimized_pose =
GetLocalToGlobalTransform(trajectory_id) *
constant_data->local_pose;随后再用这个 optimized_pose 初始化 TrajectoryNode 的 global_pose。也就是说，新 Node 的全局位姿不是直接复制 local_pose，而是先经过当前的 Local-to-Global 修正。
因此，只有在下面这个条件成立时，两者数值上才相同：
T_G←L = II（单位变换）表示不平移、不旋转，也就是 Local SLAM 坐标系与 Global SLAM 坐标系完全重合。
这在刚启动建图时很常见。例如刚开始建图，还没有发生回环，也没有加载外部地图或初始位姿修正：
local_pose = (10.0 m, 2.0 m, 30°)
T_G←L = I
global_pose = (10.0 m, 2.0 m, 30°)此时可以近似认为：
local_pose = global_pose但这个"相同"不是因为两者本质上是同一个变量，而是因为当前没有全局修正，两个坐标系暂时重合。
2．发生过回环后，新 Node 一插入就可能不同
假设机器人已经走了很远，Local SLAM 因累计误差向前漂了 1 m。后端通过回环检测发现：机器人实际已经回到旧区域，因此已经把全局地图修正回来。
假设此时：
当前 Node 的局部位姿：
T_L←N = (101.0 m, 0.0 m, 0°)
当前 Local-to-Global 修正：
T_G←L = (-1.0 m, 0.0 m, 0°)由于这里没有旋转，计算可以简单理解为：
T_G←N = T_G←L × T_L←N
= (-1.0 m) × (101.0 m)
= (100.0 m, 0.0 m, 0°)因此，新 Node 刚创建时就是：
local_pose = (101.0 m, 0.0 m, 0°)
global_pose = (100.0 m, 0.0 m, 0°)这里的 local_pose 仍然保留前端一路累计出来的局部坐标；global_pose 则已经被映射到回环修正后的全局地图位置。
上面的示例只包含平移，所以看起来像直接减去 1 m。真实情况下若 T_G←L 同时包含平移和 yaw（航向角）修正，则必须进行刚体变换复合，不能简单地对 x、y、yaw 分别相加减。
3．local_pose 会越来越偏，后面的 Node 会不会越来越不准
从"相对于真实全局世界的位置"来看，确实可能越来越偏。因为 Local SLAM 的主要任务是保证短时间内的连续追踪，而不是不断和很久以前经过的区域做全局一致性校验。
例如机器人实际直行了 100 m：
真实世界位置：
Node 0 = (0.0 m, 0.0 m)
Node 500 = (100.0 m, 0.0 m)
Local SLAM 位姿：
Node 0 = (0.0 m, 0.0 m)
Node 500 = (101.0 m, 0.2 m)此时 Node 500 的 local_pose 相对于真实世界已经存在大约 1 m 的累计误差。
但这不代表 Node 500 的点云匹配一定已经很差。关键要区分两种准确性：
局部准确性：
当前 Node 相对附近 Node、当前 Active Submap、附近墙体的关系是否正确。
全局准确性：
当前 Node 相对真实起点、历史区域、整张全局地图的位置是否正确。Local SLAM 通常优先保证局部准确性。即使整条局部轨迹已经慢慢偏离真实世界，当前帧仍然可以较好地贴合正在构建的活动 Submap，因为这个 Submap 本身也是沿着同一条 Local SLAM 轨迹建立的。
可以理解成：机器人和附近墙体一起在局部坐标系中慢慢漂了。局部地图内部仍然连续，机器人相对最近墙体的位置也通常合理；但整张局部地图相对真实仓库坐标已经整体错位。
例如：
Node 499 local_pose = (100.8 m, 0.2 m)
Node 500 local_pose = (101.0 m, 0.2 m)
局部相对运动 = 0.2 m如果真实情况下两个 Node 之间也移动了约 0.2 m，那么前端局部运动估计仍然是正确的。问题在于前面几十米、几百米累计下来的微小误差不断叠加，使得它们相对全局真值偏了。
所以更准确的说法是：
local_pose不会在 Node 创建后继续变差，因为它已经冻结；但越晚创建的 Node，其local_pose相对真实全局环境的累计漂移可能越大。
4．为什么 Local SLAM 不直接把漂移修正回 local_pose
因为 local_pose 是前端实时扫描匹配产生的历史观测结果。它记录的是："当时前端依据当前点云、预测位姿和活动 Submap，认为机器人位于哪里。"
如果后端一发现回环，就直接把所有 local_pose 改掉，会带来两个问题。
第一，Local SLAM 的连续性会被破坏。机器人控制、局部避障、实时路径跟踪等模块通常希望得到连续平滑的局部位姿；如果每次回环后前端坐标突然跳几米，控制模块会很难处理。
第二，后端优化不是重新运行前端扫描匹配。后端做的是：保留前端已经得到的局部观测和局部相对运动，再利用回环、里程计、地标等全局约束，重新安排这些 Node 和 Submap 在全局地图中的位置。
因此：
Node local_pose：
前端扫描匹配结果，固定保存，不直接优化。
Node global_pose：
后端全局图优化结果，可反复修改。
Submap 内部栅格：
固定，不重新插图、不拉伸、不扭曲。
Submap global_pose：
后端优化，可整体平移、旋转。Cartographer 的 2D 优化问题中，Node 的 global_pose_2d 和 Submap 的 global_pose 都会被作为 Ceres 优化变量；而 Node 的 local_pose_2d 作为前端局部观测保存下来，用来构建约束。
5．后端如何利用 local_pose 修正 global_pose
当前 Node 插入一个 Submap 时，前端已经知道该 Node 相对这个 Submap 的局部关系。设：
T_L←S：
Submap 在 Local SLAM 坐标系中的局部位姿。
T_L←N：
Node 的 local_pose。
Z_S←N：
前端测得的 Node 相对于 Submap 的局部约束。那么这个局部约束可以写成：
Z_S←N = (T_L←S)^-1 × T_L←N其中，(T_L←S)^-1 表示从 Local 坐标系转换回该 Submap 坐标系；相乘后就得到 Node 在 Submap 局部坐标系中的位置。
后端优化时，不再直接修改 Z_S←N，而是寻找一组新的全局 Node 位姿和全局 Submap 位姿，使它们在全局坐标下推算出来的相对关系，尽量符合前端观测：
(T_G←S)^-1 × T_G←N ≈ Z_S←N其中，T_G←S 是 Submap 的全局位姿，T_G←N 是 Node 的全局位姿。
这说明后端不是只拉 Node，也不是只拉 Submap，而是同时调整两者，使所有局部插图约束、回环约束、轮速里程计约束、连续 Local SLAM 约束尽量一致。
Cartographer 还会对连续 Node 加入基于局部位姿的相对约束：
Z_Ni←Nj =
(T_L←N_i)^-1 × T_L←N_j也就是前端认为 Node j 相对 Node i 发生了怎样的运动。若有轮速里程计，还会额外加入相邻 Node 之间的 Odometry 相对约束。这样即使回环要修正整条轨迹，后端也不会让相邻 Node 的局部关系随意散掉。
6．新 Node 刚进入后端后，还要经历两个阶段
新 Node 的 global_pose 不是一次确定、永远不变的，它至少有两个阶段。
第一阶段是"刚创建时的初始全局位姿"：
global_pose_initial =
T_G←L(current) × local_pose这一步利用的是当前已经存在的 Local-to-Global 修正关系。若之前发生过回环，新 Node 会立刻继承这个修正，不会继续按旧的漂移坐标直接摆进全局地图。
第二阶段是"下一轮 PoseGraph 优化后的全局位姿"。当新 Node 对历史完成 Submap 的匹配成功，或者已有足够 Node 触发优化时，后端会综合新的约束重新求解。求解完成后，Node 的 global_pose 可能继续变化，Submap 的 global_pose 也可能同步变化。源码中，优化结束后会把求出的 global_pose_2d 写回每个 Node 的 global_pose，并把新的全局 Submap 位姿保存下来。
因此可以记成：
刚插入：
global_pose = 当前 Local-to-Global 修正 × local_pose
发生新的优化后：
global_pose = 所有约束共同优化后的结果7．一个完整例子：局部持续漂移，全局通过回环拉回
假设机器人从起点出发，绕仓库走一圈，最终回到起点附近。
开始时，Local 与 Global 坐标系重合：
T_G←L = I
Node 0：
local_pose = (0.0, 0.0, 0°)
global_pose = (0.0, 0.0, 0°)机器人持续行驶后，前端累计了一些漂移：
Node 500：
local_pose = (101.0, 0.5, 2°)
真实位置大约：
(100.0, 0.0, 0°)在尚未发现回环前，后端不知道 Local SLAM 已经漂了，因此当前可能仍然有：
global_pose ≈ local_pose当 Node 500 与历史 Submap 匹配成功后，系统发现：当前看到的墙体、门框、货架边缘，与起点区域高度一致。于是形成回环约束，PoseGraph 开始优化。
优化后，可能得到：
Node 500：
local_pose = (101.0, 0.5, 2°) // 不变
global_pose = (100.0, 0.0, 0°) // 被后端修正接着 Node 501 到来。假设它的前端局部位姿为：
Node 501：
local_pose = (101.2, 0.5, 2°)此时当前 T_G←L 已经带有回环修正，因此 Node 501 刚插入时就可能直接得到：
Node 501：
global_pose ≈ (100.2, 0.0, 0°)这就是后端修正的价值：前端 Local SLAM 可以继续保持连续运行；但从回环发生之后开始，新 Node 会立刻继承全局校正，不必重新从错误的全局位置继续累计。
8．2D Cartographer 中还要注意 gravity_alignment
虽然 2D Cartographer 最终重点优化的是 x、y、yaw，但 Node 中的 local_pose 和 global_pose 类型仍是 Rigid3d（三维刚体位姿）。这是因为实际机器人可能存在轻微 roll（横滚）和 pitch（俯仰），例如过门槛、经过坡道、底盘振动、LiDAR 安装角度误差等。
Cartographer 会使用 gravity_alignment（重力对齐旋转）先尽量消除 roll、pitch 对二维匹配的影响，再把位姿投影到二维平面进行优化。优化完成后，再把二维全局位姿和原始重力对齐信息组合回三维 global_pose。因此在平坦地面上，你可以主要把两种位姿理解为：
local_pose ≈ (x_L, y_L, yaw_L)
global_pose ≈ (x_G, y_G, yaw_G)但在源码层面，它们仍保留三维表达能力。
9．最后总结
Node 的 local_pose 和 global_pose 不是同一个东西。
local_pose：
当前帧点云经过前端扫描匹配后得到的局部位姿。
创建 Node 后固定不变。
保证局部连续性和实时性。
相对真实全局地图可能逐渐累计漂移。
global_pose：
当前 Node 在全局地图中的位姿。
刚插入时由 T_G←L × local_pose 初始化。
后续会随回环、里程计、地标、Node-Submap 约束等不断优化。刚启动且 T_G←L =I时，二者通常数值相同；一旦发生过回环或其他全局修正，新 Node 的 global_pose 就会先经过当前 Local-to-Global 修正，因此通常不再等于 local_pose。
最核心的一句话是：
Local SLAM 负责连续地"往前走"，即使长期会整体漂移；Global SLAM 不改写这条局部历史，而是通过优化 Node 和 Submap 的全局位姿，把已经漂移的局部轨迹重新摆回正确的全局地图位置。
四、Node 和 Submap 之间不是一种关系，而是两种关系
Node 与 Submap 的关系分成两层，必须区分。
第一层：当前 Node 被插入当前 Active Submap。这是前端 Local SLAM 已知的局部关系，后端直接建立 INTRA_SUBMAP 约束。
第二层：当前 Node 或历史 Node 与某张历史完成 Submap 匹配成功。这是后端回环搜索得到的关系，后端建立 INTER_SUBMAP 约束。
可以压缩成：
当前 Node ↔ 当前 Active Submap
= INTRA_SUBMAP
= 前端插图关系
Node ↔ 历史 Finished Submap
= INTER_SUBMAP
= 后端回环匹配关系这两类关系的来源、计算方式、可信度和作用都不一样。
五、INTRA_SUBMAP：当前 Node 与当前 Submap 的局部约束
稳定运行时，ActiveSubmaps2D 通常维护两张活动 Submap：一张较旧、结构更成熟，用于当前 Scan-to-Map 匹配；一张较新、正在同步积累点云，准备未来接替旧图。
因此一个有效 Node 通常会被同时插入两张图：
Node k
├── old_submap
└── new_submap当前 Node 与这两张 Submap 的关系不需要重新做 Fast Correlative 或 Ceres 匹配，因为它已经由 Local SLAM 的插图过程确定。PoseGraph 直接利用 Node 和 Submap 在 Local SLAM 坐标系中的位姿计算相对变换：
Z_S←N = (T_L←S)⁻¹ × T_L←N其中：
T_L←S：
Submap 在 Local SLAM 坐标系中的局部位姿。
T_L←N：
Node 在 Local SLAM 坐标系中的 local_pose。
Z_S←N：
Node 相对于该 Submap 的局部测量位姿。例如 Submap 5 位于 Local SLAM 中的 [10 m, 2 m, 0°]，当前 Node 31 位于 [14 m, 3 m, 15°]，则 Node 31 相对 Submap 5 大致处于 [4 m, 1 m, 15°]。这个相对关系就是 INTRA_SUBMAP 约束的测量值。
源码中，PoseGraph2D 会将当前 Node 加入对应 Submap 的 node_ids 集合，再根据 Submap 局部位姿的逆变换与 Node local_pose 相乘，构造 INTRA_SUBMAP Constraint；其权重使用 matcher_translation_weight 与 matcher_rotation_weight。
INTRA_SUBMAP 的核心作用是维持局部结构。它告诉后端："这个 Node 当时确实以这个相对位置插入了这张 Submap。"相邻 Node 通过共同连接同一 Submap 而保持局部连续；相邻 Submap 又通过共享 Node 保持彼此连续。
Node 20 ── Submap 4
Node 21 ── Submap 4、Submap 5
Node 22 ── Submap 4、Submap 5
Node 23 ── Submap 5这里并没有显式建立 "Submap 4 ↔ Submap 5" 的边，但 Node 21 和 Node 22 同时属于两张图，因此两张 Submap 已经通过共享 Node 被关联起来。
六、INTER_SUBMAP：回环约束到底是不是"新 Node 匹配旧 Submap"
是。最常见的回环流程就是：
新 Node
↓
匹配历史完成 Submap
↓
成功后建立 INTER_SUBMAP例如机器人早期经过仓库入口，建立了 Submap 0、Submap 1，并产生 Node 0 到 Node 30。之后机器人绕仓库运行一圈，当前生成 Node 180。若 Node 180 重新看到了入口处的柱子、墙面、门框和货架，那么后端会尝试把 Node 180 的过滤点云匹配到历史完成的 Submap 0 或 Submap 1 上。
如果匹配成功，就建立：
Node 180 ↔ Submap 0
INTER_SUBMAP这条约束的含义不是"Node 180 本来插入了 Submap 0"，而是"当前环境与 Submap 0 的历史环境再次重合，因此当前 Node 应与这张旧图保持某个相对位姿关系"。
不过源码还有一个补充方向：当某张新的 Submap 刚刚完成时，PoseGraph 也会拿历史 Node 去匹配这张新完成图。也就是说，回环不仅是"新 Node 找旧图"，还包括：
历史 Node
↓
匹配刚完成的新 Submap这样可避免时间顺序导致漏检。例如某批旧 Node 早期经过区域 A，但覆盖同一区域的新 Submap 很晚才建完；若只让新 Node 匹配旧图，旧 Node 永远不会再有机会匹配新图。PoseGraph2D 在 newly_finished_submap 为真时，会遍历历史 Node，并让不属于该 Submap 的 Node 尝试建立约束。
七、为什么回环只针对 Finished Submap
回环目标必须是完成并冻结的 Submap，而不是仍在持续插图的活动图。
活动 Submap 的 ProbabilityGrid 每一帧都可能发生变化：新的 hit 会增加占据概率，新的 miss 会清理自由空间，栅格范围也可能扩展。这样的地图结构不稳定，不适合作为后台长期匹配目标。
Finished Submap 则不同：它已经停止接收新的 range data，内部 ProbabilityGrid 固定。FastCorrelativeScanMatcher2D 可以为它建立多分辨率预计算栅格，并让后续多个 Node 反复复用同一个匹配器。
因此职责分工是：
Active Submap：
前端 Scan-to-Map 匹配
+
持续写入概率栅格。
Finished Submap：
后端 Node-to-Submap 回环匹配
+
长期约束构建
+
全局优化。PoseGraph2D 收集回环候选时，明确只取状态为 Finished 的 Submap；当一张旧图完成后，才会把它从"无约束搜索"状态转成可参与 Constraint Search 的 Finished 状态。
八、Submap 的 local_pose 与 global_pose：从创建、初始化到后端优化
Submap（子地图）和 Node 一样，也同时存在两套位置：一套来自 Local SLAM（前端局部建图），另一套来自 Global SLAM（后端 PoseGraph 全局优化）。
T_L←S：Submap 的 local_pose，表示该子地图在 Local SLAM 坐标系 L 中的位置。
T_G←S：Submap 的 global_pose，表示该子地图在 Global SLAM 坐标系 G 中的位置。
T_L←N：Node 的 local_pose。
T_G←N：Node 的 global_pose。其中，L 是前端连续维护的局部坐标系，短时间内连续稳定、但长期可能累计漂移；G 是后端优化后的全局坐标系，用于让所有 Node 和 Submap 在整张地图中保持一致。Local SLAM 的目标是持续构建局部一致的 Submap，Global SLAM 的目标则是把这些 Submap 和 Node 在全局范围内摆放一致。
1．Submap 的 local_pose 是怎么产生的
一帧 LiDAR 点云完成预测、去畸变和实时扫描匹配后，前端会得到当前雷达在 Local SLAM 坐标系中的位姿。当前帧点云也会被转换到 Local 坐标系，形成：
range_data_in_local：当前帧已经放入 Local SLAM 坐标系 L 的点云。
range_data_in_local.origin：当前帧 LiDAR 原点在 L 中的位置。当活动子地图数量不足，或者最新 Submap 已经插入足够多帧有效点云时，ActiveSubmaps2D 会创建新 Submap。标准 2D 在线建图链路中，新 Submap 的锚点来自当前帧的 LiDAR 原点：
AddSubmap(range_data_in_local.origin.head<2>());因此，新 Submap 的局部位姿可以概念性写成：
T_L←S_new = Translation(range_data_in_local.origin)例如，当前帧扫描匹配后，LiDAR 原点在 Local SLAM 中的位置为：
range_data_in_local.origin = (50.0 m, 12.0 m)那么新建的 Submap 可能具有：
T_L←S_new = (50.0 m, 12.0 m, 0°)这里的含义是：Submap 的局部参考原点被放在 Local SLAM 坐标系中的 (50.0, 12.0) 位置。
在标准 Cartographer 2D 在线建图实现中，Submap 创建函数接收的是当前位置 origin，其 local_pose 通常以平移形式保存，因此 Submap 坐标轴一般与 Local SLAM 坐标轴平行；它不是严格跟随机器人当时车头朝向旋转的一张"小地图纸"。后端计算时仍使用完整的二维刚体变换形式，以兼容全局优化产生的旋转修正。
2．Submap 自己的局部坐标系 S 是什么
每张 Submap 都可以看成有自己的参考坐标系 S。Submap 内部的墙体、栅格、障碍物、空闲区域，都是以这张局部地图为参考构建的。
p_L = T_L←S × p_S其中：
p_S：环境点在 Submap 坐标系中的位置。
p_L：同一个环境点在 Local SLAM 坐标系中的位置。
T_L←S：这张 Submap 在 Local SLAM 中的固定锚点。反过来，若当前点云已经在 Local SLAM 坐标系中，想知道它落在某张 Submap 内部的什么位置：
p_S = (T_L←S)^-1 × p_L例如：
T_L←S = (50.0 m, 12.0 m, 0°)
某个墙点在 Local SLAM 中：
p_L = (53.0 m, 14.0 m)则它相对这张 Submap 的位置为：
p_S = (3.0 m, 2.0 m)也就是说，这面墙在 Submap 自己的局部参考下，位于"向前 3 m、向左或向上 2 m"的位置。
但还要区分两个概念：
T_L←S：
PoseGraph 意义上的 Submap 局部锚点。
Grid2D / MapLimits：
Submap 内部栅格的分辨率、尺寸、单元格索引范围和栅格坐标转换规则。T_L←S 决定"这张子地图在 Local SLAM 中从哪里开始"；MapLimits 决定"墙体最终写进哪一个栅格单元"。前者是位姿关系，后者是栅格存储结构，不能混为同一个变量。
3．Submap 的 local_pose 创建后会不会变
不会。
Submap 的 local_pose 属于前端历史结果。它表达的是：这张子地图刚创建时，Local SLAM 认为它应该锚定在哪里。
T_L←S：固定，不优化，不修改。但是 Submap 内部栅格在刚创建后并不会立即固定。它会继续接收后续有效点云，逐渐更新墙体、障碍物和空闲区域。Cartographer 通常同时维护两张 Active Submap，使相邻子地图拥有重叠区域；旧 Submap 最终完成后不再插图，内部栅格才彻底冻结。
所以要区分：
Submap local_pose：
创建时固定。
Submap 内部栅格：
Active 阶段持续更新；
Finished 后停止更新。4．Submap 的 global_pose 是怎么初始化的
Submap 的全局位姿是：
T_G←S它表示这张 Submap 在全局地图中的位置和朝向。这个位姿不是保存在 Submap2D 内部不断改写，而是由 PoseGraph / OptimizationProblem2D 单独保存和优化。
第一张 Submap 创建时，初始化逻辑可以理解为：
T_G←S0 = T_G←L × T_L←S0刚开始建图时，通常 Local 和 Global 坐标系重合：
T_G←L = I因此：
T_G←S0 = T_L←S0也就是第一张 Submap 的局部位姿和全局位姿数值上通常相同。
但后续新 Submap 的初始化，不是直接把 T_L←S_new 当作全局位姿，而是通过上一张已知 Submap 的全局位置进行推算。Cartographer 的实际代码逻辑可以抽象为：
T_G←S_new_initial = T_G←S_old × (T_L←S_old)^-1 × T_L←S_new源码中对应的核心形式是：
first_submap_pose *
ComputeSubmapPose(old_submap).inverse() *
ComputeSubmapPose(new_submap)也就是"旧 Submap 全局位姿 × 旧 Submap 局部位姿的逆 × 新 Submap 局部位姿"。
5．这条公式到底是什么意思
前面那句"先从旧 Submap 的全局位置回到 Local SLAM，再移动到新 Submap 的局部位置"只是便于理解的口头说法。严格从数学和代码角度说，真正发生的是：
先计算：新 Submap 相对于旧 Submap 的局部相对位姿。
再利用：旧 Submap 已优化后的全局位姿，把这个相对位姿映射到全局地图。公式从右往左看：
T_G←S_new = T_G←S_old × (T_L←S_old)^-1 × T_L←S_new其中：
T_L←S_new：
把新 Submap 坐标系中的点转换到 Local SLAM 坐标系 L。
(T_L←S_old)^-1：
把 Local SLAM 坐标系 L 中的点，转换到旧 Submap 坐标系 S_old。
(T_L←S_old)^-1 × T_L←S_new：
得到新 Submap 相对于旧 Submap 的局部位姿。
T_G←S_old：
把"相对于旧 Submap 的新 Submap 位姿"转换到全局坐标系 G。因此，中间这一段才是最关键的局部相对关系：
T_S_old←S_new = (T_L←S_old)^-1 × T_L←S_new最终：
T_G←S_new = T_G←S_old × T_S_old←S_new这和 Node 的新全局位姿初始化非常像：都是先保留前端局部关系，再乘上当前已经存在的全局修正。
6．完整数值例子：旧 Submap 如何带出新 Submap 的初始全局位姿
假设当前 Local SLAM 已经发生了累计漂移，而且后端已经通过回环优化修正过旧 Submap。
旧 Submap 在 Local SLAM 中的位置是：
T_L←S_old = (20.0 m, 5.0 m, 0°)新 Submap 在 Local SLAM 中的位置是：
T_L←S_new = (24.0 m, 5.0 m, 0°)这说明，在 Local SLAM 看来，新 Submap 比旧 Submap 向 x 正方向前进了 4 m。
但后端优化后，旧 Submap 在全局地图中的位置已经变成：
T_G←S_old = (18.0 m, 7.0 m, 90°)这里的 90° 很关键，表示旧 Submap 的局部 x 正方向，在全局地图中指向全局 y 正方向。
第一步，先求新 Submap 相对旧 Submap 的局部运动：
T_S_old←S_new = (T_L←S_old)^-1 × T_L←S_new = (4.0 m, 0.0 m, 0°)它表示：
从旧 Submap 坐标系出发，新 Submap 位于旧 Submap 的局部
x正方向 4 m 处。
第二步，将这个"局部前进 4 m"转换到全局地图：
T_G←S_new = T_G←S_old × T_S_old←S_new代入数值：
T_G←S_new = (18.0 m, 7.0 m, 90°) × (4.0 m, 0.0 m, 0°)由于旧 Submap 在全局中的 yaw 是 90°，它的局部 x 正方向已经旋转为全局 y 正方向，因此这 4 m 不会加到全局 x，而是加到全局 y：
T_G←S_new = (18.0 m, 11.0 m, 90°)最终结果是：
Local SLAM 中：
S_old = (20.0, 5.0, 0°)
S_new = (24.0, 5.0, 0°)
Global SLAM 中：
S_old = (18.0, 7.0, 90°)
S_new = (18.0, 11.0, 90°)这个例子说明：前端只知道新旧 Submap 在 Local SLAM 中相差 4 m；后端利用旧 Submap 已经被回环修正过的全局姿态，把这 4 m 放到正确的全局方向上。
也可以先计算当前 Local-to-Global 修正：
T_G←L = T_G←S_old × (T_L←S_old)^-1然后直接写成：
T_G←S_new = T_G←L × T_L←S_new两种计算方式完全等价。
7．Submap 与 Node 的局部约束怎么产生
当前 Node 插入某张 Submap 时，前端已经同时知道：
T_L←N：当前 Node 的 local_pose。
T_L←S：当前 Submap 的 local_pose。因此可以直接求出 Node 相对于 Submap 的局部观测关系：
Z_S←N = (T_L←S)^-1 × T_L←N其中，Z_S←N 表示：
当前 Node 在这张 Submap 坐标系中位于哪里、朝向哪里。
例如：
T_L←S = (50.0 m, 12.0 m, 0°)
T_L←N = (52.0 m, 13.0 m, 10°)则：
Z_S←N = (2.0 m, 1.0 m, 10°)这说明当前 Node 相对于这张 Submap，位于前方 2 m、侧向 1 m、朝向偏转 10° 的位置。
这个约束称为：
INTRA_SUBMAP 约束即 Node 与其实际插入的 Submap 之间的局部约束。Cartographer 在 Node 加入子地图时，正是利用 SubmapPose^-1 × local_pose_2d 构造该约束。
8．回环约束怎么作用到 Submap
当当前 Node 到达历史区域附近时，后端会让当前 Node 的过滤点云，与已经完成的历史 Submap 栅格地图做匹配。
若匹配成功，会建立：
INTER_SUBMAP 约束形式仍然是：
Z_S_old←N_new_loop但它的含义变成：
当前新 Node 与历史旧 Submap 匹配后，认为它相对旧 Submap 应该位于哪里。
例如 Local SLAM 认为机器人已经走到 (101 m, 0.3 m)，但当前点云与起点附近 Submap 0 匹配后发现，它实际上应该靠近起点区域。这个匹配结果会作为回环约束加入 PoseGraph。
因此 Cartographer 图中的核心边通常是：
Submap ← Node而不是直接构造：
Submap A ← Submap B相邻 Submap 因为共享了一批 Node 而被连接；历史 Submap 与当前 Node 因为回环匹配而被连接。所有 Submap 之间的全局关系，最终通过这些 Node--Submap 约束间接形成。
9．后端如何优化 Submap 的 global_pose
后端不会优化：
T_L←S也不会修改 Submap 内部已经插入的栅格结构。
后端真正优化的是：
T_G←S即每张 Submap 在全局地图中的：
x_G、y_G、yaw_G同时，后端也优化所有 Node 的：
T_G←N因此优化变量可以写成：
X = {T_G←S_0, T_G←S_1, ..., T_G←S_m, T_G←N_0, T_G←N_1, ..., T_G←N_n}对于每条 Node--Submap 约束，优化器希望全局位姿推导出的相对关系，尽量等于前端或回环匹配得到的测量关系：
(T_G←S)^-1 × T_G←N ≈ Z_S←N对应残差可写成：
r_SN = Log(Z_S←N^-1 × (T_G←S)^-1 × T_G←N)当 r_SN 越小，说明当前优化后的 Node 和 Submap 全局摆放关系，越符合前端插图或回环匹配得到的局部测量。
Cartographer 2D 优化器会把每张 Submap 和每个 Node 的 (x, y, θ) 都作为 Ceres 参数块；第一张 Submap 通常被固定，避免整张地图在没有绝对参考时可以任意整体平移、旋转。优化问题中会加入 Intra-Submap 约束、回环 Inter-Submap 约束、相邻 Node 的 Local SLAM 相对约束、可用的里程计约束，以及地标或 Fixed Frame 约束。
概念上的总目标可以写成：
min Σ||r_intra||² + Σρ(||r_loop||²) + Σ||r_local_slam||² + Σ||r_odom||² + Σ||r_landmark/fixed_frame||²其中：
r_intra：Node 插入自身 Submap 时的局部约束误差。
r_loop：Node 与历史 Submap 回环匹配后的约束误差。
r_local_slam：连续 Node 之间的前端相对运动误差。
r_odom：轮速里程计相对运动误差。
r_landmark/fixed_frame：二维码、地标、GPS 或固定参考系约束误差。
ρ(...)：回环约束使用的鲁棒损失，降低错误回环的破坏力。10．优化后，Submap 到底发生了什么变化
假设第 10 张 Submap 在 Local SLAM 中已经漂移：
T_L←S10 = (101.0 m, 0.3 m, 2°)回环前，它的全局初值可能也接近：
T_G←S10 ≈ (101.0 m, 0.3 m, 2°)当前 Node 与起点区域的历史 Submap 成功匹配后，PoseGraph 优化可能将其修正为：
T_G←S10 = (100.0 m, 0.0 m, 0°)变化过程可以概括为：
T_L←S10：不变。
Submap 10 内部栅格：不变。
Submap 10 的 global_pose：平移、旋转到新的全局位置。
相关 Node 的 global_pose：同步被优化。因此回环不是把 Submap 内部墙体拉弯、把栅格拉伸，或者重新运行所有历史点云插图；它是把整张 Submap 当成局部刚体地图块，重新放到更合理的全局位置。
优化器求解完成后，会将新的 global_pose 分别写回 Submap 和 Node；但 Node / Submap 的 Local SLAM 位姿仍保留原始前端结果。
11．Node 与 Submap 的四种位姿最终对比
Node local_pose：T_L←N
当前关键点云在 Local SLAM 中的位置。
前端扫描匹配得到，固定，不优化。
Node global_pose：T_G←N
当前关键观测在全局地图中的位置。
后端持续优化。
Submap local_pose：T_L←S
当前局部栅格地图在 Local SLAM 中的固定锚点。
创建时确定，固定，不优化。
Submap global_pose：T_G←S
当前局部栅格地图在全局地图中的摆放位置。
后端持续优化。最核心的两条公式是：
前端局部约束：Z_S←N = (T_L←S)^-1 × T_L←N
后端全局优化： (T_G←S)^-1 × T_G←N ≈ Z_S←N最后一句话总结：
Submap 的
local_pose来自创建时当前 LiDAR 在 Local SLAM 中的位置，用于固定这张局部地图的前端锚点；Submap 的global_pose则由 PoseGraph 根据 Node--Submap 局部约束、回环、里程计和地标等信息不断优化。后端不会重写 Submap 的局部历史和内部栅格，而是把整张 Submap 与相关 Node 一起重新摆放到一致的全局地图中。
九、回环匹配具体怎样做：筛选、粗搜索、精修
Node 并不会一加入 PoseGraph 就对所有历史 Submap 做暴力搜索。后端会先根据当前全局估计计算 Node 相对历史 Submap 的预测位置：
T_S←N_pred = (T_G←S)⁻¹ × T_G←N其中：
T_G←S：
历史 Submap 当前的全局位姿。
T_G←N：
当前 Node 当前的全局位姿。
T_S←N_pred：
按照当前全局估计推导出的 Node 相对历史 Submap 的预测位置。若预测距离已经超过 max_constraint_distance，ConstraintBuilder2D 会直接跳过该 Node-Submap 候选；之后还会使用每张 Submap 对应的采样器控制匹配任务数量。这样系统不会因为历史图越来越多而让后台任务无限堆积。
通过筛选后，回环任务进入后台线程池，流程是：
预测初始位姿
↓
FastCorrelativeScanMatcher2D
↓
得到离散粗匹配 score 与 pose_estimate
↓
score 是否达到阈值
↓
CeresScanMatcher2D
↓
得到连续精修后的匹配位姿
↓
生成 INTER_SUBMAP Constraint局部搜索使用 Match(...)：当前系统已有一个大致可靠的初始位置，因此只在预测位姿附近的平移和角度窗口内搜索。
全局搜索使用 MatchFullSubmap(...)：当前初始位置不可信，例如跨 Trajectory、全局重定位或尚未建立连接关系的情况，需要在整张 Submap 中搜索可能位置。ConstraintBuilder2D 分别提供 MaybeAddConstraint 和 MaybeAddGlobalConstraint 两条路径。
FastCorrelativeScanMatcher2D 的匹配对象是：
Node.filtered_gravity_aligned_point_cloud
↔
历史 Finished Submap 的 ProbabilityGrid它不是 Node 点云对历史 Node 点云。它寻找一个二维位姿 [t_x, t_y, θ]，使当前 Node 点云经过变换后尽量落在历史 Submap 的高占据概率区域。
qᵢ = R(θ) × pᵢ + [tₓ, t_y]ᵀ其中：
pᵢ：
Node 过滤点云中的第 i 个点。
qᵢ：
经候选位姿变换后，落到历史 Submap 中的位置。
R(θ)：
当前候选 yaw 对应的二维旋转。Fast Correlative 使用多分辨率预计算栅格和分支定界搜索，先排除明显低分区域，再细化有希望的候选区域。它解决的是"在较大范围中找到正确区域"的问题；CeresScanMatcher2D 则在粗匹配结果附近做连续优化，解决"亚栅格、连续角度精修"的问题。
十、构建约束到底有哪些：必须分成两层理解
这里要严格区分：源码中的 Constraint 对象类型 ，以及最终 Ceres 优化中加入的所有残差类型。
1. Constraint 对象只有两类
当前 PoseGraph 的 Constraint 标签只有：
INTRA_SUBMAP
INTER_SUBMAPINTRA_SUBMAP 是当前 Node 与当前插入 Submap 的局部插图关系。它直接来自 Local SLAM，数量多、关系稳定、用于维持局部地图骨架。
INTER_SUBMAP 是 Node 与历史完成 Submap 匹配成功后得到的跨 Submap 关系。它可能是同一 Trajectory 内的回环，也可能连接不同 Trajectory。INTER_SUBMAP 成功后，PoseGraph 还会更新 trajectory connectivity state，表示两条轨迹在空间上已经建立联系。
在 OptimizationProblem2D 中，INTRA_SUBMAP 和 INTER_SUBMAP 都会转成 Node-Submap 残差。但 INTER_SUBMAP 属于回环类约束，源码会对它使用 Huber Loss，降低偶发错误匹配对全局优化的破坏；INTRA_SUBMAP 则不使用该鲁棒核。
2. 除了 Node-Submap Constraint，优化器还会加入相邻 Node 的 Local SLAM 相对位姿残差
这个约束不是 Constraint::INTRA_SUBMAP，也不是 Constraint::INTER_SUBMAP，而是在 OptimizationProblem2D::Solve() 中直接加入相邻 Node 之间的残差。
对于连续的 Node i 和 Node i+1，系统从它们的 local_pose 计算相对位姿：
Z_local(i→i+1) =
(T_L←Nᵢ)⁻¹ × T_L←Nᵢ₊₁它表达的是：前端 Local SLAM 认为两个相邻 Node 之间发生了怎样的相对移动。
这个残差的意义是，即使某些 Node-Submap 关系或 Odom 数据不够强，后端仍保留相邻 Node 之间的连续性，避免整条轨迹被回环约束拉得过于松散或断裂。源码会对连续 node_index 的 Node 对加入该 Local SLAM 相对位姿残差，并使用 local_slam_pose_translation_weight 与 local_slam_pose_rotation_weight。
3. Odom 相对位姿残差
若 PoseGraph 收到 Odom 数据，OptimizationProblem2D 会在相邻 Node 之间插值出对应时刻的里程计相对运动，再添加 Node-Node 残差。
Z_odom(i→i+1)表示 Odom 认为 Node i 到 Node i+1 之间发生的相对位移和转角。
它的作用不是取代 LiDAR 回环，而是提供短时间运动连续性。比如走廊、重复货架、特征较少区域中，单纯依赖点云约束可能不够稳定；Odom 可以给后端一个"机器人在这段时间内大概怎样移动"的附加测量。
当前 2D 优化源码中，只要能计算出两 Node 之间的相对 Odom，就会添加对应残差，并使用 odometry_translation_weight 与 odometry_rotation_weight。
4. Fixed Frame Pose 残差
若系统输入了 FixedFramePoseData，例如外部定位系统、GNSS、UWB 或其他可转换为固定参考坐标系位姿的数据，OptimizationProblem2D 会将其作为 Node 与固定坐标系之间的残差。
它的作用是给整条 Trajectory 一个外部全局锚点。普通回环只能说明"当前区域和历史区域重合"，但它不一定知道整个地图在真实世界坐标中的绝对方向和绝对位置；Fixed Frame Pose 则可以提供这类全局参考。
源码中会按 Node 时间插值固定坐标系测量，并用 fixed_frame_pose_translation_weight 与 fixed_frame_pose_rotation_weight 加入残差；还可以选择 tolerant loss，以降低偶发外部定位异常造成的影响。
5. Landmark 观测残差
若系统输入了 LandmarkData，例如二维码、AprilTag、人工标志点或其他具有稳定 ID 的定位标记，PoseGraph 会保存 LandmarkObservation。它记录 landmark 相对 tracking frame 的变换，以及平移和旋转权重。
优化时，系统会将 Landmark 也视为待估计或固定的全局变量：如果 Landmark 已知且被冻结，它相当于全局锚点；如果未知但被多次观测，系统会根据多个 Node 对它的观测共同估计 Landmark 的全局位置。
Landmark 残差的作用是把"机器人看到某个已知标志"转化成对全局轨迹的约束。对于你的二维码定位场景，只要上层把二维码观测转成 Cartographer 的 LandmarkData，而不是只在 Local SLAM 外部强行改位姿，它就可以以这种形式进入全局图优化。
6. IMU 在当前 2D 后端中的真实位置
这一点必须特别说明：当前 Cartographer master 的 OptimizationProblem2D 并不把 IMU 数据作为 2D 全局优化残差。
PoseGraph2D 可以接收 IMU 数据，但 OptimizationProblem2D::AddImuData() 的源码明确说明：IMU 数据在 2D optimization 中不使用，因此该部分接口被忽略。IMU 对 2D Cartographer 更重要的作用是在 Local SLAM 中提供重力对齐、姿态预测和点云处理支持，而不是作为单独的 2D PoseGraph 残差。3D Cartographer 的情况不同，不能混在一起理解。
因此，二维后端的完整约束体系可以压缩成：
显式 Node-Submap Constraint：
1. INTRA_SUBMAP
2. INTER_SUBMAP
OptimizationProblem2D 额外残差：
3. 相邻 Node 的 Local SLAM 相对位姿
4. 相邻 Node 的 Odom 相对位姿（有 Odom 时）
5. Fixed Frame Pose 残差（可选）
6. Landmark 观测残差（可选）
注意：
IMU 在当前 2D 后端优化中不构成独立残差。十一、所有约束最终如何作用于 Node 和 Submap
Cartographer 2D 后端最终优化的对象，是所有 Node 的全局位姿 T_G←N_i、所有 Submap 的全局位姿 T_G←S_j，以及可选的 Fixed Frame 位姿、Landmark 位姿。这里的"全局位姿"指对象最终在 map 坐标系中的 [x, y, yaw]；但前端 Local SLAM、回环匹配、轮速里程计等提供的通常不是绝对全局坐标，而是"两个对象之间应该满足怎样的相对位姿关系"。因此，Ceres 做的事情不是给某个 Node 指定一个死的全局坐标，而是不断移动所有 Node、Submap 等全局变量，让全部相对测量尽量同时成立。
先约定符号：
T_G←N_i：第 i 个 Node 在全局 map 坐标系中的位姿
T_G←S_j：第 j 张 Submap 在全局 map 坐标系中的位姿
T_A←B：将 B 坐标系中的点变换到 A 坐标系
Z_A←B：测量得到的 B 相对于 A 的位姿对于任意一条从对象 A 到对象 B 的二维相对位姿约束，当前全局状态会先预测"B 相对于 A"的位置。设 A 的全局位姿是 [x_A, y_A, θ_A]，B 的全局位姿是 [x_B, y_B, θ_B]，则预测相对位姿为：
Δx_hat = cos(θ_A) × (x_B - x_A)
+ sin(θ_A) × (y_B - y_A)
Δy_hat = -sin(θ_A) × (x_B - x_A)
+ cos(θ_A) × (y_B - y_A)
Δθ_hat = NormalizeAngle(θ_B - θ_A)前两项表示先在全局坐标系中计算 B 与 A 的平移差，再旋转回 A 自身坐标系，因此得到的是"B 在 A 看来位于哪里"；第三项表示两者的相对朝向。若测量值为 Z_A←B = [z_x, z_y, z_θ]^T，则未加权误差为：
δ_A←B =
[
Δx_hat - z_x,
Δy_hat - z_y,
NormalizeAngle(Δθ_hat - z_θ)
]^TCartographer 会为平移和旋转分别赋予权重。设平移权重为 w_t，旋转权重为 w_r，则权重矩阵和最终残差为：
W_A←B = diag(w_t, w_t, w_r)
r_A←B = W_A←B × δ_A←B因此单条普通约束的代价为：
||r_A←B||² =
w_t² × (Δx_hat - z_x)²
+ w_t² × (Δy_hat - z_y)²
+ w_r² × NormalizeAngle(Δθ_hat - z_θ)²这说明权重不会遗漏，它们已经被乘进残差内部。由于最终代价是残差平方和，因此平移权重变为原来的 10 倍时，对应平移误差的惩罚约会变为原来的 100 倍。SpaCostFunction2D 的核心就是按照这种"预测相对位姿－测量相对位姿－权重缩放"的逻辑计算残差。
1. INTRA_SUBMAP：Node 与当前插入 Submap 的局部约束
INTRA_SUBMAP 来自当前 Node 的正常插图过程。当前 Node 已经在 Local SLAM 中插入一张或两张 Active Submap，因此其相对位置无需再做回环搜索，直接由前端局部位姿计算。设 L 是 Local SLAM 坐标系，则当前插图时的测量关系为：
Z_intra(S_j, N_i) =
(T_L←S_j)^-1 × T_L←N_i进入全局优化后，Ceres 调整的是 Node 和 Submap 的全局位姿，但要求它们计算出的相对关系仍接近前端测量：
(T_G←S_j)^-1 × T_G←N_i≈Z_intra(S_j, N_i)这条约束的右侧是局部测量，左侧是全局变量导出的相对关系。INTRA_SUBMAP 的作用是维持局部地图骨架：即使回环发生后整段轨迹被重新调整，Node 也不应脱离它当初插入的 Submap。稳定双 Submap 阶段中，一个 Node 往往同时插入 old submap 与 new submap，因此通常会形成两条 INTRA_SUBMAP 约束；共享 Node 正是相邻 Submap 能够持续连接的原因。
其权重来自前端匹配配置：
w_t = matcher_translation_weight
w_r = matcher_rotation_weight2. INTER_SUBMAP：Node 与历史完成 Submap 的回环约束
INTER_SUBMAP 来自后端回环匹配。当前 Node 本来没有插入历史 Submap，但当前环境与历史环境重新重合，因此系统将当前 Node 的过滤点云与历史 Finished Submap 的 ProbabilityGrid 匹配。Fast Correlative Scan Matcher 先在较大范围中找候选位置，Ceres Scan Matcher 再做连续精修，最终得到：
Z_inter(S_old, N_current) =
当前 Node 相对于历史 Submap 的匹配位姿进入全局优化后，约束形式仍与 INTRA 相同：
(T_G←S_old)^-1 × T_G←N_current≈Z_inter(S_old, N_current)二者唯一的本质区别是测量来源。INTRA 表示"当前 Node 当初就是这样插入当前 Submap 的"；INTER 表示"当前 Node 虽然不是插入这张历史 Submap 的，但扫描匹配证明它们对应同一片环境"。因此，INTER_SUBMAP 负责修正长期漂移，把当前区域和历史区域重新闭合。
其权重是：
w_t = loop_closure_translation_weight
w_r = loop_closure_rotation_weightINTER_SUBMAP 通常还会使用 Huber 鲁棒核。因为回环可能受重复货架、相似走廊、重复柱子影响，少数约束可能不可靠；Huber Loss 会削弱大残差回环的影响，避免一条错误回环把整张地图强行拉坏。
3. 相邻 Node 的 Local SLAM 相对位姿约束
除了 Node-Submap Constraint，OptimizationProblem2D 还会直接在同一 Trajectory 中的连续 Node 之间加入 Local SLAM 相对位姿残差。它不属于 INTRA_SUBMAP 或 INTER_SUBMAP，而是后端为保持轨迹连续性额外加入的 Node-Node 约束。
前端 Local SLAM 给出的相对运动为：
Z_local(i→i+1) =
(T_L←N_i)^-1 × T_L←N_(i+1)全局优化要求：
(T_G←N_i)^-1 × T_G←N_(i+1)
≈
Z_local(i→i+1)这条约束可以理解为："回环允许长期轨迹整体重新排布，但不能破坏相邻 Node 之间前端已经确定的短时间运动关系。"例如 Node 10 和 Node 11 原本只相距 0.2 m，优化后不应被拉开成数米；原本连续转弯的轨迹，也不应被优化成明显折线。
其权重为：
w_t = local_slam_pose_translation_weight
w_r = local_slam_pose_rotation_weight4. 相邻 Node 的 Odom 相对位姿约束
Odom 约束也是 Node-Node 相对约束。系统在两个 Node 的时间戳处，从轮速、编码器、视觉里程计或外部 Odom 队列中插值得到机器人位姿，再计算对应的相对运动。忽略重力对齐和二维投影细节，可先写成：
Z_odom(i→i+1) =
(T_O←B(t_i))^-1 × T_O←B(t_(i+1))其中 O 是 odom 坐标系，B 是机器人 tracking/body 坐标系。经过重力对齐并投影到二维后，最终优化关系为：
(T_G←N_i)^-1 × T_G←N_(i+1)
≈
Z_odom(i→i+1)Odom 约束不提供绝对位置，它只说"机器人从 Node i 到 Node i+1 应该如何移动"。它和 Local SLAM 相邻约束同时存在：Local SLAM 基于激光点云匹配给出相对运动；Odom 基于轮速或外部里程计给出另一份相对运动；优化器根据权重决定两者的综合影响。
其权重为：
w_t = odometry_translation_weight
w_r = odometry_rotation_weight5. Fixed Frame Pose：外部固定参考系约束
Fixed Frame Pose 用于接入 GNSS、UWB、外部定位系统等信息。设外部参考坐标系是 F，外部系统测得的 Node 相对该参考系位姿为 Z_F←N，则约束为：
(T_G←F)^-1 × T_G←N
≈
Z_F←N这里 T_G←F 表示外部参考坐标系在 map 中的位姿，T_G←N 表示 Node 的全局位姿。Fixed Frame 的意义是让轨迹尽量符合外部定位系统，但在当前二维优化中，T_G←F 本身也可以参与优化，因此它不一定天然就是绝对世界原点。是否能提供真正绝对锚点，取决于外部参考系是否具有可信、固定的真实世界定义。
其权重为：
w_t = fixed_frame_pose_translation_weight
w_r = fixed_frame_pose_rotation_weight对于可能跳变的 GNSS、UWB 或外部定位，Cartographer 可使用 Tolerant Loss，降低异常测量的拉扯作用。
6. Landmark：二维码、AprilTag 等标志物约束
Landmark 是带稳定 ID 的环境实体，例如二维码、AprilTag、人工标志物。设 Landmark k 的全局位姿为 T_G←L_k，机器人在观测时刻 t_obs 测得 Landmark 相对于机器人坐标系的观测为 Z_N←L_k，则理想关系为：
(T_G←N(t_obs))^-1 × T_G←L_k
≈
Z_N←L_k二维码或 AprilTag 的观测时间不一定恰好等于某个 Node 的时间，因此源码会使用观测时刻前后的两个 Node，对机器人位姿进行插值，得到 T_G←N(t_obs)。
若 Landmark 没有已知全局坐标，它自身也是优化变量，多次观测会共同确定 Landmark 与轨迹的位置；若 Landmark 被标记为 frozen，其全局位姿固定，便会成为真正的外部绝对锚点。Landmark 的平移、旋转可信度由每次观测自身携带：
w_t = landmark_observation.translation_weight
w_r = landmark_observation.rotation_weightLandmark 残差通常也是鲁棒处理的，因为错误识别二维码、误关联 AprilTag 或观测噪声都可能引入异常。
7. IMU 在二维后端中的位置
当前 Cartographer 2D 的 OptimizationProblem2D 不会将 IMU 直接作为独立 Ceres 残差加入总目标函数。IMU 的主要作用在 Local SLAM 前端：提供重力对齐、姿态预测、点云去畸变和更稳定的局部位姿。因此，IMU 会间接影响 Node 的 local_pose、gravity_alignment 以及 Odom 数据如何投影到二维，但不会单独成为二维 PoseGraph 中的一项残差。
因此二维后端最终约束体系应理解为：
Node-Submap 约束
+
相邻 Node 的 Local SLAM 约束
+
相邻 Node 的 Odom 约束
+
Fixed Frame Pose 约束（可选）
+
Landmark 观测约束（可选）最终带权重的总目标函数
将所有待优化变量写为：
Θ =
{
所有 Node 的 T_G←N_i，
所有 Submap 的 T_G←S_j，
每条 Trajectory 的 T_G←F_t，
所有可优化 Landmark 的 T_G←L_k
}最终要求解的是：
Θ* = argmin_Θ J(Θ)完整总目标函数可以写为：
J(Θ) =
Σ_(e∈E_intra)
|| W_intra,e × δ_intra,e ||²
+ Σ_(e∈E_inter)
ρ_Huber(
|| W_inter,e × δ_inter,e ||²
)
+ Σ_(e∈E_local)
|| W_local,e × δ_local,e ||²
+ Σ_(e∈E_odom)
|| W_odom,e × δ_odom,e ||²
+ Σ_(e∈E_fixed)
ρ_tol(
|| W_fixed,e × δ_fixed,e ||²
)
+ Σ_(e∈E_landmark)
ρ_Huber(
|| W_landmark,e × δ_landmark,e ||²
)这里换行只是为了避免公式框横向滚动，公式本身仍是一整个目标函数。E_intra 是局部插图约束集合；E_inter 是回环约束集合；E_local 是相邻 Node 的 Local SLAM 相对运动集合；E_odom 是 Odom 相对运动集合；E_fixed 是外部固定参考集合；E_landmark 是二维码、AprilTag 等 Landmark 观测集合。
对于前五类二维位姿约束，其权重矩阵通常都是：
W = diag(w_t, w_t, w_r)两个 w_t 分别缩放 x、y 平移误差，w_r 缩放 yaw 误差。Landmark 内部通常保留更完整的三维观测，因此会按自身平移、旋转权重缩放对应残差。
总目标函数中，INTRA、Local SLAM、Odom 默认直接使用平方误差；INTER 回环与 Landmark 常使用 ρ_Huber；Fixed Frame 可按配置使用 ρ_tol。这些鲁棒核不是取消某一条约束，而是当某条约束残差异常大时，降低它继续增大的速度，让优化器不会被一条错误回环、错误二维码或异常 UWB 定位强行带偏。
Huber Loss 可概念化理解为：
ρ_Huber(s) = s，
当 s ≤ δ² 时
ρ_Huber(s) = 2δ√s - δ²，
当 s > δ² 时
其中：s = || W × δ ||²最后，Cartographer 还会固定第一张 Submap：
T_G←S_0 = constant它不是一条带权重的残差，而是一个固定条件。因为若所有 Node 和 Submap 都可以自由移动，则整张地图一起平移、一起旋转后，所有相对约束仍不变，优化器无法确定 map 原点。固定第一张 Submap 的意义就是消除这种整体自由度；Frozen Trajectory 中的 Node 和 Submap 同样会被设为常量，不再参与后续优化。
总结
Cartographer 的 Global SLAM 后端并不是把每帧激光点云简单拼到一张越来越大的地图上，而是把 Local SLAM 前端产生的有效观测组织成一套可优化的空间关系系统。这个系统最上层是 Trajectory，它代表一次独立 SLAM 运行实例；Trajectory 内部持续产生 Node 和 Submap；Node 保存某一时刻被后端正式采用的局部观测；Submap 保存一段时间内逐步积累出来的局部概率地图；Constraint 则记录 Node 与 Submap、Node 与 Node、Node 与外部参考之间应满足的相对关系。
Node 不是原始 LiDAR 帧。原始点云先经过 Local SLAM 的去畸变、位姿预测和扫描匹配，得到 MatchingResult；随后还要经过 MotionFilter 判断，只有当前点云真正插入 ActiveSubmaps2D，产生 InsertionResult 后，GlobalTrajectoryBuilder 才会调用 PoseGraph2D::AddNode。这个机制决定了 Node 是经过前端筛选后的关键观测，而不是高频重复传感器数据的简单复制。这样既能降低后端回环与优化负担，也能保证进入 PoseGraph 的观测具有足够空间变化和局部地图价值。
Node 内部保存的内容也远不止一个 x、y、yaw。它包含时间戳、重力对齐旋转、过滤后的重力对齐点云、Local SLAM 位姿以及可被后端优化更新的 global_pose。其中最关键的是 filtered_gravity_aligned_point_cloud，因为二维回环匹配真正使用的就是这份点云。它保留当前环境中墙体、柱子、货架、门框、通道边缘等主要几何结构，并移除过密、重复或不必要的点。Node 的 local_pose 表示前端 Local SLAM 认为机器人在哪里，强调连续性和实时性；global_pose 则表示后端 PoseGraph 优化后机器人应该在哪里，强调长期全局一致性。回环发生时，主要被修正的是 global_pose 以及 Local-to-Global 变换，而不是前端原始 local_pose。
Node 与 Submap 之间有两种本质不同的关系。第一种是 INTRA_SUBMAP 约束，它来自当前 Node 插入当前 Active Submap 的过程。当前 Node 本来就是在前端扫描匹配得到的位姿下写入这张图的，因此后端不需要重新匹配，只需根据 Node 和 Submap 在 Local SLAM 坐标系中的局部位姿，直接计算 Node 相对于 Submap 的变换。INTRA_SUBMAP 的作用是维持局部骨架：同一张 Submap 中的多个 Node 通过它保持相对一致；相邻 Submap 又通过共享 Node 保持连续。它解决的是局部地图不散、局部轨迹不裂的问题。
第二种是 INTER_SUBMAP 约束，它来自回环或跨区域匹配。最常见的形式确实是"新 Node 匹配旧的完成 Submap"。机器人绕场一圈后重新回到历史区域，当前 Node 的过滤点云会尝试与历史 Finished Submap 的 ProbabilityGrid 对齐。若墙、柱子、货架等结构重合，FastCorrelativeScanMatcher2D 会先在大范围或局部窗口中找到高分候选，CeresScanMatcher2D 再在该候选附近做连续精修，最终得到 Node 相对于历史 Submap 的精确变换。这个变换被包装成 INTER_SUBMAP Constraint，成为后端修正长期累计漂移的关键信息。除了新 Node 匹配旧图，源码还支持旧 Node 匹配刚完成的新 Submap，从而避免由于地图完成时间不同而漏掉回环。
从源码角度看，PoseGraph 中显式保存的 Constraint 对象只有 INTRA_SUBMAP 和 INTER_SUBMAP 两类。但最终的 OptimizationProblem2D 并不只依赖这两类 Node-Submap 关系。它还会加入相邻 Node 之间的 Local SLAM 相对位姿残差，保证前端连续运动关系不会被破坏；若有轮速里程计，则加入相邻 Node 的 Odom 相对位姿残差；若输入 FixedFramePoseData，则加入外部固定坐标系约束；若输入 LandmarkData，例如二维码、AprilTag 或其他标志点观测，则加入 Landmark 观测残差。这些约束共同构成完整的全局优化问题。需要特别注意的是，当前 Cartographer 2D 的 OptimizationProblem2D 不把 IMU 当作独立后端优化残差；IMU 在二维系统中主要服务于前端重力对齐和位姿预测。
最终，Cartographer 不是"用回环直接把机器人拉回原点"，而是把回环变成一条新的空间事实：某个 Node 相对于某张历史 Submap 应该处于某个位置。优化器随后同时调整大量 Node 和 Submap 的 global pose，使 INTRA_SUBMAP、INTER_SUBMAP、Odom、Local SLAM 相邻约束、Landmark 和外部坐标系约束尽可能同时成立。于是局部地图内部结构得以保留，长期积累的漂移被逐步分摊到整条轨迹上，最终形成首尾闭合、全局一致的地图。