30_李群基础与SO3_SE3

专题3：李群基础与 SO(3)/SE(3) 专论¶

前置要求：专题 1 的光滑流形、专题 2 的 Retraction 与流形优化。 学习目标：读完本专题后，你应能解释为什么旋转和位姿不能当作普通欧氏向量处理，能从矩阵指数推导 SO(3) 的 Rodrigues 公式，能写出 SE(3) 指数映射中的 \(V\) 矩阵，能解释 Adjoint 在坐标变换中的作用，并能识别 Sophus、manif、GTSAM、Ceres 在扰动顺序和切向量排序上的差异。

预计阅读时间¶

阅读方式	时间	适合谁
精读（含练习与代码验证）	10–14 小时	需要手推 Rodrigues 公式和 SE(3) 指数映射的读者
速读（跳过推导细节）	4–5 小时	已有旋转表示基础、只需补全 Adjoint 与 convention 知识的读者
速查（只看表格和代码模板）	30–45 分钟	遇到具体 SO(3)/SE(3) 公式或库 convention 问题时回来查

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回专题 1-2 复习

编号	问题	答不出→回顾
1	什么是光滑流形？为什么旋转矩阵所在的空间不是欧氏空间？	专题 1
2	Retraction 与指数映射的关系是什么？为什么流形优化需要 retraction 而非直接加减？	专题 2
3	切空间的直觉含义是什么？它与流形上的"方向导数"有什么关系？	专题 1
4	正交矩阵 \(R^T R = I\) 施加了几个独立约束？\(3 \times 3\) 旋转矩阵有几个自由度？	线性代数基础
5	给定两个旋转 \(R_1, R_2\)，如何定义它们之间的"距离"？直接做 \(R_1 - R_2\) 有什么问题？	专题 1-2

0. 为什么机器人必须学习李群 ⭐¶

机器人最常处理的状态是姿态和位姿：

姿态：机器人朝向哪里
位置：机器人在哪里
位姿：机器人在哪里，并且朝向哪里

位置可以用 \(\mathbb{R}^3\) 向量表示。两个位置相减得到位移，两个位移相加仍然是位移，这与普通线性代数完全一致。但姿态不同。一个旋转矩阵 \(R\) 必须满足：

\[ R^\top R = I,\qquad \det(R)=1 \]

这两个约束意味着旋转矩阵不是任意 \(3\times3\) 矩阵，而是嵌在 \(\mathbb{R}^{9}\) 中的一片弯曲空间。如果把旋转当成普通矩阵做加法，马上会出问题。

例如两个合法旋转矩阵 \(R_1,R_2\in SO(3)\)，它们的平均：

\[ \bar R = \frac{1}{2}(R_1+R_2) \]

通常不再满足 \(\bar R^\top\bar R=I\)。这说明“旋转的差”和“旋转的平均”不能直接照搬欧氏向量公式。

反面案例在机器人中非常常见：

错误做法	表面结果	深层问题
直接对旋转矩阵做加法	数值上得到一个矩阵	不再是旋转
直接对欧拉角做 EKF	公式简单	接近奇异点时线性化崩坏
把位姿误差写成 \(T_1-T_2\)	实现容易	不满足群结构
在优化中让四元数自由更新	梯度下降方便	单位长度约束被破坏

李群提供的解决思路是：

在弯曲的群空间上保存真实状态，在单位元附近的线性切空间里表示小扰动。

这个思路与地图学中的投影类似——地球表面是弯曲的球面，但导航地图把局部区域投影到平面上。只要投影范围足够小（一座城市），平面近似就很准确，但若把整个大陆投影成矩形，形变就不可忽略（格陵兰在墨卡托投影中被放大了 14 倍）。李群与李代数之间的指数/对数映射正是这种"局部平面近似"的严格数学版：切空间是局部地图，群空间是真实地球，指数映射是从地图到地球的投影函数，对数映射则是从地球回到地图的反投影。两者的关键区别在于，李群上的"投影"不仅保持距离近似，还保持代数运算的一阶结构。

这句话连接了机器人学中大量内容：

SLAM 位姿图优化中的扰动 \(\delta\xi\in\mathbb{R}^6\)；
VIO 中的 IMU 预积分；
机械臂末端位姿误差；
移动机器人 SE(2) 位姿复合；
状态估计中的 ESKF、MEKF、InEKF；
Ceres/GTSAM/Sophus/manif 中的 Plus、Minus、retract、localCoordinates。

1. 从流形到李群：多了一个群运算 ⭐¶

回顾专题 1：流形的核心思想是“局部像欧氏空间”。在一个光滑流形上，每个点附近都可以用坐标图近似成 \(\mathbb{R}^n\)，因此可以定义切空间、微分和局部优化。

李群在流形上再加一层代数结构：

\[ G\ \text{是光滑流形},\qquad m:G\times G\to G,\quad (g,h)\mapsto gh \]

以及求逆映射：

\[ i:G\to G,\quad g\mapsto g^{-1} \]

如果乘法 \(m\) 和求逆 \(i\) 都是光滑映射，那么 \(G\) 就是李群。

这个定义为什么重要？因为机器人运动天然需要“复合”和“求逆”：

操作	群语言	机器人含义
\(T_{ab}T_{bc}=T_{ac}\)	群乘法	坐标变换复合
\(T_{ab}^{-1}=T_{ba}\)	群逆	反向坐标变换
\(T\exp(\delta\xi^\wedge)\)	右扰动	在当前位姿附近更新
\(\log(T_1^{-1}T_2)\)	局部坐标	两个位姿之间的误差

没有群结构，流形只能告诉我们“局部怎么像欧氏空间”；有了群结构，我们还能自然描述坐标变换的复合、逆变换和误差。

1.1 矩阵李群 ⭐¶

机器人中最常见的是矩阵李群。它们是矩阵乘法下的群，同时也是光滑流形。

李群	元素	机器人含义
\(SO(2)\)	2D 旋转矩阵	平面朝向
\(SE(2)\)	平面位姿	差速/阿克曼底盘
\(SO(3)\)	3D 旋转矩阵	刚体姿态
\(SE(3)\)	3D 刚体位姿	机械臂末端、相机、IMU
\(Sim(3)\)	相似变换	单目 SLAM 尺度漂移
\(SE_2(3)\)	姿态、速度、位置	InEKF/IMU 导航

本专题聚焦 \(SO(3)\) 和 \(SE(3)\)，因为它们是机器人三维运动的主干。

2. 李代数：单位元处的线性化空间 ⭐¶

李群是弯曲空间，但单位元附近可以线性化。李代数定义为单位元处的切空间：

\[ \mathfrak g = T_eG \]

这个定义很短，但意义很深。它说明所有局部扰动都可以先放在单位元处的线性空间中，再通过群运算搬到任意状态附近。

对矩阵李群，李代数可以看成满足特定结构的矩阵集合。例如 \(SO(3)\) 的李代数是：

\[ \mathfrak{so}(3)=\{\Omega\in\mathbb{R}^{3\times3}\mid \Omega^\top=-\Omega\} \]

也就是所有 \(3\times3\) 反对称矩阵。任意向量 \(\omega=[\omega_x,\omega_y,\omega_z]^\top\) 可以通过 hat 运算变成反对称矩阵：

\[ \omega^\wedge = \begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x\\ -\omega_y & \omega_x & 0 \end{bmatrix} \]

它满足：

\[ \omega^\wedge v = \omega\times v \]

这说明 \(\mathfrak{so}(3)\) 与 \(\mathbb{R}^3\) 可以一一对应。vee 运算则把反对称矩阵取回向量：

\[ (\omega^\wedge)^\vee=\omega \]

2.1 为什么李代数可以做加法 ⭐¶

李代数是切空间，切空间是线性空间。因此小扰动可以相加：

\[ \delta\omega = \delta\omega_1 + \delta\omega_2 \]

但这不意味着大旋转可以直接相加。正确关系是：

小扰动在线性空间里相加
真实状态在李群上复合

这正是流形优化的基本结构。优化变量保存在 \(SO(3)\) 或 \(SE(3)\) 上；优化增量保存在 \(\mathbb{R}^3\) 或 \(\mathbb{R}^6\) 中。

💡 概念误区：认为"李代数元素可以相加"意味着"旋转可以相加"

新手想法："既然 \(\delta\omega_1 + \delta\omega_2\) 合法，那两个旋转的和也应该合法吧？"

实际上：李代数上的加法只在小扰动时有效。两个大旋转的复合必须通过群乘法 \(R_1 R_2\)，而不是李代数加法 \(\omega_1 + \omega_2\)。BCH 公式表明，即使 \(\omega_1\) 和 \(\omega_2\) 都是中等大小，\(\log(\exp(\omega_1^\wedge)\exp(\omega_2^\wedge))\) 也不等于 \(\omega_1 + \omega_2\)，而是需要加上交换子修正项 \(\frac{1}{2}[\omega_1^\wedge, \omega_2^\wedge]\)。

正确做法：小扰动在切空间相加，大状态在群上乘法复合。这是李群优化的基本纪律。

3. 指数映射：从小扰动回到群 ⭐⭐¶

指数映射把李代数元素映射回李群：

\[ \exp:\mathfrak g\to G \]

对矩阵李群，指数映射就是矩阵指数：

\[ \exp(A)=I+A+\frac{1}{2!}A^2+\frac{1}{3!}A^3+\cdots \]

为什么这个级数能产生旋转？以 \(SO(3)\) 为例，取 \(\Omega=\omega^\wedge\)，由于 \(\Omega^\top=-\Omega\)，可以证明：

\[ \exp(\Omega)^\top \exp(\Omega) =\exp(\Omega^\top)\exp(\Omega) =\exp(-\Omega)\exp(\Omega)=I \]

而且 \(\det(\exp(\Omega))=\exp(\operatorname{tr}(\Omega))=1\)，因为反对称矩阵迹为 0。因此 \(\exp(\omega^\wedge)\in SO(3)\)。

3.1 Rodrigues 公式的推导 ⭐⭐¶

令 \(\omega=\theta n\)，其中 \(\|n\|=1\)，\(\theta=\|\omega\|\)。设 \(N=n^\wedge\)，则：

\[ \omega^\wedge = \theta N \]

单位轴的反对称矩阵有两个关键性质：

\[ N^2 = nn^\top-I,\qquad N^3=-N \]

矩阵指数展开为：

\[ \exp(\theta N) = I+\theta N+\frac{\theta^2}{2!}N^2+\frac{\theta^3}{3!}N^3+\frac{\theta^4}{4!}N^4+\cdots \]

利用 \(N^3=-N\)、\(N^4=-N^2\)，把奇数项和偶数项分组：

\[ \theta N+\frac{\theta^3}{3!}N^3+\frac{\theta^5}{5!}N^5+\cdots =\left(\theta-\frac{\theta^3}{3!}+\frac{\theta^5}{5!}-\cdots\right)N =\sin\theta\,N \]

偶数项为：

\[ \frac{\theta^2}{2!}N^2+\frac{\theta^4}{4!}N^4+\frac{\theta^6}{6!}N^6+\cdots =\left(\frac{\theta^2}{2!}-\frac{\theta^4}{4!}+\frac{\theta^6}{6!}-\cdots\right)N^2 =(1-\cos\theta)N^2 \]

于是得到 Rodrigues 公式：

\[ \exp(\theta n^\wedge) = I+\sin\theta\,n^\wedge+(1-\cos\theta)(n^\wedge)^2 \]

这个推导非常重要，因为它说明三维旋转的闭式公式不是凭空记忆出来的，而是矩阵指数级数在反对称矩阵特殊代数结构下的化简。

⚠️ 概念误区：认为 Rodrigues 公式只是"一种旋转表示的计算公式"

新手想法："Rodrigues 公式就是把轴角转成旋转矩阵的工具，记住就行。"

实际上：Rodrigues 公式是矩阵指数在 \(SO(3)\) 上的闭式求和结果。它之所以存在闭式，是因为反对称矩阵的幂具有循环性 \(N^3 = -N\)。这个代数性质不是所有李群都有——例如 \(SE(3)\) 的指数映射就不能写成如此简洁的三项和，需要额外的 \(V\) 矩阵。理解推导过程比记住公式更重要，因为同样的分组技巧（奇数项/偶数项分别求和）在 \(J_l\)、\(J_r\) 的推导中会反复出现。

