SO(3) 李代数：BA 与预积分的切空间语言

1. 为什么不能 R += ΔR

旋转矩阵 $R \in S O (3)$ 不在向量空间里——直接 R + Δ 会丢正交性，滤波几步后出现 NaN。BA、位姿图、IMU 预积分都在切空间（李代数）做加法，再用指数映射回到流形。看 VINS 附录里满屏 Jr、Exp，本质就是这套语言；抄雅可比而不懂来源，改一个 convention 全线发散。李群工具不是数学选修，而是读懂预积分与重投影导数的最低门槛。

2. 指数映射与对数映射

轴角 $ϕ \in R^{3}$ ， $θ = ∣ ϕ ∣$ ，Rodrigues 公式：

R = Exp (ϕ) = I + \frac{sin θ}{θ} [ϕ] \times + \frac{1 - cos θ}{θ ^{2}} [ϕ] \times^{2}

逆映射 $ϕ = Log (R)$ 。小角近似 $R \approx I + [ϕ]_{\times}$ 只在 $∣ ϕ ∣ ≲ 5°$ 可靠；大回环修正必须用完整 Exp，否则矩阵不正交。底盘上做回环 PGO 时，单帧修正 30° 仍常见，不能用小角糊弄。实现 Exp/Log 后先做往返单元测试，再对接 BA。

3. 右雅可比 Jr

右乘扰动框架：

Exp (ϕ + δ ϕ) \approx Exp (ϕ), Exp (J_{r} (ϕ), δ ϕ)

$J_{r}$ 把切空间增量映射到组合后的旋转。预积分 bias 一阶修正、位姿图 uncertainty 传播都依赖它；小角时 $J_{r} (ϕ) \approx I - \frac{1}{2} [ϕ]_{\times}$ 。全系统必须统一右乘或左乘——混用是雅可比符号错的头号原因；左乘对应 $J_{l}$ ，差一个伴随变换。对接两个库时，先用同一组 $(R, t, p_{w})$ 对比解析与数值 Jacobian，比读 README 快。

4. 伴随 Ad

把 body 系扰动变到 world 系：

Ad T = [R 0 [t] \times R R], ξ_{w} = Ad_{T}, ξ_{b}

多传感器外参、IMU noise propagation 都走 Ad。读 g2o oplus 或 GTSAM retract 前先确认是左还是右；对接新库 grep boxplus/retract 比读 README 可靠。外参标定误差在 Ad 链上放大，表现为多传感器融合时长时 drift 方向固定。

5. SE(3) 与重投影链

$ξ = [ρ, ϕ]^{⊤} \in R^{6}$ ，位姿更新 $T^{'} = T, Exp (δ ξ)$ 。相机系点 $p_{c} = R p_{w} + t$ ，对 $δ ξ$ ：

\frac{\partial p _{c}}{\partial δ ξ} = [I - R [p_{w}]_{\times}]

再乘投影 Jacobian 得重投影残差对位姿的导数。手推一次 $\partial p_{c} / \partial δ ϕ = - R [p_{w}]_{\times}$ ，以后看 ORB-SLAM ProjectJac 就不会懵——这一 afternoon 的投入回报率极高。链式法则顺序是：投影 → 相机系坐标 → 位姿扰动，任一层符号反都会表现为「能收敛但地图扭」。

6. 与 SLAM 模块的对应

模块	工具
重投影 BA	SE(3) 右扰动 + 投影链
IMU 预积分	SO(3) Exp/Log + $J_{r}$ + Ad
位姿图	$Log (T_{i}^{- 1} T_{j})$ 残差
ESKF	error state 在 se(3) 更新

读 Forster 预积分 appendix 时，对照上表标记每处 Jr/Ad 出现的位置——第二遍会快很多。g2o VertexSE3::oplus 与 GTSAM Pose3::retract 是同一套数学的两套 API。

7. 数值坑

Log(R) 在 trace 接近 $- 1$ （近 180°）需 Shepperd 或 Eigen::AngleAxisd，naive acos 丢精度。四元数扰动 $q^{'} = q \otimes δ q$ 与右乘 SE(3) 的 Jacobian 差伴随——两种 convention 都能工作，但混在一个因子图里不行。单元测试应覆盖 179° 附近旋转，以及 $ϕ = 0$ 与 $ϕ$ 较大两种扰动，否则生产环境大回环时才暴雷。

8. 学习路径（工程师向）

不必先读完整本 Chirikjian。我的顺序：① 手写 $p^{'} = R p + t$ 对 $(ϕ, t)$ 的导数；② 实现 Exp/Log 并验证往返；③ 读 Forster 预积分 appendix 或 Barfoot Ch.7；④ 对照 g2o VertexSE3::oplus。一下午手推 SE(3) 重投影 Jacobian，比看十篇综述有用。

9. 常见坑汇总

小角度近似用在大回环 → 旋转不正交；右乘/左乘混用 → 发散；Log 在 $2 π$ 附近多值 → 插值跳变；把 $[ϕ]_{\times}$ 与 $[p]_{\times}$ 搞混 → 投影 Jacobian 符号反。读新库先 grep oplus/retract/boxplus，再对接 IMU 因子。

10. 验收

单元测试：Log(Exp(φ)) ≈ φ，含 179° 附近；
解析 Jacobian vs 数值微分 diff < 1e-5；
小角 vs 大角近似误差曲线可量化。

李群不是数学课——它是 SLAM 优化里「在流形上求导」的唯一正确写法。把 Exp/Log/Jr 做成独立小库并单测，比在每个 factor 里 copy-paste 安全得多。数值 Jacobian 对照应在同一 perturbation 约定下进行，否则 diff 通过仍可能在真优化里错。团队 wiki 应固定一份「右乘 SE(3) 重投影 Jacobian」推导，新人对照实现。Barfoot Ch.7 与 Forster appendix 对照读，比单看一篇快。实现 Exp/Log/Jr 时用 double 与 float 各跑一遍往返测试，嵌入式上精度问题更早暴露。李代数是预积分与 BA 共享的语言，越早统一越省集成时间。