李群

基本定义

• 李群（Lie Group）：既是群又是光滑流形，群运算在流形上是可微的。直观上是“可微的变换集合”，如空间旋转/位姿。
• 李代数（Lie Algebra）：李群在“单位元处的切空间”，配备一个“括号运算”[·,·]（对矩阵群就是交换子 AB−BA）。直观上是“无穷小变换”的线性化空间。

为什么有用（优化/滤波视角）

• 位姿等变量活在流形上（不能直接做欧式加法）。用李代数的向量 ξ 做小增量，再通过 exp 映射回李群更新：T ← exp(ξ^) T。
• 优点：最小参数、数值稳定、梯度/Jacobian 推导规范，避免欧拉角奇异和直接对旋转矩阵做不合法加法。

常见对象（SLAM/VIO）

• SO(3)：3D 旋转群；so(3) 是 3 维向量空间（角速度/无穷小旋转）。
• SE(3)：刚体位姿群（旋转 + 平移）；se(3) 是 6 维向量空间（twist）。
• Sim(3)：相似变换（含尺度），用于闭环的尺度矫正。

hat/vee 与 exp/log（关键操作）

• 向量到矩阵的“帽”映射 hat（^），及其逆 vee（∨），把 R^n 的增量编码为李代数矩阵。
• so(3) 的 hat 与罗德里格公式： $$[\omega {]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right],\theta =\Vert \omega \Vert ,\exp ([\omega {]}_{\times })=I+\frac{\sin \theta }{\theta }[\omega {]}_{\times }+\frac{1-\cos \theta }{{\theta }^2}[\omega {]}_{\times }^2$$
• se(3) 的 hat 与指数： $$\xi =\left[\begin{array}{c}\rho \\ \omega \end{array}\right]\in {\mathbb{R}}^6,\hat{\xi }=\left[\begin{array}{cc}[\omega {]}_{\times }& \rho \\ 0& 0\end{array}\right],\exp (\hat{\xi })=\left[\begin{array}{cc}R& V\rho \\ 0& 1\end{array}\right]$$ 其中 $R=\exp ([\omega {]}_{\times })$，$V=I+\frac{1-\cos \theta }{{\theta }^2}[\omega {]}_{\times }+\frac{\theta -\sin \theta }{{\theta }^3}[\omega {]}_{\times }^2$。
• log 是逆映射：把群元素（R 或 T）映到其对应的无穷小向量（ω 或 ξ）。

右/左扰动与更新规则（实现要点）

• 左扰动：$T\leftarrow \exp (\hat{\xi })T$；右扰动：$T\leftarrow T\exp (\hat{\xi })$。二者雅可比/Adjoint 不同，代码需前后一致。
• Adjoint（变换李代数向量到另一坐标系）： $${\mathrm{Ad}}_T=\left[\begin{array}{cc}R& [t{]}_{\times }R\\ 0& R\end{array}\right],{\xi }^{\prime }={\mathrm{Ad}}_T\xi$$ 在残差从不同参考系表达时常用。

与 Sophus 的关系（工程落地）

• Sophus 提供 SO3/SE3/Sim3 的类、exp/log、hat/vee、Adjoint、雅可比等；内部用 Eigen，接口与上述公式一一对应。
• ORB-SLAM3 到处用 Sophus::SE3f/SO3f 表示位姿/旋转，用 exp/log 实现优化更新与 IMU 预积分等。

常见数值细节与坑

• 小角度（θ≈0）：用泰勒展开稳定计算 V、sinθ/θ 等；库里已处理，但自己推导时要注意。

• π 附近（旋转轴翻转）：log/轴角存在符号歧义，确保一致的约定与归一化。

• 右/左扰动混用会导致雅可比错配；明确全链路的扰动约定。

• 四元数可用于存储旋转，但更新宜走 so(3) 的 exp/log，再归一化四元数，避免漂移。

• 一句话记忆

  • 李群是“可微的变换集合”，李代数是“在单位元处的线性化（无穷小变换）”；用 exp/log 把“线性增量”与“非线性变量”桥接起来，是现代 SLAM/视觉几何优化的基础。

Q&A

这和直接用 Eigen 里的列向量、矩阵等来表示位姿和转换矩阵有何区别