3.2 小角度数值稳定 ⭐⭐¶

当 \(\theta\) 很小时，\(\sin\theta/\theta\) 和 \((1-\cos\theta)/\theta^2\) 会遇到数值消减。工程实现通常使用泰勒展开：

\[ \frac{\sin\theta}{\theta} =1-\frac{\theta^2}{6}+\frac{\theta^4}{120}+O(\theta^6) \]

\[ \frac{1-\cos\theta}{\theta^2} =\frac{1}{2}-\frac{\theta^2}{24}+\frac{\theta^4}{720}+O(\theta^6) \]

因此库函数不能简单照公式写，还要处理小角度分支。

#include <Eigen/Dense>
#include <cmath>

Eigen::Matrix3d hat(const Eigen::Vector3d& w) {
  Eigen::Matrix3d W;
  W << 0.0, -w.z(), w.y(),
       w.z(), 0.0, -w.x(),
      -w.y(), w.x(), 0.0;
  return W;
}

Eigen::Matrix3d so3_exp(const Eigen::Vector3d& w) {
  const double theta = w.norm();
  const Eigen::Matrix3d W = hat(w);

  double A;
  double B;
  if (theta < 1e-8) {
    // 小角度下使用泰勒展开，避免 1-cos(theta) 的消减误差。
    const double theta2 = theta * theta;
    A = 1.0 - theta2 / 6.0;
    B = 0.5 - theta2 / 24.0;
  } else {
    A = std::sin(theta) / theta;
    B = (1.0 - std::cos(theta)) / (theta * theta);
  }

  return Eigen::Matrix3d::Identity() + A * W + B * W * W;
}

4. 对数映射：从旋转回到局部坐标 ⭐⭐¶

对数映射是指数映射的局部逆：

\[ \log: SO(3)\to\mathfrak{so}(3) \]

如果 \(R=\exp(\theta n^\wedge)\)，则：

\[ \operatorname{tr}(R)=1+2\cos\theta \]

因此：

\[ \theta = \cos^{-1}\left(\frac{\operatorname{tr}(R)-1}{2}\right) \]

再由 Rodrigues 公式可得：

\[ R-R^\top = 2\sin\theta\,n^\wedge \]

所以：

\[ \log(R) =\frac{\theta}{2\sin\theta}(R-R^\top) \]

这就是 SO(3) 对数映射的常见闭式。

这个闭式默认工作在主值分支 \(0<\theta<\pi\)，并且 \(\theta\) 要远离 0 和 \(\pi\)。真实实现中需要三类保护：对 \(\cos^{-1}\) 的输入做 \([-1,1]\) 裁剪，小角度用泰勒展开，接近 \(\pi\) 时用专门的旋转轴提取方法。

4.1 \(\pi\) 附近为什么麻烦 ⭐⭐⭐¶

当 \(\theta\to\pi\) 时，\(\sin\theta\to0\)，上式分母接近 0。更深层的原因是拓扑：\(SO(3)\) 不是全局可用一个无奇异三维坐标覆盖的空间。单位四元数 \(q\) 和 \(-q\) 表示同一个旋转，轴角在 \(\theta=\pi\) 附近也有符号歧义：

\[ \pi n \quad\text{和}\quad \pi(-n) \]

表示同一个旋转。

工程上必须处理：

对数映射的角度分支；
四元数符号连续性；
优化残差接近 \(\pi\) 时的收敛风险；
轨迹插值时不能突然从 \(q\) 跳到 \(-q\)。

5. SE(3)：旋转和平移的半直积 ⭐⭐¶

三维刚体位姿写成齐次矩阵：

\[ T= \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix}, \qquad R\in SO(3),\quad p\in\mathbb{R}^3 \]

群乘法为：

\[ T_1T_2= \begin{bmatrix} R_1R_2 & R_1p_2+p_1\\ 0 & 1 \end{bmatrix} \]

注意平移部分不是 \(p_1+p_2\)，而是 \(R_1p_2+p_1\)。这说明 \(SE(3)\) 不是 \(SO(3)\) 和 \(\mathbb{R}^3\) 的简单直积，而是半直积：

\[ SE(3)=\mathbb{R}^3\rtimes SO(3) \]

如果把位姿误差错误地写成旋转误差和平移误差独立相加，就会漏掉旋转对平移坐标的作用。

\(SE(3)\) 的半直积结构可以类比汽车导航中的"先转向再前进"。假设你站在十字路口朝北，收到指令"右转 90 度，再前进 100 米"。如果你把这两步拆开独立处理——"我转了 90 度"加上"我走了 100 米向北"——最终位置就错了，因为前进的方向取决于转向后的朝向。\(SE(3)\) 乘法中 \(R_1 p_2 + p_1\) 的旋转项 \(R_1 p_2\) 正是这个"转向改变了前进方向"的数学表达。纯平移群 \(\mathbb{R}^3\) 没有这个耦合，因为平移不改变方向；半直积正是描述"旋转对平移施加作用"的代数结构。

5.1 se(3) 与 twist ⭐⭐¶

\(SE(3)\) 的李代数元素写成：

\[ \xi^\wedge= \begin{bmatrix} \omega^\wedge & \rho\\ 0 & 0 \end{bmatrix} \]

其中 \(\xi=[\rho,\omega]^\top\in\mathbb{R}^6\)。这里采用”平移在前、旋转在后”的顺序，Sophus 和 manif 常用这种排列。GTSAM 许多接口采用”旋转在前、平移在后”的顺序，使用时必须明确。

⚠️ 编程陷阱：切向量排序不一致导致 SE(3) 公式全部错误

错误做法：从 Solà 论文抄了 \([\rho, \omega]\) 排序的 Adjoint 公式，直接用在 GTSAM 的 \([\omega, \rho]\) 向量上。

现象：代码编译通过，SE(3) 的 Adjoint 结果看起来”差不多对”，但协方差传播方向偏了，位姿图优化收敛变慢甚至发散。

根本原因：排序不同导致 6x6 矩阵的分块位置互换——右上变左下、左下变右上。代码不会报错，但物理含义全部混乱。

正确做法：在项目代码中显式声明排序约定（如 // Convention: [rho, phi] = [translation, rotation]），并用 Adjoint 的基本性质（\(T\exp(\xi^\wedge)T^{-1} = \exp((\text{Ad}_T \xi)^\wedge)\)）做数值验证。

5.2 SE(3) 指数映射 ⭐⭐¶

对 \(\xi=[\rho,\omega]\)，有：

\[ \exp(\xi^\wedge)= \begin{bmatrix} \exp(\omega^\wedge) & V\rho\\ 0 & 1 \end{bmatrix} \]

其中：

\[ V = I +\frac{1-\cos\theta}{\theta^2}\omega^\wedge +\frac{\theta-\sin\theta}{\theta^3}(\omega^\wedge)^2, \qquad \theta=\|\omega\| \]

很多人第一次看到 \(V\rho\) 会问：为什么平移不是直接 \(\rho\)？原因是 twist 表示的是刚体瞬时运动。旋转和平移在积分过程中相互耦合，\(V\) 正是把局部 twist 的平移分量积分到最终位移的矩阵。

本质洞察：\(V\) 矩阵不是一个"修正因子"，而是旋转对平移的积分耦合效应的精确表达。当旋转为零时 \(V = I\)，平移分量直接就是位移，没有耦合；当旋转不为零时，物体在旋转的同时平移，走出的是一条螺旋线而非直线，\(V\) 正好把微分层面的瞬时 twist 积分成有限运动的最终位移。这也是为什么 \(V(\omega) = J_l^{SO(3)}(\omega)\)——SE(3) 指数映射中的平移积分和 SO(3) 左 Jacobian 是同一个数学对象，两者都在回答"沿着弯曲路径积分后最终到哪"的问题。

当 \(\omega=0\) 时，\(V=I\)，于是：

\[ \exp(\xi^\wedge)= \begin{bmatrix} I & \rho\\ 0 & 1 \end{bmatrix} \]

这与纯平移直觉一致。

5.3 SE(3) 对数映射 ⭐⭐¶

若：

\[ T=\begin{bmatrix}R&p\\0&1\end{bmatrix} \]

先求：

\[ \omega = \log(R)^\vee \]

再由：

\[ p=V\rho \]

得到：

\[ \rho=V^{-1}p \]

因此：

\[ \log(T)^\vee = [\rho,\omega]^\top \]

这也解释了为什么不能把 SE(3) 的 log 写成 \([\ p,\log(R)^\vee\ ]\)。只有在小角度或纯平移时，\(V^{-1}p\approx p\)。

6. Adjoint：扰动在哪个坐标系表达 ⭐⭐¶

同一个刚体速度可以在不同坐标系表达。Adjoint 解决的就是”切向量如何随坐标系改变”。

Adjoint 变换可以类比货币汇率。同一件商品的价值不变，但用人民币和美元表示的数字不同。汇率把一种表示转换成另一种表示，而不改变价值本身。Adjoint 把同一个物理运动从一个坐标系的切空间表示转换到另一个坐标系的切空间表示，而不改变物理运动本身。区别在于：汇率是标量乘法（一维），Adjoint 是矩阵乘法（六维），并且 SE(3) 的 Adjoint 中旋转和平移存在耦合（\([t]_\times R\) 项），这意味着坐标原点的平移也会影响角速度到线速度的分量映射。

对：

\[ T=\begin{bmatrix}R&p\\0&1\end{bmatrix} \]

采用 \(\xi=[\rho,\omega]\) 顺序时，Adjoint 为：

\[ \operatorname{Ad}_T = \begin{bmatrix} R & [p]_\times R\\ 0 & R \end{bmatrix} \]

它满足：

\[ \log\!\left(T\exp(\xi^\wedge)T^{-1}\right)^\vee = \operatorname{Ad}_T\xi \]

更严格地说：

\[ T\exp(\xi^\wedge)T^{-1} =\exp((\operatorname{Ad}_T\xi)^\wedge) \]

6.1 Adjoint 的工程意义 ⭐⭐¶

场景	为什么需要 Adjoint
IMU 预积分	在不同 keyframe 坐标中传播扰动
位姿图优化	between factor 的误差扰动要变换坐标
机械臂雅可比	body Jacobian 与 spatial Jacobian 互转
协方差传播	切空间协方差随坐标变换

协方差变换是最常见用法。若扰动变换为：

\[ \xi_b = \operatorname{Ad}_T \xi_a \]

则协方差变换为：

\[ \Sigma_b =\operatorname{Ad}_T \Sigma_a \operatorname{Ad}_T^\top \]

注意这个公式只在切向量顺序和扰动约定一致时成立。若库中采用 \([\omega,v]\) 顺序，需要使用对应的 Adjoint 排列。

⚠️ 思维陷阱：认为"SO(3) 的 Adjoint 就是旋转矩阵 \(R\)，所以 SE(3) 的 Adjoint 也应该很简单"

新手想法："SO(3) 中 \(\text{Ad}_R = R\)，一个 3x3 矩阵就行了，SE(3) 大概也差不多？"

实际上：SO(3) 的简化 \(\text{Ad}_R = R\) 是一个巧合——旋转群的 Adjoint 表示恰好等于群元素本身。SE(3) 不享有这个性质：它的 Adjoint 是 6x6 矩阵，含有 \([t]_\times R\) 耦合项，这意味着平移位置也会影响角速度到线速度的坐标变换。忽略这个耦合是 IMU 预积分和位姿图优化中常见的 bug 来源。

7. ad 与 BCH：为什么扰动不能简单相加 ⭐⭐⭐¶

李代数上的 bracket 定义为：

\[ [X,Y]=XY-YX \]

对向量形式，写成：

\[ \operatorname{ad}_\xi\eta = [\xi^\wedge,\eta^\wedge]^\vee \]

BCH 公式描述两个指数相乘后对应的李代数元素：

\[ \log(\exp(X)\exp(Y)) =X+Y+\frac{1}{2}[X,Y]+\frac{1}{12}[X,[X,Y]] -\frac{1}{12}[Y,[X,Y]]+\cdots \]

