一、Cartographer 后端先看整体
Cartographer 后端的核心不是单纯"保存地图"，而是做 Pose Graph Optimization（位姿图优化） 。前端 Local SLAM 会不断输出 scan 的局部位姿，并把 scan 插入 submap。后端拿到这些信息后，不是重新做完整前端，而是把 trajectory node（轨迹节点，也就是进入后端优化的一帧 scan） 、submap（子地图，也就是一小段 scan 组成的局部地图） 和 constraint（约束，也就是 node 和 submap / node 和 node / node 和外部观测之间的相对关系） 组织成图。然后后端把这些 constraint 转成 residual（残差，也就是当前估计值和观测值之间的误差），丢给 Ceres 做非线性最小二乘优化。
后端主线可以理解成：
前端输出 node + insertion_submaps
↓
PoseGraph2D / PoseGraph3D 接收 node
↓
记录 node 和 submap 的关系
↓
ConstraintBuilder2D / ConstraintBuilder3D 构建约束
↓
OptimizationProblem2D / OptimizationProblem3D 把约束变成残差
↓
Ceres 优化所有 node 和 submap 的 global pose
↓
地图和轨迹整体变得更一致其中 insertion_submaps（当前 scan 插入过的 active submap 列表）很关键，因为它决定了当前 node 和哪些 submap 天然存在局部约束。global pose（全局位姿）也很关键，它是后端优化的结果；前端给的是局部位姿，后端要把所有局部位姿调整到全局一致。
二、Cartographer 2D 后端详细讲解
1. 2D 后端优化什么？
2D 后端优化的是平面位姿，也就是每个 node 和每个 submap 的：
x：平面 x 方向位置
y：平面 y 方向位置
yaw：绕 z 轴的旋转角，也就是车头朝向所以在 2D 里，一个 node 的全局位姿可以理解成：
T_node = [x, y, yaw]一个 submap 的全局位姿也可以理解成：
T_submap = [x, y, yaw]后端要做的是：调整所有 T_node（每个轨迹节点的全局位姿） 和 T_submap（每个子地图的全局位姿），让所有约束尽量同时满足。
2. 2D 后端构建了哪些约束？
2D 后端主要构建 6 类约束：
1. INTRA_SUBMAP 约束：node 和它插入过的 active submap 之间的局部约束。
2. INTER_SUBMAP 约束：node 和历史 finished submap 之间的跨子地图 / 回环约束。
3. Odometry 约束：相邻 node 之间的里程计相对运动约束。
4. Local SLAM pose 约束：相邻 node 之间的前端局部位姿连续性约束。
5. Fixed frame pose 约束：node 和 GPS / 外部固定坐标系之间的约束。
6. Landmark 约束：node 和二维码 / AprilTag / 人工地标之间的观测约束。下面分开讲，先讲"约束怎么构建"，再讲"残差怎么构建"。
3. 2D 第一类约束：INTRA_SUBMAP 约束
3.1 INTRA_SUBMAP 是什么？
INTRA_SUBMAP（子地图内部约束）来自当前 scan 插入 active submap 的关系。当前 scan 被前端匹配到了 active submap，并且插入了这个 submap，那么后端就知道：这个 node 应该在这个 submap 的某个相对位置上。
比如当前 node_j（第 j 个轨迹节点） 插入了 submap_i（第 i 个子地图），那么就可以建立：
node_j 相对于 submap_i 的位姿应该等于 zbar_ij这里 zbar_ij（scan-to-submap 得到的观测相对位姿） 表示：node_j（当前轨迹节点） 在 submap_i（当前子地图） 坐标系下的位置和朝向。
3.2 INTRA_SUBMAP 怎么构建？
INTRA_SUBMAP 不需要重新做大范围回环搜索，因为当前 node 本来就是插入这个 active submap 的。它主要来自前端已经算好的局部关系。
流程是：
LocalTrajectoryBuilder2D 得到当前 scan 的 local_pose
↓
当前 scan 插入 active submap
↓
PoseGraph2D::AddNode() 接收 node 和 insertion_submaps
↓
ComputeConstraintsForNode() 遍历 insertion_submaps
↓
为每个 active submap 构建 INTRA_SUBMAP constraint其中 local_pose（前端局部位姿） 是前端 scan matching 后得到的当前 scan 在 local 坐标系下的位置。insertion_submaps（当前 scan 插入的 active submap 列表） 决定了当前 node 和哪些 submap 有局部约束。
3.3 INTRA_SUBMAP 的作用
INTRA_SUBMAP 的作用是保持局部一致性。它告诉后端：这个 node 本来就在这个 submap 里，二者之间的局部相对关系是比较可信的。全局优化可以调整整条轨迹和所有 submap 的全局位置，但不要把一个 submap 内部的 scan 随便拉散。
简单说：
INTRA_SUBMAP 约束负责"局部不要乱"。4. 2D 第二类约束：INTER_SUBMAP 约束
4.1 INTER_SUBMAP 是什么？
INTER_SUBMAP（跨子地图约束）通常就是回环约束。它不是当前 node 插入 active submap 得来的，而是后端额外拿某个 node 去和历史 finished submap 匹配。如果匹配成功，就说明这个 node 和这个历史 submap 实际上能对齐，于是建立一条跨子地图约束。
比如机器人走了一圈回到原来的区域，当前 node_j（当前轨迹节点） 的激光点云可以和历史 submap_i（已经完成的历史子地图） 对齐，那么后端就会建立：
node_j 应该位于 submap_i 的某个相对位置 zbar_ij 上这个 zbar_ij（匹配得到的相对位姿观测） 是后端 scan-to-submap matching 重新算出来的。
4.2 INTER_SUBMAP 怎么构建？
INTER_SUBMAP 的构建比 INTRA_SUBMAP 复杂，需要后端重新匹配。
流程是：
选择候选 node 和 finished submap
↓
根据当前全局估计计算 initial_relative_pose
↓
判断距离是否超过 max_constraint_distance
↓
如果太远，跳过
↓
FastCorrelativeScanMatcher2D 粗匹配
↓
得到 score 和 coarse_pose
↓
如果 score < min_score，丢弃
↓
CeresScanMatcher2D 精匹配
↓
得到 refined_pose
↓
构建 INTER_SUBMAP constraint这里 initial_relative_pose（node 相对 submap 的初始猜测位姿） 是根据当前后端已有的 global pose 算出来的。它不是最终结果，只是告诉匹配器："大概从这里附近开始搜"。max_constraint_distance（允许尝试构建约束的最大距离） 用来过滤明显离得太远的 node-submap 候选。FastCorrelativeScanMatcher2D（快速相关扫描匹配器） 用来做粗匹配，快速找一个大概对齐的位置。score（匹配得分） 表示当前 node 点云和 submap 概率栅格对齐得好不好。min_score（最低接受得分） 是过滤坏约束的阈值。coarse_pose（粗匹配位姿） 是粗匹配结果。CeresScanMatcher2D（Ceres 精匹配器） 会在 coarse_pose 附近继续优化，得到更精确的 refined_pose（精匹配后的相对位姿）。
4.3 INTER_SUBMAP 的作用
INTER_SUBMAP 主要负责修正长期漂移。前端 Local SLAM 只关注局部 scan-to-submap，所以时间长了会有累计误差。INTER_SUBMAP 一旦匹配成功，就相当于告诉后端：
当前 node 和历史 submap 实际上能对齐，你之前积累的误差需要整体调整。简单说：
INTER_SUBMAP 约束负责"回到老地方时把轨迹拉回来"。5. 2D 第三类约束：Odometry 约束
5.1 Odometry 约束是什么？
Odometry（里程计）一般来自轮速编码器、轮式里程计、外部里程计等。它提供相邻时刻之间机器人移动了多少。
Odometry 约束表达的是：
node_i 到 node_j 的相对运动，应该接近 odom_ij其中 node_i（前一个轨迹节点） 和 node_j（后一个轨迹节点） 是相邻或相近的两个节点，odom_ij（里程计测得的相对位姿） 是里程计给出的两帧之间的运动。
5.2 Odometry 约束怎么构建？
后端会根据 node 的时间戳找到对应的里程计数据，然后插值得到相邻两个 node 的里程计位姿。再计算：
odom_ij = odom_i^-1 * odom_j其中 odom_i（node_i 时刻的里程计位姿） 和 odom_j（node_j 时刻的里程计位姿） 是里程计在两个 node 时间点的位姿。这个 odom_ij（里程计相对运动） 就作为观测约束加入优化。
5.3 Odometry 约束的作用
Odometry 约束主要增强短期连续性。比如激光在长走廊里容易退化，scan matching 的约束不强，此时里程计可以告诉系统：这两帧之间大概移动了多少。
简单说：
Odometry 约束负责"短时间运动不要乱跳"。6. 2D 第四类约束：Local SLAM pose 约束
6.1 Local SLAM pose 约束是什么？
Local SLAM pose（前端局部位姿）是前端 Local SLAM 估计出来的 node 局部位姿。Local SLAM pose 约束表达的是：相邻 node 的优化结果，应该不要严重偏离前端给出的局部相对运动。
假设有两个相邻节点：node_i（前一个轨迹节点） 和 node_j（后一个轨迹节点）。前端给出它们的局部位姿：local_pose_i（node_i 的前端局部位姿） 和 local_pose_j（node_j 的前端局部位姿）。那么可以构建观测：
local_relative_ij = local_pose_i^-1 * local_pose_j其中 local_relative_ij（前端局部相对运动） 表示前端认为两个 node 之间移动了多少。
6.2 Local SLAM pose 约束的作用
它的作用是保护局部轨迹形状。全局优化时，回环约束可能会拉动整条轨迹，如果没有 local SLAM pose 约束，局部轨迹可能被拉得很扭曲。这个约束会告诉后端：
你可以调整全局形状，但相邻 node 的局部运动关系不要太离谱。简单说：
Local SLAM pose 约束负责"局部轨迹形状不要被拉坏"。7. 2D 第五类约束：Fixed frame pose 约束
7.1 Fixed frame pose 约束是什么？
Fixed frame（固定坐标系）可以理解成一个外部全局参考坐标系，比如 GPS 坐标系、外部定位系统坐标系、工厂地图坐标系等。Fixed frame pose 约束表达的是：
某个 node 的全局位置应该接近外部固定坐标系给出的观测。比如 GPS 说机器人在某个时刻的位置是 gps_pose（GPS 或外部定位给出的位姿），后端就可以把当前 node 的 global pose 和这个 gps_pose 之间的差做成残差。
7.2 Fixed frame pose 约束的作用
它提供全局锚点。如果只有激光和里程计，地图可能整体漂移；fixed frame 约束能把地图拉到外部坐标系上。
简单说：
Fixed frame pose 约束负责"地图不要整体漂走"。8. 2D 第六类约束：Landmark 约束
8.1 Landmark 约束是什么？
Landmark（地标）可以是二维码、AprilTag、反光柱、人工标志物、已知地图点等。Landmark 约束表达的是：
node 在某个时刻看到了某个 landmark，
那么 node 和 landmark 之间的相对位置应该等于观测值。比如机器人看到一个二维码，观测到二维码相对机器人是 landmark_observation（地标相对机器人观测），如果二维码在地图中是稳定的，那么这个观测可以作为强约束加入后端。
8.2 Landmark 约束的作用
Landmark 约束可以在特定位置提供强修正，尤其适合走廊、重复场景、点云退化区域。
简单说：
Landmark 约束负责"看到明确地标时强制校正位置"。9. 2D：约束和残差分开讲
9.1 约束是什么？
约束是"观测关系"。比如 node-submap 约束说：
node_j 在 submap_i 坐标系下的观测相对位姿是 zbar_ij其中 zbar_ij（node_j 相对 submap_i 的观测位姿） 是由前端插入关系或者后端 scan-to-submap 匹配得到的。
9.2 残差是什么？
残差是"当前估计和观测之间的差"。对于 node-submap 约束，当前优化变量有两个：
T_i：submap_i 的当前全局位姿估计
T_j：node_j 的当前全局位姿估计根据当前估计，可以算出预测相对位姿：
z_pred_ij = T_i^-1 * T_j其中 z_pred_ij（由当前优化变量预测出来的 node 相对 submap 的位姿） 应该接近 zbar_ij（匹配或插入得到的观测相对位姿）。
所以残差就是：
error_ij = zbar_ij^-1 * z_pred_ij其中 error_ij（当前估计与观测之间的误差） 在 2D 里可以拆成 dx（x 方向误差）、dy（y 方向误差） 和 dtheta（yaw 角误差）。
最终 residual 可以理解成：
residual = [
translation_weight * dx,
translation_weight * dy,
rotation_weight * dtheta
]其中 translation_weight（平移误差权重） 控制平移误差对优化的影响，rotation_weight（旋转误差权重） 控制角度误差对优化的影响。
9.3 为什么要有残差？
因为 Ceres 优化器最小化的是 residual，而不是直接最小化"约束"这个词。约束是观测，残差是观测和当前估计之间的误差。Cartographer 后端就是把所有约束都变成 residual block（Ceres 中的一块误差函数），然后统一优化。
10. 2D 代码：构建 node-submap 约束
下面是理解版代码，每一行都有注释。
void BuildNodeSubmapConstraint2D( // 构建一个 2D node-submap 约束
const SubmapId& submap_id, // submap_id 表示当前要匹配的子地图编号
const NodeId& node_id, // node_id 表示当前要匹配的轨迹节点编号
const Rigid2d& initial_relative_pose) { // initial_relative_pose 表示 node 相对 submap 的初始猜测
const Submap2D* submap = GetSubmap2D(submap_id); // 根据 submap_id 取出目标 2D 子地图
const TrajectoryNode::Data* node_data = GetNodeData(node_id); // 根据 node_id 取出当前 node 的点云和位姿数据
double distance = initial_relative_pose.translation().norm(); // 计算 node 和 submap 的初始相对平移距离
if (distance > options_.max_constraint_distance()) { // 如果距离超过最大允许构建约束距离
return; // 直接跳过这个候选，避免无意义匹配
} // 距离过滤结束
FastCorrelativeScanMatcher2D* fast_matcher = // 定义快速相关扫描匹配器
GetFastMatcherForSubmap(submap_id); // 根据 submap_id 获取对应的 FastCorrelativeScanMatcher2D
Rigid2d coarse_pose; // coarse_pose 保存粗匹配得到的相对位姿
float score = 0.0f; // score 保存 scan 和 submap 的匹配得分
bool success = fast_matcher->Match( // 调用快速相关匹配器进行粗匹配
initial_relative_pose, // 输入初始相对位姿作为搜索中心
node_data->filtered_gravity_aligned_point_cloud, // 输入当前 node 的滤波点云
options_.min_score(), // 输入最低匹配得分阈值
&score, // 输出匹配得分
&coarse_pose); // 输出粗匹配相对位姿
if (!success) { // 如果粗匹配失败
return; // 不构建这条约束
} // 粗匹配结果判断结束
Rigid2d refined_pose = CeresScanMatch2D( // 调用 CeresScanMatcher2D 做精匹配
submap->grid(), // 输入 submap 的概率栅格地图
node_data->filtered_gravity_aligned_point_cloud, // 输入当前 node 的滤波点云
coarse_pose); // 使用粗匹配结果作为 Ceres 初值
Constraint constraint; // 创建一个约束对象
constraint.submap_id = submap_id; // 记录这条约束连接哪个 submap
constraint.node_id = node_id; // 记录这条约束连接哪个 node
constraint.pose.zbar_ij = Embed3D(refined_pose); // 保存 node 在 submap 坐标系下的观测相对位姿
constraint.pose.translation_weight = options_.translation_weight(); // 设置这条约束的平移残差权重
constraint.pose.rotation_weight = options_.rotation_weight(); // 设置这条约束的旋转残差权重
constraint.tag = Constraint::INTER_SUBMAP; // 把这条约束标记为跨子地图约束
constraints_.push_back(constraint); // 把构建成功的约束保存到约束列表
} // 2D node-submap 约束构建完成11. 2D 代码：约束变成残差
void AddNodeSubmapResidual2D( // 把 2D node-submap 约束加入 Ceres
ceres::Problem* problem, // problem 是 Ceres 优化问题对象
const Constraint& constraint, // constraint 是已经构建好的 node-submap 约束
CeresPose2D* submap_pose, // submap_pose 是 submap 的待优化全局位姿
CeresPose2D* node_pose) { // node_pose 是 node 的待优化全局位姿
ceres::LossFunction* loss = nullptr; // 默认不使用鲁棒核函数
if (constraint.tag == Constraint::INTER_SUBMAP) { // 如果这条约束是跨子地图约束
loss = new ceres::HuberLoss(options_.huber_scale()); // 使用 HuberLoss 降低错误回环的影响
} // 鲁棒核函数设置完成
auto* cost_function = CreateAutoDiffSpaCostFunction( // 创建 SPA 残差函数
constraint.pose); // constraint.pose 里面包含 zbar_ij 和权重
problem->AddResidualBlock( // 向 Ceres 添加一个 residual block
cost_function, // cost_function 负责计算预测相对位姿和观测相对位姿之间的误差
loss, // loss 用于降低错误 INTER_SUBMAP 约束的影响
submap_pose->data(), // 第一个优化变量是 submap 的全局位姿
node_pose->data()); // 第二个优化变量是 node 的全局位姿
} // 2D 残差添加完成12. 2D 总结
Cartographer 2D 后端可以理解成一个平面位姿图优化系统。它不是简单地把前端给出的位姿直接当最终结果，而是把所有进入后端的 scan 变成 trajectory node，把一段段局部地图变成 submap，然后在 node 和 submap 之间建立约束。2D 中每个 node 和 submap 的优化变量主要是 x、y、yaw，也就是平面位置和朝向。后端最终要调整这些变量，让所有约束对应的 residual 尽量小。
2D 后端构建的第一类重要约束是 INTRA_SUBMAP。它来自当前 scan 插入 active submap 的关系。当前 node 本来就属于这个 submap，所以二者之间的相对位姿是局部可信的。这个约束的作用是保护局部结构，避免全局回环优化时把一个 submap 内部的 scan 拉散。它解决的是"局部不要乱"的问题。第二类重要约束是 INTER_SUBMAP。它来自 node 和历史 finished submap 的重新 scan-to-submap 匹配。后端会先根据当前估计计算 initial_relative_pose，距离太远就跳过；距离合理就用 FastCorrelativeScanMatcher2D 粗匹配，得到 score 和 coarse_pose；如果 score 达标，再用 CeresScanMatcher2D 精匹配，得到 refined_pose，并把它作为 zbar_ij 保存成约束。这个约束解决的是"长期漂移和回环修正"的问题。
约束和残差一定要分清楚。约束是观测关系，比如 node_j 在 submap_i 坐标系下的观测相对位姿是 zbar_ij。残差是当前优化变量算出来的预测相对位姿和这个观测之间的差。优化变量是 T_i 和 T_j，分别表示 submap_i 和 node_j 的当前全局位姿。根据当前变量可以算出 z_pred_ij = T_i^-1 * T_j。然后 error_ij = zbar_ij^-1 * z_pred_ij。2D 中这个误差可以拆成 dx、dy、dtheta，乘以平移权重和旋转权重后加入 Ceres。Ceres 会同时最小化所有这些 residual block，从而调整所有 node 和 submap 的全局位姿。
除了 node-submap 约束，2D 后端还可以构建 Odometry 约束、Local SLAM pose 约束、Fixed frame pose 约束和 Landmark 约束。Odometry 约束负责短期运动连续性，Local SLAM pose 约束负责保护前端局部轨迹形状，Fixed frame pose 约束负责给地图提供外部坐标锚点，Landmark 约束负责在识别到固定地标时提供强修正。最终这些约束都会变成 Ceres residual。Cartographer 2D 后端减少误差的核心，就是把局部 scan-to-submap 关系、回环关系、里程计关系、外部坐标关系和地标关系统一放进一个优化问题里，让它们整体误差最小。
三、Cartographer 3D 后端
13. 3D 后端优化什么？
3D 后端优化的是完整 6DoF 位姿：
x：三维空间 x 方向位置
y：三维空间 y 方向位置
z：高度方向位置
roll：绕 x 轴旋转
pitch：绕 y 轴旋转
yaw：绕 z 轴旋转代码里通常不会直接用 roll、pitch、yaw 存旋转，而是用 rotation（旋转，一般是四元数） 和 translation（三维平移向量） 表示一个 3D 位姿。
所以 3D 位姿可以写成：
T = [R, t]其中 R（旋转） 表示姿态，t（平移） 表示位置。
14. 3D 后端构建了哪些约束？
3D 后端主要构建 8 类约束：
1. 3D INTRA_SUBMAP 约束：node 和 active 3D submap 的局部约束。
2. 3D INTER_SUBMAP 约束：node 和历史 finished 3D submap 的回环约束。
3. IMU Rotation 约束：相邻 node 的旋转变化应该接近 IMU 陀螺仪积分。
4. IMU Acceleration 约束：连续 node 的运动变化应该接近 IMU 加速度积分。
5. Odometry 约束：相邻 node 的相对运动应该接近外部里程计。
6. Local SLAM pose 约束：相邻 node 的相对运动应该接近前端 local pose。
7. Fixed frame pose 约束：node 应该接近 GPS / 外部固定坐标观测。
8. Landmark 约束：node 和 landmark 的相对关系应该接近地标观测。下面分开讲。
15. 3D 第一类约束：INTRA_SUBMAP 约束
3D 的 INTRA_SUBMAP 和 2D 一样，来自当前 3D scan 插入 active submap 的关系。区别是 3D 约束是完整 6DoF 的。也就是说，它不仅约束 x、y、yaw，还约束 z、roll、pitch。
它表达的是：
node_j 相对于 submap_i 的 6DoF 位姿应该等于 zbar_ij其中 zbar_ij（node_j 在 submap_i 坐标系下的观测 3D 相对位姿） 包含三维平移和三维旋转。
它的作用是：
保持局部 3D 子地图内部结构稳定。16. 3D 第二类约束：INTER_SUBMAP 约束
3D 的 INTER_SUBMAP 是 3D 回环约束。构建流程是：
选择候选 node 和 finished 3D submap
↓
计算 initial_relative_pose
↓
距离过远则跳过
↓
FastCorrelativeScanMatcher3D 粗匹配
↓
score 太低则丢弃
↓
CeresScanMatcher3D 精匹配
↓
得到 6DoF refined_pose
↓
生成 3D INTER_SUBMAP constraint其中 FastCorrelativeScanMatcher3D（3D 快速相关扫描匹配器） 做粗搜索，CeresScanMatcher3D（3D Ceres 精匹配器） 做精修，refined_pose（精匹配得到的 node 相对 submap 的 3D 位姿） 会作为 zbar_ij（观测相对位姿） 存到 constraint 里面。
它的作用是修正：
x、y 平面漂移；
z 高度漂移；
roll / pitch 姿态漂移；
yaw 朝向漂移；
长期累计误差。17. 3D 第三类约束：IMU Rotation 约束
17.1 它是什么？
IMU Rotation 约束来自 IMU 陀螺仪。陀螺仪测量角速度，把两个 node 之间的角速度积分起来，就得到 delta_rotation（两个 node 时间段内的 IMU 旋转增量）。
它约束的是：
R_i^-1 * R_j ≈ delta_rotation其中 R_i（node_i 的旋转） 是前一个 node 的姿态，R_j（node_j 的旋转） 是后一个 node 的姿态，delta_rotation（IMU 积分得到的旋转变化） 是 IMU 给出的观测。
17.2 它的作用
IMU Rotation 约束主要抑制姿态漂移，尤其是：
roll 漂移；
pitch 漂移；
yaw 短期漂移。点云在结构单一场景中可能约束不住姿态，IMU 可以提供额外姿态变化约束。
18. 3D 第四类约束：IMU Acceleration 约束
18.1 它是什么？
IMU Acceleration 约束来自 IMU 加速度计。加速度和位置是二阶关系，所以这个约束通常会用三个连续 node 的平移来构建。
比如有 node_i（第一个节点）、node_j（第二个节点）、node_k（第三个节点）。根据这三个节点的位置变化，可以估计轨迹的加速度趋势；IMU 加速度积分也可以得到 delta_velocity（IMU 积分得到的速度变化）。优化器会让这两者尽量一致。
18.2 它约束什么？
它约束：
由三个 node 的 translation 推出来的运动变化
应该接近 IMU 加速度积分得到的 delta_velocity其中 translation（三维平移） 包含 x、y、z。这个残差还会涉及 gravity_constant（重力大小参数） 和 imu_calibration（IMU 标定参数）。
18.3 它的作用
IMU Acceleration 约束主要抑制：
z 方向高度漂移；
roll / pitch 姿态不稳定；
重力方向估计错误；
三维轨迹上下漂。19. 3D 其余约束
3D 的 Odometry 约束（外部里程计相对运动约束）、Local SLAM pose 约束（前端局部连续性约束）、Fixed frame pose 约束（外部固定坐标锚点约束）、Landmark 约束（地标观测约束） 和 2D 思路一致，只是位姿从 2D 的 [x, y, yaw] 变成 3D 的 [R, t]。
20. 3D：约束和残差分开讲
20.1 3D node-submap 约束
3D node-submap 约束说的是：
node_j 在 submap_i 坐标系下的观测相对位姿是 zbar_ij其中 zbar_ij（3D scan-to-submap 匹配得到的相对位姿） 包含旋转和平移。
20.2 3D node-submap 残差
当前优化变量是 T_i（submap_i 的全局 3D 位姿） 和 T_j（node_j 的全局 3D 位姿）。根据它们可以预测：
z_pred_ij = T_i^-1 * T_j其中 z_pred_ij（当前变量预测出的 node 相对 submap 的 3D 位姿） 应该接近 zbar_ij（观测相对位姿）。
残差是：
error_ij = zbar_ij^-1 * z_pred_ij3D 里这个误差可以拆成：
translation_error：dx、dy、dz
rotation_error：droll、dpitch、dyaw最终 residual 是：
residual = [
translation_weight * dx,
translation_weight * dy,
translation_weight * dz,
rotation_weight * droll,
rotation_weight * dpitch,
rotation_weight * dyaw
]21. 3D 代码：添加 IMU Rotation 残
void AddImuRotationResidual3D( // 添加 3D IMU 旋转残差
ceres::Problem* problem, // problem 是 Ceres 优化问题
const NodeId& first_node_id, // first_node_id 是前一个 node 的编号
const NodeId& second_node_id, // second_node_id 是后一个 node 的编号
const ImuIntegrationResult& imu_result, // imu_result 保存两个 node 之间的 IMU 积分结果
CeresPoseMap3D* C_nodes, // C_nodes 保存所有 node 的 Ceres 位姿参数
TrajectoryData* trajectory_data) { // trajectory_data 保存轨迹级数据，例如 IMU 标定
Quaterniond delta_rotation = imu_result.delta_rotation; // delta_rotation 是 IMU 陀螺积分得到的旋转增量
auto* cost_function = // 创建 Ceres 旋转残差函数
RotationCostFunction3D::CreateAutoDiffCostFunction( // 使用 Cartographer 的 3D RotationCostFunction
options_.rotation_weight(), // rotation_weight 是 IMU 旋转残差权重
delta_rotation); // 输入 IMU 观测到的相对旋转
problem->AddResidualBlock( // 向 Ceres 添加一个 residual block
cost_function, // cost_function 比较 R_i^-1 * R_j 和 delta_rotation 的差
nullptr, // 这里不使用鲁棒核函数
C_nodes->at(first_node_id).rotation(), // 优化变量 1：第一个 node 的旋转
C_nodes->at(second_node_id).rotation(), // 优化变量 2：第二个 node 的旋转
trajectory_data->imu_calibration.data()); // 优化变量 3：IMU 标定参数
} // IMU Rotation 残差添加结束22. 3D 代码：添加 IMU Acceleration 残差
void AddImuAccelerationResidual3D( // 添加 3D IMU 加速度残差
ceres::Problem* problem, // problem 是 Ceres 优化问题
const NodeId& first_node_id, // first_node_id 是第一个 node
const NodeId& second_node_id, // second_node_id 是第二个 node
const NodeId& third_node_id, // third_node_id 是第三个 node
const Vector3d& delta_velocity, // delta_velocity 是 IMU 积分得到的速度变化
double first_duration, // first_duration 是第一个时间段长度
double second_duration, // second_duration 是第二个时间段长度
CeresPoseMap3D* C_nodes, // C_nodes 保存所有 node 的 Ceres 位姿参数
TrajectoryData* trajectory_data) { // trajectory_data 保存重力和 IMU 标定参数
auto* cost_function = // 创建 Ceres 加速度残差函数
AccelerationCostFunction3D::CreateAutoDiffCostFunction( // 使用 Cartographer 的 3D AccelerationCostFunction
options_.acceleration_weight(), // acceleration_weight 是加速度残差权重
delta_velocity, // 输入 IMU 积分得到的速度变化观测
first_duration, // 输入第一个时间段
second_duration); // 输入第二个时间段
problem->AddResidualBlock( // 向 Ceres 添加 residual block
cost_function, // cost_function 比较轨迹二阶运动和 IMU 加速度积分
nullptr, // 这里不使用鲁棒核函数
C_nodes->at(second_node_id).rotation(), // 优化变量 1：第二个 node 的旋转
C_nodes->at(first_node_id).translation(), // 优化变量 2：第一个 node 的平移
C_nodes->at(second_node_id).translation(), // 优化变量 3：第二个 node 的平移
C_nodes->at(third_node_id).translation(), // 优化变量 4：第三个 node 的平移
&trajectory_data->gravity_constant, // 优化变量 5：重力大小参数
trajectory_data->imu_calibration.data()); // 优化变量 6：IMU 标定参数
} // IMU Acceleration 残差添加结束22. 3D 总结
Cartographer 3D 后端和 2D 后端的框架一致，但复杂度更高。2D 只优化 x、y、yaw，而 3D 要优化 x、y、z、roll、pitch、yaw。代码中通常用 translation 表示三维平移，用 rotation 表示三维旋转。因为多了 z、roll、pitch 等自由度，3D 后端更容易遇到高度漂移、姿态漂移和点云几何退化问题，所以 3D 不仅依赖 node-submap 约束，还明显依赖 IMU 约束。
3D 中同样有 INTRA_SUBMAP 和 INTER_SUBMAP 两类 node-submap 约束。INTRA_SUBMAP 来自当前 3D scan 插入 active submap 的关系，用于保持局部三维子地图结构稳定。INTER_SUBMAP 来自历史 node 和 finished 3D submap 的重新匹配，用于回环和跨子地图漂移修正。构建 INTER_SUBMAP 时，后端会先计算 initial_relative_pose，然后通过 FastCorrelativeScanMatcher3D 粗匹配，得分合格后再用 CeresScanMatcher3D 精匹配，最终得到 6DoF 的 zbar_ij。这个 zbar_ij 表示 node 在 submap 坐标系下的观测相对位姿。
3D node-submap 残差的构建方式和 2D 本质相同。当前优化变量是 T_i 和 T_j，分别表示 submap_i 和 node_j 的全局 3D 位姿。优化器根据它们预测 z_pred_ij = T_i^-1 * T_j，然后和观测 zbar_ij 比较，得到 error_ij = zbar_ij^-1 * z_pred_ij。3D 中 error_ij 包含 dx、dy、dz 这样的平移误差，也包含 droll、dpitch、dyaw 这样的旋转误差。它们乘以 translation_weight 和 rotation_weight 后，作为 residual block 加入 Ceres。
3D 后端最关键的额外约束是 IMU Rotation 和 IMU Acceleration。IMU Rotation 约束来自陀螺仪积分。两个 node 之间，IMU 可以给出 delta_rotation，优化器根据当前 node 的 rotation 也能预测 R_i^-1 * R_j，两者之间的差就是 rotation residual。它主要抑制 roll、pitch、yaw 漂移。IMU Acceleration 约束来自加速度计，它通常关联三个连续 node 的 translation，因为加速度和位置是二阶关系。它会利用 delta_velocity、gravity_constant 和 imu_calibration 来约束轨迹运动变化，主要抑制 z 方向漂移、重力方向错误和三维轨迹上下漂。
所以 3D 后端减少误差的核心是多源约束融合。INTRA_SUBMAP 保持局部 3D 结构，INTER_SUBMAP 修正全局回环和长期漂移，IMU Rotation 稳定姿态变化，IMU Acceleration 稳定高度和重力方向，Odometry 保持短期运动连续，Local SLAM pose 保护局部轨迹形状，Fixed frame pose 提供外部全局锚点，Landmark 在特定区域提供强修正。最后这些约束都被转换成 Ceres residual block，Ceres 统一优化所有 node 和 submap 的 6DoF 位姿。Cartographer 3D 后端的本质就是：把点云匹配、IMU、里程计、外部定位和地标观测全部转成残差，通过全局非线性优化减少三维轨迹和子地图的累计误差。