SE(3) 刚体变换：位姿状态与协方差的正确写法

1. 欧拉角加减的 silent wrong

把位姿当 $(x, y, z, roll, pitch, yaw)$ 直接滤波，pitch 接近 ±90° 时协方差会给出荒谬 confidence；旋转做欧氏加减则矩阵不正交、NaN 在某一角突然冒出来。SE(3) 不是数学洁癖——是避免 silently wrong 的必要条件，尤其多传感器融合里各模块必须共享同一 perturbation 约定。底盘上 pitch 大幅变化不罕见，用欧拉角协方差会在奇异点附近给出「极准」的假象。

2. SO(3) 与 SE(3) 复合

旋转 $R^{⊤} R = I, det R = 1$ 。刚体位姿：

T = [R t 0 1] \in S E (3)

复合 $T_{a c} = T_{ab} T_{b c}$ ；相对位姿 $T_{ab} = T_{a w} T_{b w}^{- 1}$ （约定因库而异，读文档）。存储用四元数或 $3 \times 3$ 旋转矩阵；更新用李代数小量 + Exp；切忌长期用欧拉角做状态。点变换 $p^{'} = R p + t$ 是写 Jacobian 的出发点，所有 SE(3) 导数最终归结于此。

3. 里程计因子残差

BetweenFactor 标准形式：

r = Log (T^_{ij}^{- 1}, T_{i}^{- 1}, T_{j})

$T^_{ij}$ 来自 VO/LO/预积分；残差在 se(3) 切空间，不在欧拉角上。信息矩阵 $Ω$ 也应在切空间定义——把 VO 输出的 6×6 协方差直接当 edge information 前，确认 VO 侧是否已在 tangent space 给出。LO 给的是 relative pose 与 covariance，进 g2o/GTSAM 前须确认 convention 与符号。

4. 右乘 vs 左乘

右乘： T^{'} = T, Exp (ξ) （扰动在 body ）

左乘： T^{'} = Exp (ξ), T （扰动在 world ）

g2o 默认右乘；GTSAM Pose3::retract 有固定约定。混用 → 雅可比差 $Ad$ → 优化发散或收敛到错解。团队应写进编码规范，PR review 查 Exp 乘在左还是右。

5. 协方差在切空间

$6 \times 6$ 的 $P_{ξ}$ 定义在李代数 $ξ$ 上，不是 $(x, y, z, r, p, y)$ 。ESKF error state、预积分 noise propagation、位姿图 information matrix 全在切空间。把 roll-pitch-yaw 的 $3 \times 3$ 当旋转协方差用，奇异点附近会出大问题——表现为「置信椭圆看起来合理但滤波器行为怪」。VO 输出 covariance 时，确认是否已在 se(3) 定义，再喂给 BetweenFactor。

6. 测地插值

位姿插值：

T (t) = T_{0}, Exp (α, Log (T_{0}^{- 1} T_{1}))

IMU 去畸变、TF 外推、关键帧间预测都用测地线——对 $(R, t)$ 线性插值会产生非物理中间态，高速 rolling shutter 下更明显。四元数 Slerp 是 SO(3) 特例；平移可单独 lerp，旋转必须走 Exp/Log。点云去畸变若对每点用不同 $T (t)$ ，插值质量直接决定畸变残差大小。

7. TF 与优化器对接

ROS geometry_msgs/Pose 是 $(x, y, z, q_{x}, q_{y}, q_{z}, q_{w})$ ；优化器内部是 se(3)。wrapper 在四元数归一化与 Exp/Log 间转换，最易混 left/right——团队应固定一份 TF ↔ tangent 单测，提交前必跑。tf2 的 Pose 转 GTSAM Pose3 的 helper 应集中维护，禁止各节点 copy-paste。Nav2 与 SLAM 若共用 TF tree，world 系定义也须一致。

8. 参数化取舍

表示	优点	缺点
旋转矩阵	无奇异	9 参数冗余
四元数	4 参数、插值 smooth	双覆盖、需归一化
轴角/李代数	优化友好	大角需注意
欧拉角	直觉	万向锁

工程上：状态存四元数或旋转矩阵，优化更新走 Exp/Log。四元数 LERP 非最短路径，旋转插值必须 Slerp 或测地线公式。

9. 常见错误后果

旋转欧氏加减 → 不正交、NaN；欧拉角协方差 → 奇异点 confidence 荒谬；四元数不归一化 → 漂移；位姿 LERP → 非物理中间态。每一条都能在 long run 日志里找到对应 crash 或 drift。

10. 验收

运行时监控 $∣ R^{⊤} R - I ∣_{F}$ 、 $∣ det R - 1∣$ ；
Exp/Log 往返误差；
插值路径上 $∣ Log (T_{k}^{- 1} T_{k + 1}) ∣$ 应平滑；
解析 vs 数值 Jacobian 一致；
TF wrapper 单测覆盖 left/right 两种 perturbation。

SE(3) 写对了，里程计因子、预积分因子、重投影 BA 才能在同一状态空间里说话；写错了，bias 会慢慢「吸收」几何错误，十分钟后才暴雷。把 perturbation 约定写进接口文档，比事后 debug 便宜一个数量级。Multi-session 合并前，先统一 world 系与 body 系定义，再谈 BetweenFactor。URDF 与 SLAM world 不一致时，外参 wrapper 是第一排查点。Docking 场景若 mix 2D 与 3D 状态，更须写清 perturbation。发布前跑一遍 Exp/Log 往返与 BetweenFactor 残差符号单测，纳入 CI 必跑项。