先给你一个对照式结论：Eigen 负责“线性代数”，Sophus（李群/李代数）负责“正确的位姿数学”。用纯 Eigen 的矩阵/向量可以存储和做乘法，但一旦涉及“更新/优化/噪声建模”，就需要 Sophus 的 exp/log、hat/vee、Adjoint 等来保证数学正确与数值稳定。

• 表达 vs. 运算

  • 纯 Eigen 矩阵/向量
    • 能表示 R、t、T=[R|t]，能做乘法 T1T2、作用到点 Tp。
    • 容易做出“非法更新”（比如直接 R += dR、或对欧拉角做加法），破坏正交性 det(R)=1，出现漂移/畸变。
  • Sophus（SO3/SE3/Sim3）
    • 在流形上进行更新：T_new = exp(ξ^) T_old（左扰动；右扰动也支持），ξ 是最小参数（so(3)/se(3) 向量）。
    • 提供 log/exp、hat/vee、Adjoint、左右雅可比，优化/滤波时的增量、导数都是标准做法。

• 约束与稳定性

  • 纯 Eigen 需手工维持旋转约束（正交、单位行列式），常见做法是正交化/归一化，易引入累积误差或数值震荡。
  • Sophus 的群运算天然满足约束；小角度、π 附近的数值细节（泰勒展开、轴角退化）库内已处理。

• 参数化与优化

  • 纯 Eigen 的“加法更新”不适用于非线性流形，推雅可比容易错，边界情况（角度环绕、奇异）多。
  • Sophus 提供最小参数（3/6 维）更新，方便 LM/GN/IEKF；Adjoint 让不同坐标系之间的扰动转换 ξ’ = Ad_T ξ 明确可用。

• 可读性与 API

  • 纯 Eigen 代码里很难一眼看出“左/右扰动、log/exp、坐标系约定”，易埋坑。
  • Sophus 类型语义清晰：SE3f/SO3f、T.inverse()、T*point、SE3::exp/log 等表达直接对应几何意义。

• 与四元数的关系

  • 即便用 Eigen::Quaternion 表示旋转，优化更新仍应走 so(3) 的 exp/log，再归一化四元数；Sophus::SO3 已封装好这套流程。

• 什么时候仅用 Eigen 就够

  • 只读/固定标定（已知外参与外参），只做 T1T2 或 Tp，不做任何估计/更新时。
  • 需要高性能的纯代数运算（矩阵块操作）但不触碰“位姿更新”的环节。
  • 一旦涉及位姿估计、融合、优化、IMU 预积分等，建议用 Sophus。

• 常见坑（只用 Eigen 时容易踩）

  • 直接对欧拉角做加法（万向节锁/奇异）；
  • 直接对旋转矩阵加小量导致非正交，越跑越歪；
  • 混用左/右扰动导致雅可比错配、收敛异常；
  • 忘了 det(R)=1（得到不正的“旋转”，带尺度/剪切）。

一句话：Eigen 是底层“数值工具箱”，Sophus 是把“位姿当作李群元素”来计算的“几何工具箱”。存储/乘法用 Eigen 足够，但只要需要“更新/优化/导数”，就用 Sophus 才是稳妥的工程选择。

用李群对象表示位姿和转换的好处

我用要点说明用李群（配合 Sophus 的 SO3/SE3/Sim3）来表示位姿/变换的实际收益，并结合 ORB-SLAM3/Settings.cc 的场景举例。

• 保持几何约束

  • 旋转始终满足 R∈SO(3), det(R)=1；用 exp(·) 更新不会破坏正交性，避免矩阵“越跑越歪”的问题。
  • 避免欧拉角的万向节锁与角度环绕等奇异。

• 最小参数与稳定更新

  • 在切空间用 3/6 维向量做增量（so(3)/se(3)），通过 exp/log 映射回群：T ← exp(ξ^) T。
  • 左/右扰动有严格定义，优化与滤波更规范一致。

• 可导性与优化友好

  • 提供 hat/vee、Adjoint、左右雅可比等解析工具；残差在切空间线性化，GN/LM/IEKF 更稳、更快收敛。
  • 小角度用泰勒展开稳定计算，π 附近的数值细节由库处理，减少自实现踩坑。

• 不确定性与传感器融合

  • 噪声在李代数空间近似高斯，协方差传播更自然；与 IMU 预积分、状态估计匹配良好。
  • 尺度修正可用 Sim3（闭环后全局尺度一致性）。

