3. 位姿表示与变换
在视觉SLAM中，位姿表示是贯穿前端跟踪、后端优化与回环检测的基础语言。相机在三维空间中的运动包含旋转与平移，如何用紧凑、可微、无奇异的数学形式描述它，直接决定了优化问题的收敛性与工程实现的稳定性。本节从旋转矩阵出发，逐步过渡到四元数、李群李代数，最终落到SE(3)与Sim(3)变换，并说明它们在回环与图优化中的实际角色。
一、核心原理：从旋转矩阵到李群李代数
旋转矩阵 R 是 3×3 正交矩阵，满足 R^T R = I 且 det(R)=1，属于特殊正交群 SO(3)。它直观、便于坐标变换，但存在两个工程痛点：一是用9个参数表达3个自由度，存在冗余约束；二是优化时若直接对矩阵元素求导，更新后不再保持正交性，需要额外正交化。
四元数用4个参数 q = (w, x, y, z) 表示旋转，单位四元数满足 ||q||=1。它无万向锁、插值平滑，适合姿态积分与前端跟踪。但四元数仍有单位模长约束，且对加法不封闭，直接做最小二乘更新同样需要归一化。
李群李代数提供了更优雅的解法。SO(3) 与 SE(3) 是李群，其对应的李代数 so(3) 与 se(3) 是向量空间。通过指数映射 exp 与对数映射 log，可以在无约束的李代数向量与李群元素之间双向转换。优化时在李代数上做加法，再映射回李群，天然保持约束，这正是SLAM后端采用扰动模型求导的根本原因。
二、方法流程：SE(3) 与 Sim(3) 的构造与更新
SE(3) 描述刚体变换，元素为 T = [R t; 0 1]，其中 R ∈ SO(3)，t ∈ ℝ³。其李代数 se(3) 是六维向量 ξ = (ρ, φ)，ρ 为平移部分，φ 为旋转部分。指数映射将 ξ 映射为 T，对数映射反之。在优化中，常用左乘或右乘扰动模型：
T_new = exp(δξ^) · T_old // 左扰动
T_new = T_old · exp(δξ^) // 右扰动
Sim(3) 在 SE(3) 基础上增加尺度因子 s，元素为 S = [sR t; 0 1]，共7自由度。它用于单目SLAM回环检测：单目系统存在尺度漂移，回环时两段轨迹的尺度不一致，必须用 Sim(3) 求解相似变换，才能正确闭合。其李代数 sim(3) 为七维向量 ξ = (ρ, φ, σ)，σ 对应尺度对数。
三、案例/场景：回环检测中的 Sim(3) 校正
假设单目SLAM运行一段时间后检测到回环，当前关键帧位姿为 T_cur，回环匹配关键帧位姿为 T_loop。由于尺度漂移，两者不在同一尺度下。此时构建 Sim(3) 优化问题，最小化重投影误差或位姿误差，求解 S 使得 T_cur ≈ S · T_loop。求解后，将回环约束加入位姿图，对整条轨迹进行全局优化，尺度漂移被均匀分摊到各关键帧。
g2o 或 GTSAM 中的 Sim3 类型。注意：Sim(3) 的雅可比矩阵比 SE(3) 多一维尺度项，若直接复用 SE(3) 的求导代码会导致尺度不收敛。
四、误区与复盘
- 误区一：直接用旋转矩阵做优化变量。矩阵元素更新后不再正交，必须正交化，破坏优化连续性。正确做法是在李代数上扰动。
- 误区二：四元数更新后忘记归一化。长期积分会导致模长漂移，姿态逐渐失真。每次更新后应执行
q.normalize()。 - 误区三：单目回环用 SE(3) 而非 Sim(3)。忽略尺度自由度，回环后尺度漂移依然存在，地图出现重影。
- 误区四：混淆左扰动与右扰动。两者雅可比不同，混用会导致优化方向错误。选定一种后需在全工程保持一致。
| 表示方法 | 参数个数 | 自由度 | 优点 | SLAM 中的典型用途 |
|---|---|---|---|---|
| 旋转矩阵 R | 9 | 3 | 直观，坐标变换直接 | 坐标变换、外参标定 |
| 四元数 q | 4 | 3 | 无万向锁，插值平滑 | 前端姿态积分、IMU 融合 |
| so(3) / se(3) | 3 / 6 | 3 / 6 | 无约束，可微，适合优化 | 后端 BA、位姿图优化 |
| SE(3) | 12 | 6 | 完整刚体变换 | 双目/RGB-D 回环、位姿图 |
| Sim(3) | 16 | 7 | 含尺度，可校正漂移 | 单目回环检测与全局优化 |
Sophus::SE3d 与 Sophus::Sim3d 管理位姿，避免手写矩阵运算。优化时通过 Sophus::SE3d::exp() 与 log() 完成李群李代数转换，保证数值稳定性。
总结来看，旋转矩阵适合表达，四元数适合积分，李代数适合优化，SE(3) 适合刚体回环，Sim(3) 适合单目尺度校正。理解它们之间的映射关系与适用边界，是构建稳定SLAM系统的数学基石。