如果 \(X\) 和 \(Y\) 可交换，即 \([X,Y]=0\)，则退化为：

\[ \log(\exp(X)\exp(Y))=X+Y \]

但旋转通常不交换。因此两个旋转小量的复合不是简单相加，而是带有交换子修正。对小扰动，常保留一阶：

\[ \log(\exp(X)\exp(Y))\approx X+Y \]

这就是许多滤波和优化推导的一阶近似来源。二阶项 \(\frac12[X,Y]\) 则解释了为什么高动态旋转下简单误差相加会产生偏差。

8. 左扰动、右扰动与 Plus/Minus ⭐⭐¶

在流形优化中，常见两种更新：

\[ T_{\text{new}}=\exp(\delta\xi^\wedge)T \]

称为左扰动；以及：

\[ T_{\text{new}}=T\exp(\delta\xi^\wedge) \]

称为右扰动。

二者都合法，但含义不同：

扰动	小量表达坐标	常见场景
左扰动	世界/空间坐标系	spatial velocity、某些滤波推导
右扰动	机体/局部坐标系	Sophus 风格优化、许多位姿图

一个库的 Plus 或 retract 必须明确使用哪种扰动。混用时，Jacobian 的符号和 Adjoint 项都会变。

8.1 库约定对比 ⭐⭐¶

库	切向量顺序	局部更新接口	注意点
Sophus	常见 \([\rho,\omega]\)	`T * Sophus::SE3::exp(dx)`	右扰动常见
manif	常见 \([\rho,\omega]\)	`plus()` / `minus()`	Jacobian 面向切空间扰动
GTSAM	常见 \([\omega,\rho]\)	`retract` / `localCoordinates`	与 Sophus 顺序不同
Ceres	由 Manifold 定义	`Plus` / `Minus`	需自行保证归一化与顺序
Pinocchio	Motion/Force 空间向量	`SE3`、`Motion`	速度/力排列与动力学约定相关

工程上最稳妥的做法是：在项目边界层写转换函数，并用单元测试验证 Exp(Log(T))≈T、Log(T^{-1}T)=0、Adjoint 协方差传播等基本性质。

// 示例：在边界层显式转换切向量顺序，避免把库约定散落在业务代码中。
Eigen::Matrix<double, 6, 1> sophus_to_gtsam_order(
    const Eigen::Matrix<double, 6, 1>& dx_sophus) {
  Eigen::Matrix<double, 6, 1> dx_gtsam;
  dx_gtsam.head<3>() = dx_sophus.tail<3>();  // 旋转部分
  dx_gtsam.tail<3>() = dx_sophus.head<3>();  // 平移部分
  return dx_gtsam;
}

9. SO(3) 与 SE(3) 的度量差异 ⭐⭐⭐¶

\(SO(3)\) 是紧致李群，存在自然的 bi-invariant metric。在这个度量下，左乘和右乘都不改变距离，one-parameter subgroup 是测地线。直觉上，旋转误差的大小可以用角度 \(\|\log(R_1^\top R_2)\|\) 表示。

\(SE(3)\) 更麻烦。它同时包含米和弧度，平移和旋转没有天然统一尺度。一个 1 cm 的平移误差和 1 度的旋转误差谁更大？答案依赖机器人尺寸、传感器安装位置和任务目标。

因此 SE(3) 误差通常需要权重：

\[ \|e\|_W^2 = e^\top W e \]

其中 \(W\) 决定平移和旋转的相对权重。工程上常见做法包括：

用测量协方差白化残差；
按传感器噪声设置权重；
按任务长度尺度把角度换算成末端位移；
在优化中分别调平移核和旋转核。

这也是为什么 SE(3) 上不存在一个对所有机器人都“自然正确”的误差权重。

10. 代码验证：最小 SO(3)/SE(3) 单元测试 ⭐⭐¶

学习李群时，公式很多，最容易混淆的是符号和约定。建议写最小测试验证。

#include <Eigen/Dense>
#include <algorithm>
#include <cassert>
#include <cmath>

void check_rotation_matrix(const Eigen::Matrix3d& R) {
  const Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
  // 正交性检查：R^T R 应接近单位矩阵。
  assert((R.transpose() * R - I).norm() < 1e-9);
  // SO(3) 要求 det(R)=1，而不是 -1。
  assert(std::abs(R.determinant() - 1.0) < 1e-9);
}

Eigen::Vector3d so3_log(const Eigen::Matrix3d& R) {
  const double cos_theta =
      std::clamp(0.5 * (R.trace() - 1.0), -1.0, 1.0);
  const double theta = std::acos(cos_theta);
  Eigen::Vector3d w;
  w << R(2, 1) - R(1, 2),
       R(0, 2) - R(2, 0),
       R(1, 0) - R(0, 1);

  if (theta < 1e-10) {
    // 小角度下 sin(theta)≈theta，直接使用一阶近似更稳定。
    return 0.5 * w;
  }

  constexpr double kPi = 3.14159265358979323846;
  // 这个最小实现覆盖常规测试区间；接近 pi 时应使用专门分支。
  assert(theta < kPi - 1e-6);
  return theta / (2.0 * std::sin(theta)) * w;
}

void check_exp_log_roundtrip(const Eigen::Vector3d& w) {
  const Eigen::Matrix3d R = so3_exp(w);
  check_rotation_matrix(R);
  const Eigen::Vector3d w_roundtrip = so3_log(R);
  assert((w_roundtrip - w).norm() < 1e-8);
}

对 SE(3)，至少测试：

Exp(0)=I
Log(I)=0
T * T^{-1}=I
Exp(Log(T))≈T
Ad_T * xi 与 T Exp(xi) T^{-1} 的一阶关系一致

这些测试不是形式主义。很多工程 bug 都来自切向量顺序、左/右扰动、四元数符号和 Adjoint 排列不一致。

11. 左 Jacobian 与右 Jacobian：从”扰动相加”到”扰动搬运” ⭐⭐⭐¶

前面已经看到，\(\exp(\phi^\wedge)\exp(\delta^\wedge)\) 不严格等于 \(\exp((\phi+\delta)^\wedge)\)。当优化器或滤波器在李代数中加小量时，必须知道这个小量是如何被搬到当前旋转附近的。左 Jacobian 和右 Jacobian 正是为这个问题服务。

对 \(SO(3)\)，定义左扰动近似：

\[ \exp((\phi+\delta)^\wedge) \approx \exp((J_l(\phi)\delta)^\wedge)\exp(\phi^\wedge) \]

也可以写成：

\[ \exp(\phi^\wedge)\exp(\delta^\wedge) \approx \exp((\phi+J_r(\phi)^{-1}\delta)^\wedge) \]

如果小量从左边乘入：

\[ \exp(\delta^\wedge)\exp(\phi^\wedge) \approx \exp((\phi+J_l(\phi)^{-1}\delta)^\wedge) \]

这些公式的直觉是：\(\delta\) 在单位元处是一个线性小量，但 \(\phi\) 已经把我们带到了群上的另一个位置。小量从单位元搬到当前点附近时，会被群的曲率和非交换性扭一下。

SO(3) 的左 Jacobian 闭式为：

\[ J_l(\phi) =I+\frac{1-\cos\theta}{\theta^2}\phi^\wedge +\frac{\theta-\sin\theta}{\theta^3}(\phi^\wedge)^2,\qquad \theta=\|\phi\| \]

这个公式和 SE(3) 指数映射里的 \(V\) 矩阵一模一样。也就是说：

\[ V(\omega)=J_l(\omega) \]

这不是巧合。SE(3) 中 \(V\rho\) 的本质，就是旋转群上的左 Jacobian 把平移 twist 积分到最终位移。

左 Jacobian 的逆为：

\[ J_l(\phi)^{-1} =I-\frac{1}{2}\phi^\wedge +\left( \frac{1}{\theta^2} -\frac{1+\cos\theta}{2\theta\sin\theta} \right)(\phi^\wedge)^2 \]

右 Jacobian 与左 Jacobian 的关系是：

\[ J_r(\phi)=J_l(-\phi) \]

因此：

\[ J_r(\phi) =I-\frac{1-\cos\theta}{\theta^2}\phi^\wedge +\frac{\theta-\sin\theta}{\theta^3}(\phi^\wedge)^2 \]

11.1 为什么 Jacobian 不能忽略 ⭐⭐⭐¶

假设你在 IMU 预积分中传播旋转噪声。连续时间角速度模型为：

\[ \dot R = R(\omega_m-b_g-n_g)^\wedge \]

离散更新常写为：

\[ R_{k+1}=R_k\exp\!\left(((\omega_m-b_g)\Delta t)^\wedge\right) \]

噪声 \(n_g\) 不是直接加到最终旋转矩阵上，而是加在指数映射内部。要把噪声协方差传播到切空间，就需要右 Jacobian 或左 Jacobian。若忽略它，在低角速度时误差不明显；在高速旋转、长时间积分或大噪声下，协方差会被系统性低估或高估。

场景	忽略 Jacobian 的后果
小角度低速运动	误差可能暂时不明显
高速旋转 IMU	协方差传播偏差变大
VIO 预积分	bias Jacobian 与噪声传播不一致
位姿图优化	线性化矩阵和残差定义不匹配
Ceres Manifold	`Plus` 与解析 Jacobian 不一致

11.2 小角度展开 ⭐⭐¶

小角度时：

\[ J_l(\phi) \approx I+\frac{1}{2}\phi^\wedge+\frac{1}{6}(\phi^\wedge)^2 \]

\[ J_l(\phi)^{-1} \approx I-\frac{1}{2}\phi^\wedge+\frac{1}{12}(\phi^\wedge)^2 \]

这些近似直接来自 \(\sin\theta\) 和 \(\cos\theta\) 的泰勒展开。工程实现中，小角度分支不仅用于避免除零，也能提升数值精度。

Eigen::Matrix3d so3_left_jacobian(const Eigen::Vector3d& phi) {
  const double theta = phi.norm();
  const Eigen::Matrix3d A = hat(phi);
  const Eigen::Matrix3d A2 = A * A;

  if (theta < 1e-8) {
    // 小角度下使用二阶展开，保证连续且避免除以很小的数。
    return Eigen::Matrix3d::Identity() + 0.5 * A + (1.0 / 6.0) * A2;
  }

  const double theta2 = theta * theta;
  const double theta3 = theta2 * theta;
  return Eigen::Matrix3d::Identity()
       + (1.0 - std::cos(theta)) / theta2 * A
       + (theta - std::sin(theta)) / theta3 * A2;
}

12. 位姿图残差：李群公式如何进入 SLAM ⭐⭐¶

SLAM 位姿图中的一条边通常给出相对位姿测量 \(Z_{ij}\)，连接两个待估计位姿 \(T_i\) 和 \(T_j\)。如果估计完全正确，应有：

\[ Z_{ij}\approx T_i^{-1}T_j \]

因此自然的群误差是：

\[ E_{ij}=Z_{ij}^{-1}T_i^{-1}T_j \]

把它拉回切空间得到残差：

\[ e_{ij}=\log(E_{ij})^\vee \]

这个残差比 \(T_j-T_i-Z_{ij}\) 更合理，因为它始终尊重 \(SE(3)\) 的群结构。

12.1 为什么要先乘再取 log ⭐⭐¶

误差 \(Z_{ij}^{-1}T_i^{-1}T_j\) 的含义是“预测相对位姿与测量相对位姿的差”。这里的差不是减法，而是群上的相对变换：

测量：Z_ij
预测：T_i^{-1} T_j
误差：Z_ij^{-1} (T_i^{-1} T_j)

如果误差为单位元 \(I\)，说明预测和测量一致。对单位元附近取 \(\log\)，得到一个 6 维向量，优化器才能做最小二乘。

12.2 一阶线性化的结构 ⭐⭐⭐¶

设右扰动：

\[ T_i' = T_i\exp(\delta_i^\wedge),\qquad T_j' = T_j\exp(\delta_j^\wedge) \]

残差变为：