• 组合、求逆与坐标变换清晰

  • T1T2、T.inverse()、Tp 语义直观；不同坐标系下的扰动用 Ad_T 映射，推导简洁。

• 插值与平均

  • SO3 的球面插值、SE3 的测地线平均更合理，用于轨迹平滑/可视化不会引入非物理变形。

• 代码语义与可维护性

  • 类型即语义（SE3f/SO3f），API 直接对应几何操作，降低“矩阵 + 向量杂糅”的歧义和隐藏 Bug。

• 与 ORB-SLAM3/Settings.cc 的直接对应

  • Settings.cc 中把 OpenCV 矩阵外参转为 Sophus：Tbc_（IMU→相机）、Tlr_（左→右目）通过 Converter::toSophus(...) 得到，后续组合/求逆安全：
    • 例如立体基线：b_ = Tlr_.translation().norm();
    • 立体校正后更新 IMU 外参：Tbc_ = Tbc_ * T_r1_u1.inverse();（群运算严格遵守位姿代数）
  • Tracking/LoopClosing/BA 中的位姿更新、闭环尺度校正（Sim3）均依赖这套流形更新与雅可比。

一句话：李群/李代数把“非线性位姿”放到正确的数学空间里做“线性增量 + 非线性映射”，既保证几何约束，又让优化、融合与数值稳定性都更可靠，比单纯用 Eigen 矩阵/向量更专业、更稳健。

这里多次提及雅克比，是指什么

• 定义（是什么）

  • 雅克比（Jacobian）是向量函数对变量的偏导数组成的矩阵：J[i,j] = ∂f_i/∂x_j，用来刻画“输入微小变化时，输出如何线性近似变化”。

• 作用（为什么需要）

  • 线性化非线性问题：r(x+δ) ≈ r(x) + J δ，用于高斯 - 牛顿/LM 迭代。
  • 不确定性/噪声传播：协方差近似传播 Σ’ ≈ F Σ Fᵀ + G Q Gᵀ。
  • 在李群上做正确的“微小扰动”更新（exp/log 的导数与左右李雅克比）。

• 在 SLAM/VIO 中的三类常见雅克比

  • 观测模型雅克比：J = ∂h(x)/∂x
    • 例：重投影残差 r = u_meas − π(K [R|t] P)，对位姿/路标/内参的偏导，用于 BA、PnP。
  • 李群扰动雅克比：J_l(φ)、J_r(φ)
    • 连接 so(3)/se(3) 的增量与群空间更新，T_new = exp(ξ̂) T_old 时需要。
    • SO(3) 左李雅克比（轴角 φ，θ=||φ||）： $$J_l(\phi )=I-\frac{1-\cos \theta }{{\theta }^2}[\phi {]}_{\times }+\frac{\theta -\sin \theta }{{\theta }^3}[\phi {]}_{\times }^2$$ 小角度可用泰勒展开近似：J_l(φ) ≈ I − 0.5[φ]× + 1/6[φ]ײ。
  • 动力学/预积分雅克比：F、G、以及对偏置的偏导
    • IMU 预积分里对状态与陀螺/加计偏置的雅克比，用于误差状态传播与因子构建。

• 优化中如何用（最小二乘一眼式）

  • 令残差 r(x)=z−h(x)，权重/核函数矩阵 W，线性化后增量解： $$\delta =-(J^{\top }WJ{)}^{-1}{J}^{\top }Wr$$
  • 在李群上更新位姿走“扰动 - 指数映射”：T ← exp(ξ̂) T（或右扰动 T ← T exp(ξ̂)），对应的雅克比与 Adjoint 必须与约定一致。

• 数值实现与工程取舍

  • 解析雅克比：快、稳、可控（ORB-SLAM3/g2o 多用解析形式；Sophus 提供 J_l/J_r 等公式）。
  • 数值差分：易用但慢且易噪。
  • 自动微分：开发快、精度高，代价是编译/运行开销与模板复杂度。

• 易错点

  • 混淆左/右扰动，导致雅克比不匹配、优化发散。
  • 旋转的“直接加法”破坏正交性，应在 so(3)/se(3) 切空间里做增量再用 exp 回到群。
  • 小角度/π 附近数值不稳定，需使用稳健的近似（库已处理）。

• 一句话

  • 雅克比就是“输出对输入的线性敏感度”。它把非线性几何（相机模型、位姿李群）变成一次次可解的线性问题，是 SLAM/VIO 优化与融合的核心工具。