\[ e_{ij}'= \log\left( Z_{ij}^{-1} (T_i\exp(\delta_i^\wedge))^{-1} T_j\exp(\delta_j^\wedge) \right)^\vee \]

利用：

\[ (T_i\exp(\delta_i^\wedge))^{-1} =\exp(-\delta_i^\wedge)T_i^{-1} \]

得到：

\[ e_{ij}'= \log\left( Z_{ij}^{-1} \exp(-\delta_i^\wedge) T_i^{-1}T_j \exp(\delta_j^\wedge) \right)^\vee \]

严格推导会引入 Adjoint 和右 Jacobian。教学上先记住结构：

\[ e_{ij}'\approx e_{ij}+J_i\delta_i+J_j\delta_j \]

其中 \(J_i\)、\(J_j\) 的具体形式取决于：

残差定义是 \(Z^{-1}T_i^{-1}T_j\) 还是 \((T_i^{-1}T_j)Z^{-1}\)；
使用左扰动还是右扰动；
切向量顺序是 \([\rho,\omega]\) 还是 \([\omega,\rho]\)；
Log 的 Jacobian 使用左 Jacobian 还是右 Jacobian。

这也是为什么机器人优化库的 Jacobian 不能随便从网上抄。公式看起来相似，但约定不同会导致符号、Adjoint 方向和列顺序变化。

12.3 用数值差分验证解析 Jacobian ⭐⭐¶

解析 Jacobian 写完后，应使用数值差分验证。

template <typename ResidualFunctor>
Eigen::MatrixXd numerical_jacobian(
    ResidualFunctor residual,
    const Eigen::Matrix<double, 6, 1>& x,
    double eps = 1e-6) {
  const Eigen::VectorXd r0 = residual(x);
  Eigen::MatrixXd J(r0.size(), x.size());

  for (int k = 0; k < x.size(); ++k) {
    Eigen::Matrix<double, 6, 1> xp = x;
    Eigen::Matrix<double, 6, 1> xm = x;
    xp(k) += eps;
    xm(k) -= eps;

    // 中心差分比前向差分更稳定，适合检查推导中的符号错误。
    J.col(k) = (residual(xp) - residual(xm)) / (2.0 * eps);
  }
  return J;
}

数值差分不适合生产求解器的热路径，但非常适合验证推导。尤其在 SE(3) 里，很多错误不是数量级错误，而是某一列符号反了。

13. 与 ESKF/MEKF/InEKF 的连接 ⭐⭐⭐¶

后续概率与估计/30_流形滤波族会讨论流形滤波。这里先建立最基本的桥：

真实状态在群上
估计状态在群上
误差状态在李代数中
滤波器协方差定义在误差状态上

以姿态为例，MEKF 不直接对四元数做普通加法，而是维护名义姿态 \(\hat q\) 和小角度误差 \(\delta\theta\)。更新时：

\[ q = \hat q \otimes \delta q,\qquad \delta q \approx \begin{bmatrix} 1\\ \frac{1}{2}\delta\theta \end{bmatrix} \]

滤波器的协方差在 \(\delta\theta\in\mathbb{R}^3\) 上，而不是在 4 维四元数上。这避免了单位四元数约束带来的秩亏问题。

ESKF 对 \(SE(3)\) 或扩展状态也采用同样思想：

量	所在空间	是否进入协方差
名义姿态 \(\hat R\)	\(SO(3)\)	否
姿态误差 \(\delta\theta\)	\(\mathbb{R}^3\)	是
名义位置 \(\hat p\)	\(\mathbb{R}^3\)	否
位置误差 \(\delta p\)	\(\mathbb{R}^3\)	是
bias 误差	\(\mathbb{R}^3\)	是

13.1 不变误差为什么更稳定 ⭐⭐⭐⭐¶

普通误差可以写成：

\[ \delta R = R-\hat R \]

但这不是旋转群上的自然误差。如果按“共同左乘后不变”命名，左不变误差写成：

\[ \eta_L=\hat R^{-1}R \]

如果按“共同右乘后不变”命名，右不变误差写成：

\[ \eta_R=R\hat R^{-1} \]

它们都仍然是 \(SO(3)\) 元素。再取 log：

\[ \xi_L=\log(\eta_L)^\vee \]

就得到切空间误差。InEKF 的核心思想正是选择与系统对称性匹配的不变误差，使误差动力学尽可能不依赖当前估计值。这样 Riccati 传播会更稳定，也更不容易产生虚假的可观测信息。

13A. SE₂(3) 群：IMU 导航的自然状态空间 ⭐⭐⭐¶

动机：为什么 \(SO(3) \times \mathbb{R}^6\) 不够用¶

在惯性导航中，IMU 的状态不仅包含姿态 \(R\)，还包含速度 \(v\) 和位置 \(p\)。一种直觉做法是把它们简单堆在一起：

\[ \text{state} = (R, v, p) \in SO(3) \times \mathbb{R}^3 \times \mathbb{R}^3 \]

这种"笛卡尔积"结构下，状态估计的误差动力学依赖当前估计值——EKF 的系统矩阵 \(F\) 取决于 \(\hat{R}\)、\(\hat{v}\)。这导致两个工程问题：

线性化精度差：当估计误差较大时，EKF 的线性化偏差显著，滤波器可能发散。
可观测性问题：标准 EKF 在数值层面会引入虚假的可观测信息，使协方差被低估。

Barrau 和 Bonnabel 在 TAC 2017（"The Invariant Extended Kalman Filter as a Stable Observer"）中发现了一个关键事实：如果把 \((R, v, p)\) 嵌入到一个更大的矩阵群中，使得 IMU 的运动方程具有**群仿射（group-affine）**性质，那么误差动力学的线性化将不依赖状态估计值。这个更大的群就是 \(SE_2(3)\)。

本质洞察：\(SE_2(3)\) 不是为了"更花哨的表示"，而是为了让 IMU 的误差动力学天然地具有状态无关性。在 \(SO(3) \times \mathbb{R}^6\) 中，误差传播矩阵随估计值变化；在 \(SE_2(3)\) 中，误差传播矩阵只依赖输入（加速度和角速度），不依赖当前状态估计。这正是不变扩展卡尔曼滤波器（InEKF）能够保证更好一致性的数学根源。

定义：\(5\times5\) 矩阵嵌入¶

\(SE_2(3)\) 群的元素由三个分量 \((R, v, p)\) 组成，嵌入为 \(5 \times 5\) 矩阵：

\[ \mathcal{T} = \begin{bmatrix} R & v & p\\ 0_{1 \times 3} & 1 & 0\\ 0_{1 \times 3} & 0 & 1 \end{bmatrix} \in SE_2(3) \]

其中 \(R \in SO(3)\)，\(v, p \in \mathbb{R}^3\)。

这个嵌入的命名来由是：\(SE_2(3)\) 可以看成 \(SO(3)\) 对两个 \(\mathbb{R}^3\) 向量的半直积——旋转同时作用于速度和位置。下标"2"表示有两个平移分量（\(v\) 和 \(p\)），而不是像 \(SE(3)\) 那样只有一个。

群运算：乘法与逆¶

两个 \(SE_2(3)\) 元素的乘法为：

\[ \mathcal{T}_1 \mathcal{T}_2 = \begin{bmatrix} R_1 R_2 & R_1 v_2 + v_1 & R_1 p_2 + p_1\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \]

与 \(SE(3)\) 的乘法结构类似，旋转部分直接复合，而速度和位置部分都被左侧的旋转"搬运"——这正是半直积的特征。

逆元为：

\[ \mathcal{T}^{-1} = \begin{bmatrix} R^\top & -R^\top v & -R^\top p\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix} \]

验证 \(\mathcal{T}\mathcal{T}^{-1} = I_{5 \times 5}\) 是直接的矩阵乘法练习。

运算	\(SE(3)\)	\(SE_2(3)\)
矩阵大小	\(4 \times 4\)	\(5 \times 5\)
旋转作用对象	1 个平移向量	2 个向量（\(v, p\)）
群结构	\(\mathbb{R}^3 \rtimes SO(3)\)	\((\mathbb{R}^3 \times \mathbb{R}^3) \rtimes SO(3)\)
典型应用	刚体位姿	IMU 导航状态

为什么 SE₂(3) 使 InEKF 成为可能¶

IMU 的连续时间运动方程为：

\[ \dot{R} = R(\omega_m - b_g - n_g)^\wedge, \quad \dot{v} = R(a_m - b_a - n_a) + g, \quad \dot{p} = v \]

如果把状态定义为 \(\mathcal{T} \in SE_2(3)\)，可以验证上述运动方程具有**群仿射**性质：

\[ f(\mathcal{T}) = \mathcal{T} \cdot \mathcal{U} + \mathcal{W} \]

其中 \(\mathcal{U}\) 和 \(\mathcal{W}\) 只取决于 IMU 输入和重力，不取决于 \(\mathcal{T}\) 本身。群仿射性质的关键后果是：定义左不变误差 \(\eta = \hat{\mathcal{T}}^{-1} \mathcal{T}\) 后，\(\eta\) 的时间导数只依赖输入，不依赖估计值 \(\hat{\mathcal{T}}\)。这意味着 InEKF 的系统矩阵 \(F\) 是"真实的"——不受线性化点的影响——从而保证了更好的一致性和收敛性。

如果不用 SE₂(3) 会怎样？ 在 \(SO(3) \times \mathbb{R}^6\) 上做 EKF 时，系统矩阵 \(F\) 包含 \(\hat{R}\) 和 \([\hat{R}(a_m - b_a)]_\times\) 等项，这些项随估计值变化。当初始误差较大或传感器退化时，错误的线性化会加剧可观测性矛盾，导致滤波器"过于自信"（协方差被低估而真实误差很大）。

Adjoint 矩阵与其幂零结构¶

\(SE_2(3)\) 的李代数元素为 \(\xi = [\xi_v, \xi_p, \xi_\phi]^\top \in \mathbb{R}^9\)（速度扰动、位置扰动、旋转扰动各 3 维），Adjoint 为 \(9 \times 9\) 矩阵：

\[ \operatorname{Ad}_{\mathcal{T}} = \begin{bmatrix} R & 0 & [v]_\times R\\ 0 & R & [p]_\times R\\ 0 & 0 & R \end{bmatrix} \]

注意 \(\operatorname{ad}_\xi\) 算子（Lie bracket 的矩阵表示）满足一个重要的幂零性质：

\[ \operatorname{ad}_\xi^3 = 0 \]

也就是说，\(\operatorname{ad}\) 的立方自动为零。这个性质直接影响 BCH 公式的截断——\(SE_2(3)\) 上的 BCH 展开到二阶就精确了，不需要无穷级数。在数值实现中，这意味着左/右 Jacobian 的级数是有限项的，计算更加高效和精确。相比之下，\(SO(3)\) 的 \(\operatorname{ad}\) 虽然也有循环性质（\(\operatorname{ad}^3 = -\theta^2 \operatorname{ad}\)），但并非幂零——级数需要通过三角函数封闭求和。

与 Barrau-Bonnabel TAC 2017 的连接¶

Barrau 和 Bonnabel 的核心贡献在于：

识别出 IMU 运动方程的群仿射结构。
证明在群仿射系统上，不变误差的线性化是精确的（不依赖估计值）。
证明 InEKF 是 \(SE_2(3)\) 上的局部渐近稳定观测器。

后续工作如 Hartley et al. (ICRA 2020) 的 "Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation" 将 InEKF 应用于足式机器人状态估计，取得了比标准 EKF 更好的鲁棒性和一致性。

⚠️ 概念误区：混淆 \(SE_2(3)\) 与 \(SE(3) \times \mathbb{R}^3\)

新手想法："\(SE_2(3)\) 不就是 \(SE(3)\) 加上一个速度向量吗？那 \(SE(3) \times \mathbb{R}^3\) 应该也行。"

实际上：\(SE(3) \times \mathbb{R}^3\) 是直积——旋转不作用于额外的 \(\mathbb{R}^3\)。但 \(SE_2(3)\) 的关键在于旋转**同时**作用于 \(v\) 和 \(p\)，使得整个状态空间形成一个有意义的群。在 \(SE(3) \times \mathbb{R}^3\) 上，误差动力学仍然依赖估计值，群仿射性质不成立，InEKF 的优势无法实现。两者的群乘法不同：\(SE_2(3)\) 中 \(v_1 + R_1 v_2\)（旋转作用于速度），而 \(SE(3) \times \mathbb{R}^3\) 中 \(v_1 + v_2\)（简单相加）。

判断标准：如果群乘法不会用旋转去搬运速度向量，那就不是 \(SE_2(3)\)。

练习¶

验证群封闭性：取两个 \(SE_2(3)\) 元素 \(\mathcal{T}_1 = (R_1, v_1, p_1)\) 和 \(\mathcal{T}_2 = (R_2, v_2, p_2)\)，其中 \(R_1, R_2 \in SO(3)\)，\(v_1, v_2, p_1, p_2 \in \mathbb{R}^3\)。写出 \(\mathcal{T}_1 \mathcal{T}_2\) 的 \(5 \times 5\) 矩阵形式，验证结果仍然具有 \(SE_2(3)\) 的结构（即左上角是 \(SO(3)\)，第 4-5 行是 \([0\;0\;0\;1\;0]\) 和 \([0\;0\;0\;0\;1]\)）。进一步验证 \(\mathcal{T} \mathcal{T}^{-1} = I_5\)。 ⭐⭐
解释为什么 \(\operatorname{ad}_\xi^3 = 0\) 使得 \(SE_2(3)\) 的 BCH 公式比 \(SO(3)\) 的更简单。如果在数值实现中你仍然使用了三角函数系数（如 \(\sin\theta/\theta\)），可能说明了什么问题？ ⭐⭐⭐

13B. Sim(3) 群：单目 SLAM 中的尺度自由度 ⭐⭐⭐¶

动机：单目相机为什么看不出绝对尺度¶

双目相机或 RGB-D 相机可以通过基线或深度传感器恢复场景的绝对尺度。但单目相机只有一个投影中心，无法区分"近处的小物体"和"远处的大物体"——它们在图像上可以产生完全相同的投影。

数学上，单目相机的投影模型为：

\[ \lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K [R \mid t] \begin{bmatrix} P \\ 1 \end{bmatrix} \]

如果同时把三维点 \(P\) 缩放 \(s\) 倍、把平移 \(t\) 也缩放 \(s\) 倍，投影结果不变。这意味着单目 SLAM 重建的三维地图只能确定到一个全局尺度因子。

在运动过程中，由于噪声和估计误差的累积，这个尺度因子会缓慢漂移（scale drift）。当回环检测（loop closure）发生时，系统发现起点和终点应该重合，但由于尺度漂移，地图的局部尺度不一致。修正回环误差时，如果只在 \(SE(3)\) 上优化位姿，无法吸收尺度差异——位姿图优化会试图通过旋转和平移来弥补尺度不匹配，导致地图扭曲。

\(Sim(3)\) 群正是为解决这个问题而引入的。

定义：7 自由度相似变换¶

\(Sim(3)\) 群的元素是三维空间的**相似变换**——在旋转和平移之外，增加一个全局缩放因子 \(s > 0\)：

\[ S = \begin{bmatrix} sR & t\\ 0 & 1 \end{bmatrix} \in Sim(3) \]

其中 \(R \in SO(3)\)，\(t \in \mathbb{R}^3\)，\(s \in \mathbb{R}^+\)。自由度为 \(3(\text{旋转}) + 3(\text{平移}) + 1(\text{尺度}) = 7\)。

相似变换作用于三维点：

\[ S \cdot \begin{bmatrix} p \\ 1 \end{bmatrix} = \begin{bmatrix} sRp + t \\ 1 \end{bmatrix} \]

它先旋转、再缩放、最后平移。

群运算¶

群乘法为：

\[ S_1 S_2 = \begin{bmatrix} s_1 R_1 \cdot s_2 R_2 & s_1 R_1 t_2 + t_1\\ 0 & 1 \end{bmatrix} = \begin{bmatrix} s_1 s_2 R_1 R_2 & s_1 R_1 t_2 + t_1\\ 0 & 1 \end{bmatrix} \]

尺度因子在乘法中相乘而非相加——这与旋转矩阵复合、平移向量在旋转下搬运的结构一致。

逆元为：

\[ S^{-1} = \begin{bmatrix} \frac{1}{s} R^\top & -\frac{1}{s} R^\top t\\ 0 & 1 \end{bmatrix} \]

可通过 \(S S^{-1} = I\) 直接验证。

与 SE(3) 的关系¶

\(Sim(3)\) 可以看作 \(SE(3)\) 的"带尺度扩展"。具体来说：

\[ Sim(3) / \mathbb{R}^+ \cong SE(3) \]

也就是说，如果"忘掉"尺度因子（令 \(s = 1\)），\(Sim(3)\) 就退化为 \(SE(3)\)。反过来，\(Sim(3)\) 是 \(SE(3)\) 在正实数乘法群 \(\mathbb{R}^+\) 方向上的扩展。

李代数 \(\mathfrak{sim}(3)\) 与指数/对数映射¶

\(Sim(3)\) 的李代数元素是 7 维向量 \(\zeta = [\rho, \phi, \sigma]^\top\)，其中 \(\rho \in \mathbb{R}^3\) 是平移分量，\(\phi \in \mathbb{R}^3\) 是旋转分量，\(\sigma \in \mathbb{R}\) 是尺度分量。矩阵形式为：

\[ \zeta^\wedge = \begin{bmatrix} \phi^\wedge + \sigma I & \rho\\ 0 & 0 \end{bmatrix} \]

指数映射 \(\exp: \mathfrak{sim}(3) \to Sim(3)\) 为：

\[ \exp(\zeta^\wedge) = \begin{bmatrix} e^\sigma \exp(\phi^\wedge) & W \rho\\ 0 & 1 \end{bmatrix} \]

其中 \(W\) 矩阵与 \(SE(3)\) 的 \(V\) 矩阵类似，但需要额外处理尺度因子的指数。当 \(\sigma = 0\) 时，\(W\) 退化为 \(SE(3)\) 的 \(V\) 矩阵；当 \(\phi = 0\) 时，\(W\) 退化为涉及 \(\frac{e^\sigma - 1}{\sigma}\) 的标量系数矩阵。完整表达式可参考 Eade 笔记中的 Sim(3) 部分。

对数映射从 \(Sim(3)\) 元素提取 7 维切向量：

\[ \sigma = \ln(s), \quad \phi = \log(R)^\vee, \quad \rho = W^{-1} t \]

注意尺度的对数映射特别简单——\(\sigma = \ln(s)\)——这是因为正实数乘法群 \((\mathbb{R}^+, \times)\) 通过对数映射与 \((\mathbb{R}, +)\) 同构。

在单目 SLAM 中的应用¶

ORB-SLAM2/3（Mur-Artal et al.）在回环修正阶段使用 \(Sim(3)\) 对齐。具体流程是：

回环检测：通过词袋模型发现当前帧与历史关键帧的视觉相似性。
Sim(3) 相对变换估计：用匹配的三维点对计算当前帧与回环帧之间的 \(Sim(3)\) 变换，包括尺度差异。
Essential Graph 优化：在位姿图的稀疏骨架（Essential Graph）上，以 \(Sim(3)\) 为位姿变量做图优化，允许每条边吸收局部尺度漂移。
地图融合：优化后把修正量分配到所有关键帧和地图点上。

如果在步骤 3 中只用 \(SE(3)\) 优化，尺度漂移无法被吸收——优化器会试图通过扭曲旋转和平移来弥补尺度差异，结果是地图变形。

如果不用 Sim(3) 会怎样？ 在纯单目 SLAM 系统中，长距离运动后尺度漂移可达 5-15%。如果回环修正只在 \(SE(3)\) 上进行，修正后的地图在回环处尺度突变，两段地图的拼接处出现明显的不连续。使用 \(Sim(3)\) 后，尺度差异被平滑地分配到整条轨迹上，地图的一致性显著改善。

⚠️ 编程陷阱：回环修正时忘记更新地图点的尺度

错误做法：在 \(Sim(3)\) 位姿图优化后，只更新了关键帧的位姿 \((R, t)\)，但忘记用修正的尺度因子 \(s\) 去缩放对应的三维地图点坐标。

现象：优化后位姿轨迹看起来正确，但地图点与位姿不一致——在回环处，三维点与重投影位置出现系统性偏差，追踪质量下降，甚至重定位失败。

根本原因：\(Sim(3)\) 变换同时作用于位姿和三维结构。位姿的尺度修正 \(s\) 意味着该关键帧"看到的世界"的尺度改变了，因此该帧观测到的三维点也必须相应缩放。

正确做法：优化完成后，对每个关键帧 \(i\)，将其观测到的地图点乘以对应的尺度修正因子：\(P_i' = s_i R_i P_i^{(\text{old})} + t_i^{(\text{new})}\)。ORB-SLAM2 的源码中 CorrectLoop() 函数展示了完整流程。

练习¶

证明 \(Sim(3)\) 在矩阵乘法下封闭：取两个一般的 \(Sim(3)\) 元素 \(S_1, S_2\)，验证 \(S_1 S_2\) 仍然具有 \(Sim(3)\) 的形式（特别是左上角 \(3 \times 3\) 块必须是 \(sR\) 的形式，其中 \(s > 0\)，\(R \in SO(3)\)）。 ⭐⭐
在一个单目 SLAM 系统中，两个关键帧之间的真实相对位姿为 \(T_{12} \in SE(3)\)，但由于尺度漂移，第二个关键帧的世界坐标系被缩放了 \(s = 1.1\) 倍。写出此时两帧之间正确的 \(Sim(3)\) 相对变换，并解释为什么 \(SE(3)\) 的 between factor 无法捕捉这个尺度差异。 ⭐⭐⭐

14. 与机械臂控制的连接 ⭐⭐¶

机械臂末端位姿控制也离不开 \(SE(3)\)。设当前末端位姿为 \(T\)，期望位姿为 \(T_d\)。常见误差定义之一是：

\[ e = \log(T^{-1}T_d)^\vee \]

它表示“从当前末端坐标出发，到期望末端位姿需要的局部运动”。如果采用另一种误差：

\[ e_s = \log(T_dT^{-1})^\vee \]

则误差表达在不同坐标系中。两者通过 Adjoint 相关。

在操作空间控制中，末端 twist 与关节速度关系为：

\[ \mathcal V = J(q)\dot q \]

如果 \(\mathcal V\) 是 body twist，则使用 body Jacobian；如果 \(\mathcal V\) 是 spatial twist，则使用 spatial Jacobian。两者满足类似：

\[ J_s = \operatorname{Ad}_T J_b \]

具体方向取决于 \(T\) 表示的是 body 到 space 还是 space 到 body。工程上必须把坐标定义写进接口注释，否则很容易出现末端沿错误方向修正的情况。

15. 常见错误诊断表 ⭐⭐¶

症状	可能原因	检查方法
`Exp(Log(T))` 不接近 `T`	Log 分支或切向量顺序错误	用小角度和普通角度分别测试
位姿图优化发散	残差定义与 Jacobian 扰动约定不一致	数值差分验证每一列
协方差变换方向反了	Adjoint 使用了 \(T\) 而非 \(T^{-1}\)	写出坐标系含义
四元数轨迹突然跳变	未处理 \(q\) 与 \(-q\) 双覆盖	插值前保证符号连续
机械臂末端往反方向走	body/spatial twist 混淆	检查 Jacobian 表达坐标系
SE(3) 平移误差异常	把 \(\rho\) 当成 \(p\)	检查是否需要 \(V^{-1}p\)
Ceres 更新后四元数不单位化	未使用 Manifold	使用 `EigenQuaternionManifold` 或自定义 Manifold

调试李群问题时，不要只看最终轨迹。应先用构造性测试验证每个局部公式：

SO(3): R^T R=I, det(R)=1
SO(3): Log(Exp(phi))≈phi
SE(3): T T^{-1}=I
SE(3): Exp(Log(T))≈T
Adjoint: T Exp(xi) T^{-1}=Exp(Ad_T xi)
Jacobian: 解析 Jacobian 与数值差分一致

16. 四元数：SO(3) 的双覆盖 ⭐⭐¶

虽然本专题主要使用旋转矩阵讲 \(SO(3)\)，工程代码中却经常用单位四元数表示姿态。原因不是四元数“更高级”，而是它在存储、插值和数值积分上更方便。

单位四元数写成：

\[ q = [w,\mathbf{v}],\qquad w\in\mathbb{R},\quad \mathbf{v}\in\mathbb{R}^3,\quad w^2+\|\mathbf{v}\|^2=1 \]

它和轴角的关系为：

\[ q = \left[ \cos\frac{\theta}{2}, \mathbf n\sin\frac{\theta}{2} \right] \]

这里的半角非常关键。旋转 \(\theta\) 对应四元数球面上的半角参数，这也是 \(q\) 和 \(-q\) 表示同一旋转的根源：

\[ q(\theta,n) = \left[\cos\frac{\theta}{2},n\sin\frac{\theta}{2}\right] \]

当 \(\theta\) 增加 \(2\pi\) 时，四元数变为 \(-q\)；当 \(\theta\) 增加 \(4\pi\) 时才回到 \(q\)。这说明单位四元数所在的 \(S^3\) 是 \(SO(3)\) 的双覆盖。

16.1 为什么四元数仍然不是欧氏向量 ⭐⭐¶

四元数有 4 个数，但只有 3 个自由度，因为它必须满足单位长度约束。若在优化中直接做：

\[ q_{\text{new}}=q+\delta q \]

通常会破坏：

\[ \|q_{\text{new}}\|=1 \]

因此工程中常用三维扰动更新四元数：

\[ q_{\text{new}} = q\otimes \exp_q(\delta\theta) \]

其中：

\[ \exp_q(\delta\theta) = \left[ \cos\frac{\|\delta\theta\|}{2}, \frac{\delta\theta}{\|\delta\theta\|}\sin\frac{\|\delta\theta\|}{2} \right] \]

小角度时：

\[ \exp_q(\delta\theta) \approx \left[ 1,\frac{1}{2}\delta\theta \right] \]

这正是 MEKF 和 ESKF 使用三维姿态误差的原因。

16.2 Eigen 四元数顺序陷阱 ⭐⭐¶

Eigen 的构造函数常写成：

Eigen::Quaterniond q(w, x, y, z);

但 coeffs() 返回顺序是：

[x, y, z, w]

如果把 coeffs() 直接存成 [w,x,y,z]，姿态会完全错误。建议在项目中明确写转换函数：

Eigen::Vector4d quaternion_to_wxyz(const Eigen::Quaterniond& q) {
  // Eigen 内部 coeffs 为 [x, y, z, w]，这里显式转成日志常用的 [w, x, y, z]。
  return Eigen::Vector4d(q.w(), q.x(), q.y(), q.z());
}

Eigen::Quaterniond quaternion_from_wxyz(const Eigen::Vector4d& v) {
  Eigen::Quaterniond q(v(0), v(1), v(2), v(3));
  // 归一化是边界层的必要保护，避免外部数据带来长度漂移。
  q.normalize();
  return q;
}

17. 从 SE(2) 到 SE(3)：先在平面里看清楚 ⭐¶

很多 SE(3) 公式看起来抽象，可以先看 SE(2)。平面位姿为：

\[ T= \begin{bmatrix} R(\theta) & p\\ 0 & 1 \end{bmatrix}, \qquad p=[x,y]^\top \]

复合为：

\[ T_1T_2= \begin{bmatrix} R_1R_2 & R_1p_2+p_1\\ 0 & 1 \end{bmatrix} \]

这和 SE(3) 共享“旋转群半直积平移空间”的结构，只是维度更低，并不意味着二者作为李群完全相同。差速机器人里程计就是最直观例子：机器人先转向，再向前走，同样的局部前进量会因为朝向不同落到世界坐标的不同方向。

17.1 反面例题：为什么不能直接加平移 ⭐¶

设机器人初始朝向世界 \(x\) 轴，先旋转 \(90^\circ\)，再在机体系前进 1 m。局部位移为：

\[ p_{\text{body}}=[1,0]^\top \]

世界坐标中的位移应为：

\[ p_{\text{world}}=R(90^\circ)p_{\text{body}}=[0,1]^\top \]

如果直接把局部位移加到世界坐标，会得到 \([1,0]^\top\)，方向错了。这就是半直积中 \(R_1p_2\) 项的直观意义。

17.2 位姿逆的推导 ⭐⭐¶

给定：

\[ T=\begin{bmatrix}R&p\\0&1\end{bmatrix} \]

设：

\[ T^{-1}=\begin{bmatrix}A&b\\0&1\end{bmatrix} \]

要求：

\[ TT^{-1}=I \]

展开：

\[ \begin{bmatrix} R A & Rb+p\\ 0 & 1 \end{bmatrix} = \begin{bmatrix} I & 0\\ 0 & 1 \end{bmatrix} \]

因此：

\[ RA=I\Rightarrow A=R^\top \]

以及：

\[ Rb+p=0\Rightarrow b=-R^\top p \]

所以：

\[ T^{-1}= \begin{bmatrix} R^\top & -R^\top p\\ 0 & 1 \end{bmatrix} \]

这个公式是位姿图、tf、机械臂坐标变换中最常用的基本操作之一。注意平移项不是 \(-p\)，而是 \(-R^\top p\)，因为反向变换中的平移要先换到反向坐标系。

18. Ceres、GTSAM、Sophus 的接口如何对应 ⭐⭐¶

同一个数学动作，在不同库中名字不同：

数学动作	Sophus	GTSAM	Ceres
\(\exp(\xi^\wedge)\)	`SE3::exp(xi)`	`Pose3::Expmap(xi)`	由自定义 `Manifold::Plus` 或 `ProductManifold` 实现
\(\log(T)^\vee\)	`T.log()`	`Pose3::Logmap(T)`	由自定义 `Manifold::Minus` 实现
状态更新	`T * SE3::exp(dx)`	`pose.retract(dx)`	`Plus(x, dx)`
局部误差	`(T1.inverse()*T2).log()`	`localCoordinates`	`Minus(y, x)`

18.1 Ceres Manifold 的本质 ⭐⭐⭐¶

Ceres 的优化变量存在一个 ambient size 和 tangent size。四元数 ambient size 是 4，tangent size 是 3；SE(3) 若用四元数加平移存储，ambient size 是 7，tangent size 是 6。

Plus 的数学含义是：

\[ x_{\text{new}} = x \boxplus \delta \]

Minus 的数学含义是：

\[ \delta = y \boxminus x \]

需要特别注意：Ceres 只定义 Manifold 接口，并不替所有 Lie group 提供统一的泛型 \(SE(3)\) 实现。单位四元数可以使用 EigenQuaternionManifold，位姿变量常见做法是把平移 Manifold 与四元数 Manifold 组合起来，或直接实现项目自己的 \(SE(3)\) Manifold。

一个 SE(3) Manifold 的接口草图可以设计为：

struct PoseManifoldConcept {
  // x: [tx, ty, tz, qx, qy, qz, qw]
  // delta: [rho_x, rho_y, rho_z, omega_x, omega_y, omega_z]
  bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    // 1. 从数组读取平移和四元数。
    // 2. 把 delta 通过 SE(3) exp 变成小位姿。
    // 3. 按约定左乘或右乘更新。
    // 4. 写回数组并归一化四元数。
    return true;
  }

  bool Minus(const double* y, const double* x, double* y_minus_x) const {
    // 1. 读取两个位姿。
    // 2. 计算相对位姿。
    // 3. 对相对位姿取 log，得到 6 维切向量。
    return true;
  }
};

这段接口草图说明：Ceres 本身不替你决定左扰动、右扰动或切向量顺序。Manifold 的实现者必须把数学约定写清楚。

18.2 GTSAM 的 `retract` 和 `localCoordinates` ⭐⭐¶

GTSAM 把流形更新称为 retract：

\[ X'=\operatorname{retract}_X(\delta) \]

把局部坐标差称为 localCoordinates：

\[ \delta=\operatorname{localCoordinates}_X(Y) \]

这和专题 2 的 Retraction 语言一致。使用 GTSAM 时，重要的是确认切向量顺序和右扰动约定，而不是只看函数名。

18.3 Sophus 的简洁性与边界 ⭐⭐¶

Sophus 的优势是贴近公式：

Sophus::SE3d T_new = T * Sophus::SE3d::exp(dx);
Eigen::Matrix<double, 6, 1> e = (T_meas.inverse() * T_pred).log();

这很适合教学和研究代码。但在大型系统里仍建议封装一层项目内部的 Pose 类型或转换函数，避免 Sophus/GTSAM/Ceres 的约定在业务代码里到处扩散。

19. 学习路线：从公式到可用能力 ⭐¶

学习李群最怕两种极端：只背公式，或者只读抽象数学。推荐按四层推进：

层级	目标	检查方式
几何直觉	知道旋转和位姿为什么不是向量	能解释半直积和双覆盖
公式推导	能推导 Exp/Log/Adjoint/Jacobian	能手推 Rodrigues 和 SE(3) 逆
工程实现	能写小角度分支和单元测试	`Exp(Log(T))≈T` 通过
系统应用	能接到 SLAM/滤波/控制	能写位姿图残差和扰动约定

每学一个公式，都建议问三个问题：

它把哪个对象从哪个空间映射到哪个空间？
它使用的是左扰动还是右扰动？
它的输入输出切向量顺序是什么？

如果这三个问题答不上来，公式很容易在工程中被误用。

20. 历史脉络：从刚体运动到现代 SLAM ⭐⭐¶

李群进入机器人学不是偶然的。刚体运动问题在经典力学、机构学和微分几何中反复出现，只是不同领域使用的语言不同。

历史阶段	主要语言	今天的对应物
刚体运动几何	旋转、平移、螺旋轴	\(SE(3)\) 群运算
Sophus Lie 的连续群理论	连续变换群与无穷小生成元	李群与李代数
机构学螺旋理论	twist、wrench、screw	空间速度、空间力
现代机器人学	指数坐标、POE 公式	机械臂运动学
SLAM/VIO	位姿图、IMU 预积分、流形优化	\(SO(3)/SE(3)\) 上的估计

Lynch & Park 教材中常见的 Product of Exponentials 公式：

\[ T(\theta)=e^{[S_1]\theta_1}e^{[S_2]\theta_2}\cdots e^{[S_n]\theta_n}M \]

和 SLAM 中的位姿更新：

\[ T_{\text{new}}=T\exp(\delta\xi^\wedge) \]

本质上用的是同一套语言：把有限刚体运动表示为无穷小生成元的指数。机械臂关节角 \(\theta_i\) 乘以螺旋轴 \(S_i\)，得到每个关节的指数运动；SLAM 优化增量 \(\delta\xi\) 乘以 \(\wedge\)，得到当前位姿附近的小指数运动。

20.1 twist 与 wrench 的对偶关系 ⭐⭐⭐¶

twist 表示刚体速度，wrench 表示力和力矩。它们之间的功率配对为：

\[ P = F^\top \mathcal V \]

其中 \(\mathcal V\) 是 6 维 twist，\(F\) 是 6 维 wrench。坐标变换时，如果 twist 使用 Adjoint：

\[ \mathcal V_b=\operatorname{Ad}_T \mathcal V_a \]

为了保持功率不变：

\[ F_b^\top \mathcal V_b = F_a^\top \mathcal V_a \]

wrench 必须使用对偶变换：

\[ F_b = \operatorname{Ad}_T^{-\top}F_a \]

这条公式解释了为什么机械臂动力学、接触力估计和操作空间控制中会频繁出现 \(\operatorname{Ad}^{-\top}\)。它不是符号技巧，而是功率守恒的要求。

20.2 为什么同一套数学横跨感知、规划与控制 ⭐⭐¶

方向	使用的李群工具	解决的问题
SLAM	\(\log(Z^{-1}T_i^{-1}T_j)\)	位姿图误差
VIO	\(SO(3)\) 预积分、右 Jacobian	IMU 噪声传播
机械臂	POE、body/spatial Jacobian	正运动学和速度映射
运动规划	\(SE(3)\) 插值、局部坐标	位姿空间轨迹
状态估计	不变误差、Adjoint 协方差	一致性和误差传播
接触控制	twist/wrench 对偶	速度与力的坐标变换

这说明李群不是某个算法的局部技巧，而是一种描述机器人空间运动的公共语言。学透这一章，后续读位姿图、IMU 预积分、机械臂 Jacobian、操作空间控制和 InEKF 时，许多公式会变成同一件事的不同投影。

21. 核心教材与资料 ⭐¶

资料	定位	推荐读法
Lynch & Park Modern Robotics Ch 3	机器人直觉	先读刚体运动和螺旋理论
Solà 等 A micro Lie theory	符号统一	用作公式和扰动约定主参考
Barfoot State Estimation for Robotics	SLAM/VIO	重点读矩阵李群和估计章节
Hall Lie Groups	数学基础	理解矩阵李群、指数、BCH
Ethan Eade notes	工程速查	查 SO(3)/SE(3)/Sim(3) 闭式

推荐顺序是：先用 Lynch & Park 建立刚体直觉，再用 Solà 统一符号，然后用 Barfoot 连接状态估计，最后按需要查 Hall 的数学证明。

22. 本专题掌握度诊断与后续学习自测 ⭐¶

在进入后续 B5 流形滤波、B5 因子图或机械臂操作空间控制前，建议用下面的问题检查掌握程度。它们不是记忆题，而是用来暴露概念缝隙。

自测问题	合格回答应包含
为什么 \(R_1-R_2\) 不是旋转误差？	旋转群不封闭于减法，差值不在切空间的自然坐标中
为什么 SE(3) 的平移 log 是 \(V^{-1}p\)？	twist 积分中旋转和平移耦合，\(\rho\) 不是最终平移
Adjoint 改变的是什么？	改变切向量表达坐标系，而不是改变物理运动本身
左扰动和右扰动差别在哪里？	小量表达坐标不同，Jacobian 和 Adjoint 方向不同
为什么四元数需要 Manifold？	四元数有单位约束，优化增量应为三维切向量
为什么 SE(3) 误差要加权？	米和弧度没有天然统一尺度

如果这些问题答不清，后续常见表现是：能看懂单个公式，但一到代码实现就不知道该用左乘还是右乘、该用 \(T\) 还是 \(T^{-1}\)、该把旋转放在前三维还是后三维。

22.1 三个最低限度的手算能力 ⭐⭐¶

学习完本专题后，至少应能在纸上完成三件事：

从矩阵指数推导 Rodrigues 公式。
从 \(TT^{-1}=I\) 推导 SE(3) 的逆。
从 \(T\exp(\xi^\wedge)T^{-1}\) 解释 Adjoint 的意义。

这三件事分别对应：

手算能力	后续用途
Rodrigues	IMU 积分、姿态误差、SO(3) Jacobian
SE(3) 逆	tf、位姿图、相对测量
Adjoint	协方差传播、机械臂 Jacobian、接触力变换

22.2 三个最低限度的代码能力 ⭐⭐¶

也建议完成三组最小代码测试：

1. 随机生成小旋转 phi，检查 Log(Exp(phi))≈phi。
2. 随机生成 SE(3) 位姿 T，检查 T*T.inverse()≈I。
3. 随机生成 xi 和 T，检查 T*Exp(xi)*T.inverse()≈Exp(Ad_T*xi)。

这些测试可以放进任何使用李群库的项目模板中。每次升级 Sophus、GTSAM、manif 或修改内部 Pose 类型时，都应重新运行。

22.3 本专题统一符号约定 ⭐¶

为避免后续章节混乱，本专题默认采用下面约定：

符号	含义
\(R\in SO(3)\)	三维旋转矩阵
\(T\in SE(3)\)	三维刚体位姿
\(\omega\in\mathbb{R}^3\)	旋转切向量
\(\rho\in\mathbb{R}^3\)	SE(3) twist 的平移分量
\(\xi=[\rho,\omega]^\top\)	平移在前、旋转在后的 6 维切向量
\((\cdot)^\wedge\)	向量到李代数矩阵
\((\cdot)^\vee\)	李代数矩阵到向量
\(\operatorname{Ad}_T\)	群元素对切向量的伴随变换
\(\operatorname{ad}_\xi\)	李代数 bracket 的矩阵表示

如果后续文档或库采用 \([\omega,\rho]\) 顺序，会在局部重新声明。所有跨库转换都应以这里的表为默认参考。

23. 练习 ⭐⭐¶

手推 Rodrigues 公式中的奇数项和偶数项分组，说明 \(N^3=-N\) 为什么成立。⭐⭐
证明 \(SO(3)\) 的矩阵指数结果满足 \(R^\top R=I\) 和 \(\det R=1\)。⭐⭐
给定 \(T=[R,p;0,1]\)，推导 \(T^{-1}\)，并解释为什么平移项是 \(-R^\top p\)。⭐⭐
使用你熟悉的库写一个测试，验证 Exp(Log(T))≈T。要求分别测试小角度、普通角度、接近 \(\pi\) 的旋转。⭐⭐
查阅 Sophus 和 GTSAM 的切向量顺序，写一个转换函数，并用一个随机位姿验证转换前后的扰动含义。⭐⭐⭐

23.5 跨章综合题 ⭐⭐⭐¶

本题综合专题 1（流形与切空间）、专题 2（Retraction 与流形优化）和本专题的李群知识。

问题：设 SLAM 后端有一个位姿图，包含三个位姿节点 \(T_0, T_1, T_2 \in SE(3)\) 和两条相对测量边 \(Z_{01}, Z_{12}\)。现在需要用 Gauss-Newton 方法优化这三个位姿。

请完成以下步骤：

写出位姿图的残差定义 \(e_{ij} = \log(Z_{ij}^{-1} T_i^{-1} T_j)^\vee\)，并解释为什么这个残差定义尊重了 SE(3) 的群结构（联系专题 1 中"流形上的距离不能用减法"的结论）。
说明右扰动 \(T_i' = T_i \exp(\delta\xi_i^\wedge)\) 对应的是专题 2 中哪种 Retraction？Retraction 的选择对优化收敛有什么影响？
写出法方程 \(J^T \Sigma^{-1} J \delta\xi = -J^T \Sigma^{-1} e\) 中 Jacobian \(J\) 的块结构（对 \(\delta\xi_i\) 和 \(\delta\xi_j\) 分别求导），说明 Adjoint 项出现在哪里、为什么出现。
如果固定 \(T_0\) 为世界原点，法方程会出现什么变化？这与 SLAM 中的"gauge freedom"有什么关系？

24. 本专题小结 ⭐¶

李群把机器人运动中的几何和代数统一起来。\(SO(3)\) 解决旋转，\(SE(3)\) 解决刚体位姿；李代数提供局部线性空间，指数和对数映射连接小扰动与真实状态；Adjoint 处理扰动的坐标系变换；BCH 解释扰动复合为什么不是简单相加。

工程上最重要的不是背公式，而是建立三个习惯：

状态留在群上，增量放在切空间。
每个 Jacobian 都要说明左扰动还是右扰动。
每个库边界都要确认切向量顺序、四元数顺序和 Adjoint 约定。

这些习惯会直接影响 SLAM、VIO、机械臂控制、IMU 预积分、因子图优化和流形滤波。后续专题中的 ESKF、InEKF、位姿图优化和 SE-Sync，都建立在本专题的语言之上。

常见陷阱与故障排查 ⭐⭐¶

⚠️ 陷阱一：把六维切向量当作普通位姿相加。 \(\xi\) 是局部增量，不是全局坐标；T += dx 这类写法会破坏 \(SE(3)\) 的群结构。

⚠️ 陷阱二：只看四元数顺序，不看扰动方向。 Hamilton/JPL 解决的是存储和乘法约定，左扰动/右扰动解决的是误差定义；两者是不同维度的问题。

⚠️ 陷阱三：Adjoint 方向写反。 从 body 切空间搬到 world 切空间和反向搬运相差一个 \(\operatorname{Ad}_{T}^{-1}\)，错一次就会让协方差椭球转到错误坐标系。

故障排查现象	优先检查	判断方法
`Exp(Log(T))` 小角度正确、大角度错误	\(\log SO(3)\) 的 \(\pi\) 附近分支	用接近 180° 的旋转做单元测试
GTSAM 与 Sophus 扰动结果相反	切向量顺序和左右扰动	用同一个小扰动比较 \(T\exp(\xi^\wedge)\) 与 \(\exp(\xi^\wedge)T\)
协方差旋转后方向不对	Adjoint 使用方向	测试纯平移位姿下角速度是否诱导线速度项

本质洞察：李群教学的核心不是让公式更抽象，而是把“状态更新”和“误差更新”分开：状态必须留在流形上，误差必须留在切空间里。

练习 ⭐⭐¶

给定 \(T=[R,p;0,1]\) 和右扰动 \(\delta\xi=[\delta\rho,\delta\omega]\)，写出一阶近似下平移项如何变化。⭐⭐
选一个随机 \(SE(3)\) 位姿，分别计算 \(\operatorname{Ad}_T\xi\) 与 \((T\exp(\xi^\wedge)T^{-1})^\vee\)，验证二者一致。⭐⭐
用同一个 \(\delta\theta\) 测试 Hamilton 与 JPL 四元数库，记录需要变号或重排的具体位置。⭐⭐⭐

25. 李群指数映射的完整推导补充 ⭐⭐¶

25.1 SO(3) Rodrigues 公式——回顾与交叉引用 ⭐⭐¶

Rodrigues 公式的完整推导已在 §3.1 给出。这里简要回顾核心结论及其在后续推导中的复用方式。

回顾 §3.1：利用单位轴反对称矩阵的循环性 \((n^\wedge)^3 = -n^\wedge\)，将矩阵指数的 Taylor 级数按奇偶项分组，封闭求和得到：

\[ \boxed{\exp(\theta n^\wedge) = I + \sin\theta\, n^\wedge + (1-\cos\theta)(n^\wedge)^2} \]

这个公式的重要性在于：它把无穷级数变成三项有限和，使得 SO(3) 指数映射可以闭式计算。同样的奇偶项分组技巧在 \(J_l\)、\(J_r\) 的推导（专题4）和 \(V\) 矩阵推导（下节）中反复出现——掌握 §3.1 的推导路径是理解这些后续公式的关键。

25.2 SE(3) 指数映射中 \(V\) 矩阵的推导 ⭐⭐⭐¶

SE(3) 的李代数元素：

\[ \xi^\wedge = \begin{pmatrix} \omega^\wedge & \rho \\ 0 & 0 \end{pmatrix} \in \mathfrak{se}(3) \]

矩阵指数：

\[ \exp(\xi^\wedge) = \sum_{k=0}^{\infty} \frac{1}{k!}(\xi^\wedge)^k \]

计算前几项幂次：

\[ (\xi^\wedge)^k = \begin{pmatrix} (\omega^\wedge)^k & (\omega^\wedge)^{k-1}\rho \\ 0 & 0 \end{pmatrix} \quad \text{（不完全正确，需要更仔细的递推）} \]

正确的递推结果是：

\[ \exp(\xi^\wedge) = \begin{pmatrix} \exp(\omega^\wedge) & V\rho \\ 0 & 1 \end{pmatrix} \]

其中 \(V\) 矩阵为：

\[ V = I + \frac{1-\cos\theta}{\theta^2}\omega^\wedge + \frac{\theta - \sin\theta}{\theta^3}(\omega^\wedge)^2 \]

\(V\) 矩阵的推导方法：将 \(\exp(t\xi^\wedge)\) 展开为 \(t\) 的级数，提取平移项的积分结构：

\[ V = \int_0^1 \exp(s\cdot\omega^\wedge)\,ds = \sum_{k=0}^{\infty} \frac{(\omega^\wedge)^k}{(k+1)!} \]

利用 \((n^\wedge)^3 = -n^\wedge\) 的循环性，这个级数同样可以封闭求和。

\(V\) 矩阵的物理意义：如果一个刚体同时以角速度 \(\omega\) 旋转并以线速度 \(\rho\) 平移，则经过单位时间后的实际位移不是 \(\rho\)，而是 \(V\rho\)。\(V\) 矩阵修正了旋转对平移的"耦合效应"——物体一边转一边走，走出来的是螺旋线而非直线。

⚠️ 概念误区：认为 SE(3) 的指数映射只是"旋转 + 平移"的简单组合

如果 SE(3) 只是 SO(3) 和 \(\mathbb{R}^3\) 的直积，那 \(\exp\) 就是各自 exp 的直积：\((\exp(\omega^\wedge), \rho)\)。但 SE(3) 是半直积，旋转和平移有耦合。这个耦合体现为 \(V\) 矩阵——它是"旋转在进行中对平移的积累效应"。忽略 \(V\) 矩阵（如使用分解 retraction）在小扰动下可接受，但在螺旋运动的轨迹积分中会引入系统误差。

本章常见误解汇总¶

误解	正确理解
"旋转矩阵的逆等于转置是巧合"	这是正交矩阵的定义性质，\(R^\top R = I\) 意味着列向量构成标准正交基，逆就是"反着转"
"李代数元素可以相加"等同于"旋转可以相加"	李代数上的加法只在小扰动时有效；两个大旋转的复合必须通过群乘法 \(R_1 R_2\)，BCH 公式给出精确修正
"SE(3) 指数映射中平移分量就是 \(\rho\)"	平移分量是 \(V\rho\)，其中 \(V\) 矩阵编码旋转对平移的积分耦合效应；\(V = I\) 仅当旋转为零
"SO(3) 的 \(\mathrm{Ad}_R = R\) 可以推广到 SE(3)"	SO(3) 的简化是特例；SE(3) 的 Adjoint 是 \(6\times6\) 矩阵，含 \([t]_\times R\) 耦合项
"切向量排序 \([\rho,\omega]\) 和 \([\omega,\rho]\) 只是记号差异"	排序不同会导致 \(6\times6\) 矩阵的分块位置互换，混用是 SE(3) 工程中最常见的 bug 来源
"\(SE_2(3)\) 就是 \(SE(3) \times \mathbb{R}^3\)"	\(SE_2(3)\) 是半直积，旋转同时作用于速度和位置；直积结构下群仿射性质不成立，InEKF 优势无法实现
"Rodrigues 公式只是一种旋转表示的计算公式"	它是矩阵指数在 \(SO(3)\) 上的闭式求和结果，同样的分组技巧在 \(J_l\)、\(J_r\)、\(V\) 矩阵推导中反复出现

26. 延伸阅读 ⭐¶

资源	难度	核心价值
Sola, Deray, Atchuthan "A micro Lie theory for state estimation" (2018)	⭐⭐	SLAM 社区标准参考，公式全表
Chirikjian "Stochastic Models, Information Theory, and Lie Groups" Vol.1-2	⭐⭐⭐⭐	最深入的李群概率论
Eade "Lie Groups for Computer Vision" (tech report)	⭐⭐	简洁实用，适合工程师快速入门
Murray, Li, Sastry "A Mathematical Introduction to Robotic Manipulation"	⭐⭐⭐	机器人李群方法的开山之作
Hall "Lie Groups, Lie Algebras, and Representations" (GTM 222)	⭐⭐⭐	纯数学标准参考
Barfoot "State Estimation for Robotics" 2nd ed. (2024) Ch.7-8	⭐⭐⭐	SE(3) 不确定性的工程标准
manif 库 C++ header-only (github.com/artivis/manif)	⭐⭐	Sola 论文的代码实现
Sophus 库 (github.com/strasdat/Sophus)	⭐⭐	最轻量的 SO3/SE3 C++ 实现

27. 累积项目：本章新增模块 ⭐¶

项目方向：手写几何验证库

本章新增：SO(3)/SE(3) 完整操作实现

import numpy as np
from scipy.linalg import expm, logm

class SO3:
    """SO(3) 李群基本操作"""

    @staticmethod
    def hat(omega):
        """R^3 -> so(3) 反对称矩阵"""
        return np.array([[0, -omega[2], omega[1]],
                         [omega[2], 0, -omega[0]],
                         [-omega[1], omega[0], 0]])

    @staticmethod
    def vee(Omega):
        """so(3) -> R^3"""
        return np.array([Omega[2,1], Omega[0,2], Omega[1,0]])

    @staticmethod
    def exp(phi):
        """Rodrigues 闭式指数映射"""
        theta = np.linalg.norm(phi)
        if theta < 1e-10:
            return np.eye(3) + SO3.hat(phi)  # 一阶近似
        n = phi / theta
        N = SO3.hat(n)
        return np.eye(3) + np.sin(theta)*N + (1-np.cos(theta))*(N@N)

    @staticmethod
    def log(R):
        """SO(3) 对数映射"""
        cos_theta = (np.trace(R) - 1) / 2
        cos_theta = np.clip(cos_theta, -1, 1)
        theta = np.arccos(cos_theta)
        if theta < 1e-10:
            return SO3.vee(R - R.T) / 2  # 小角度近似
        return theta / (2 * np.sin(theta)) * SO3.vee(R - R.T)

# 验证 Exp(Log(R)) = R
R = SO3.exp(np.array([0.5, -0.3, 0.8]))
phi_recovered = SO3.log(R)
R_recovered = SO3.exp(phi_recovered)
print("往返误差:", np.linalg.norm(R - R_recovered))  # ~1e-16

后续专题将扩展： - 专题4：实现 \(J_l\)/\(J_r\) 闭式并验证 - 专题5：实现 Concentrated Gaussian 采样与协方差传播

符号表¶

符号	含义	首次出现
\(SO(3)\)	三维特殊正交群（旋转矩阵群）	§1.1
\(SE(3)\)	三维特殊欧氏群（刚体位姿群）	§1.1
\(\mathfrak{g}\)	李群 \(G\) 的李代数（单位元处切空间 \(T_eG\)）	§2
\(\mathfrak{so}(3)\)	\(SO(3)\) 的李代数（\(3\times3\) 反对称矩阵集）	§2
\((\cdot)^\wedge\)	hat 运算：向量 \(\to\) 李代数矩阵	§2
\((\cdot)^\vee\)	vee 运算：李代数矩阵 \(\to\) 向量	§2
\(\exp(\cdot)\)	矩阵指数映射 \(\mathfrak{g} \to G\)	§3
\(\log(\cdot)\)	矩阵对数映射 \(G \to \mathfrak{g}\)	§4
\(\theta\)	旋转角（\(\theta = \\|\omega\\|\)）	§3.1
\(n\)	旋转轴单位向量	§3.1
\(\operatorname{Ad}_T\)	群元素 \(T\) 的伴随表示（Adjoint）	§22.3
\(\operatorname{ad}_\xi\)	李代数 bracket 的矩阵表示（adjoint）	§22.3
\(\xi = [\rho, \omega]^\top\)	\(SE(3)\) 的 6 维切向量（平移在前、旋转在后）	§22.3
\(V\)	\(SE(3)\) 指数映射中的平移耦合矩阵	§25.2

30_李群基础与SO3_SE3

专题3：李群基础与 SO(3)/SE(3) 专论¶

预计阅读时间¶

前置自测 ⭐¶

0. 为什么机器人必须学习李群 ⭐¶

1. 从流形到李群：多了一个群运算 ⭐¶

1.1 矩阵李群 ⭐¶

2. 李代数：单位元处的线性化空间 ⭐¶

2.1 为什么李代数可以做加法 ⭐¶

3. 指数映射：从小扰动回到群 ⭐⭐¶

3.1 Rodrigues 公式的推导 ⭐⭐¶

3.2 小角度数值稳定 ⭐⭐¶

4. 对数映射：从旋转回到局部坐标 ⭐⭐¶

4.1 \(\pi\) 附近为什么麻烦 ⭐⭐⭐¶

5. SE(3)：旋转和平移的半直积 ⭐⭐¶

5.1 se(3) 与 twist ⭐⭐¶

5.2 SE(3) 指数映射 ⭐⭐¶

5.3 SE(3) 对数映射 ⭐⭐¶

6. Adjoint：扰动在哪个坐标系表达 ⭐⭐¶

6.1 Adjoint 的工程意义 ⭐⭐¶

7. ad 与 BCH：为什么扰动不能简单相加 ⭐⭐⭐¶

8. 左扰动、右扰动与 Plus/Minus ⭐⭐¶

8.1 库约定对比 ⭐⭐¶

9. SO(3) 与 SE(3) 的度量差异 ⭐⭐⭐¶

10. 代码验证：最小 SO(3)/SE(3) 单元测试 ⭐⭐¶

11. 左 Jacobian 与右 Jacobian：从”扰动相加”到”扰动搬运” ⭐⭐⭐¶

11.1 为什么 Jacobian 不能忽略 ⭐⭐⭐¶

11.2 小角度展开 ⭐⭐¶

12. 位姿图残差：李群公式如何进入 SLAM ⭐⭐¶

12.1 为什么要先乘再取 log ⭐⭐¶

12.2 一阶线性化的结构 ⭐⭐⭐¶

12.3 用数值差分验证解析 Jacobian ⭐⭐¶

13. 与 ESKF/MEKF/InEKF 的连接 ⭐⭐⭐¶

13.1 不变误差为什么更稳定 ⭐⭐⭐⭐¶

13A. SE₂(3) 群：IMU 导航的自然状态空间 ⭐⭐⭐¶

动机：为什么 \(SO(3) \times \mathbb{R}^6\) 不够用¶

定义：\(5\times5\) 矩阵嵌入¶

群运算：乘法与逆¶

为什么 SE₂(3) 使 InEKF 成为可能¶

Adjoint 矩阵与其幂零结构¶

与 Barrau-Bonnabel TAC 2017 的连接¶

练习¶

13B. Sim(3) 群：单目 SLAM 中的尺度自由度 ⭐⭐⭐¶

动机：单目相机为什么看不出绝对尺度¶

定义：7 自由度相似变换¶

群运算¶

与 SE(3) 的关系¶

李代数 \(\mathfrak{sim}(3)\) 与指数/对数映射¶

在单目 SLAM 中的应用¶

练习¶

14. 与机械臂控制的连接 ⭐⭐¶

15. 常见错误诊断表 ⭐⭐¶

16. 四元数：SO(3) 的双覆盖 ⭐⭐¶

16.1 为什么四元数仍然不是欧氏向量 ⭐⭐¶

16.2 Eigen 四元数顺序陷阱 ⭐⭐¶

17. 从 SE(2) 到 SE(3)：先在平面里看清楚 ⭐¶

17.1 反面例题：为什么不能直接加平移 ⭐¶

17.2 位姿逆的推导 ⭐⭐¶

18. Ceres、GTSAM、Sophus 的接口如何对应 ⭐⭐¶

18.1 Ceres Manifold 的本质 ⭐⭐⭐¶

18.2 GTSAM 的 retract 和 localCoordinates ⭐⭐¶

18.3 Sophus 的简洁性与边界 ⭐⭐¶

19. 学习路线：从公式到可用能力 ⭐¶

20. 历史脉络：从刚体运动到现代 SLAM ⭐⭐¶

20.1 twist 与 wrench 的对偶关系 ⭐⭐⭐¶

20.2 为什么同一套数学横跨感知、规划与控制 ⭐⭐¶

21. 核心教材与资料 ⭐¶

22. 本专题掌握度诊断与后续学习自测 ⭐¶

22.1 三个最低限度的手算能力 ⭐⭐¶

22.2 三个最低限度的代码能力 ⭐⭐¶

22.3 本专题统一符号约定 ⭐¶

23. 练习 ⭐⭐¶

23.5 跨章综合题 ⭐⭐⭐¶

24. 本专题小结 ⭐¶

常见陷阱与故障排查 ⭐⭐¶

练习 ⭐⭐¶

25. 李群指数映射的完整推导补充 ⭐⭐¶

25.1 SO(3) Rodrigues 公式——回顾与交叉引用 ⭐⭐¶

25.2 SE(3) 指数映射中 \(V\) 矩阵的推导 ⭐⭐⭐¶

本章常见误解汇总¶

18.2 GTSAM 的 `retract` 和 `localCoordinates` ⭐⭐¶