ch08_lie_groups

第八章　矩阵李群 §

Chapter 8   Matrix Lie Groups §

---

本章目标 / Chapter Goals

第七章指出旋转矩阵不是向量——$SO(3)$ 不满足向量空间的封闭加法。那么，我们如何对旋转状态做 MAP 估计、EKF、GVI？答案是李群（Lie group）——它既是群（支持乘法复合）又是光滑流形（支持微积分）。本章的核心工具是指数映射，它将向量空间（李代数）与李群联系起来，为旋转/位姿状态提供了一套完整的扰动微积分、概率统计框架。

Chapter 7 showed that rotation matrices are not vectors — $SO(3)$ is not closed under vector addition. How then do we apply MAP, EKF, GVI to rotation states? The answer is Lie groups — mathematical structures that are simultaneously groups (with a multiplication composition) and smooth manifolds (supporting calculus). The central tool is the exponential map, which connects a vector space (the Lie algebra) to the Lie group, providing perturbation calculus and probability for rotation/pose states.

---

8.1 几何 / Geometry §

8.1.1 特殊正交群与特殊欧氏群 / SO(3) and SE(3) §

中文

特殊正交群 $SO(3)$（旋转群）：

SO(3) = \{\mathbf{C} \in \mathbb{R}^{3\times3} \mid \mathbf{C}\mathbf{C}^T = \mathbf{1},\; \det\mathbf{C} = +1\}. \tag{8.1}

$SO(3)$ 不是向量空间（其子空间）：

• 两个旋转矩阵之和不一定是旋转矩阵：${\mathbf{C}}_1+{\mathbf{C}}_2\notin SO(3)$
• 零矩阵不属于 $SO(3)$

特殊欧氏群 $SE(3)$（位姿群）：

SE(3) = \left\{\mathbf{T} = \begin{bmatrix}\mathbf{C} & \mathbf{r} \\ \mathbf{0}^T & 1\end{bmatrix} \in \mathbb{R}^{4\times4} \;\middle|\; \mathbf{C}\in SO(3),\; \mathbf{r}\in\mathbb{R}^3\right\}. \tag{8.2}

虽然 $SO(3)$ 和 $SE(3)$ 不是向量空间，但它们是矩阵李群（matrix Lie groups）。群的四个公理（封闭性、结合性、单位元、可逆性）可以验证：

公理	$SO(3)$	$SE(3)$
封闭性	${\mathbf{C}}_1{\mathbf{C}}_2\in SO(3)$	${\mathbf{T}}_1{\mathbf{T}}_2\in SE(3)$
结合性	矩阵乘法结合律	矩阵乘法结合律
单位元	$1$	$1$
可逆性	${\mathbf{C}}^{-1}={\mathbf{C}}^T\in SO(3)$	${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{C}}^T& -{\mathbf{C}}^T\mathbf{r}\\ 0^T& 1\end{array}\right]\in SE(3)$

注意：$SO(3)$ 和 $SE(3)$ 都是非阿贝尔群（non-Abelian）——元素的复合顺序很重要，旋转不可交换。

---

English

$SO(3)$ satisfies the four group axioms under matrix multiplication: closure (${\mathbf{C}}_1{\mathbf{C}}_2\in SO(3)$), associativity, identity ($1$), and invertibility (${\mathbf{C}}^{-1}={\mathbf{C}}^T\in SO(3)$). Together with smoothness (we can do calculus on it), $SO(3)$ is a matrix Lie group. Similarly $SE(3)$ is a matrix Lie group with inverse given by ${\mathbf{T}}^{-1}=[{\mathbf{C}}^T,-{\mathbf{C}}^T\mathbf{r};0^T,1]$. Both are non-commutative (non-Abelian): order matters.

---

8.1.2 李代数 / Lie Algebras §

中文

每个矩阵李群都有一个关联的李代数（Lie algebra）——它是该群在单位元处的切空间，构成一个向量空间（！），因此可以进行线性代数运算。

$SO(3)$ 的李代数 $\mathfrak{so}(3)$：所有 $3\times 3$ 反对称矩阵构成的向量空间：

\mathfrak{so}(3) = \{\boldsymbol{\phi}^\wedge \in \mathbb{R}^{3\times3} \mid \boldsymbol{\phi} \in \mathbb{R}^3\}, \tag{8.3}

其中 ${\mathbf{\varphi }}^{\wedge }$ 是 $\mathbf{\varphi }$ 的反对称矩阵（与第七章定义相同）。李括号为 $[{\Phi }_1,{\Phi }_2]={\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1$。

$SE(3)$ 的李代数 $\mathfrak{se}(3)$：

\boldsymbol{\xi}^\wedge = \begin{bmatrix}\boldsymbol{\rho} \\ \boldsymbol{\phi}\end{bmatrix}^\wedge = \begin{bmatrix}\boldsymbol{\phi}^\wedge & \boldsymbol{\rho} \\ \mathbf{0}^T & 0\end{bmatrix} \in \mathbb{R}^{4\times4}, \quad \boldsymbol{\rho}, \boldsymbol{\phi} \in \mathbb{R}^3. \tag{8.4}

这是对 $(\cdot {)}^{\wedge }$ 算子的重载——将 ${\mathbb{R}}^6$ 向量变为 $4\times 4$ 矩阵。其逆操作记为 $(\cdot {)}^{\vee }$：$\mathbf{\xi }=({\mathbf{\xi }}^{\wedge }{)}^{\vee }$。

李代数的直觉：如果把李群想象成一个弯曲的曲面（流形），那么李代数就是在单位元处的切平面——它是”平的”（向量空间），因此可以进行普通的线性代数。通过指数映射，可以将切平面上的点”弯”回到流形上。

---

English

The Lie algebra $\mathfrak{so}(3)$ is the vector space of all $3\times 3$ skew-symmetric matrices. It is the tangent space of $SO(3)$ at the identity. Being a vector space, it supports all the usual linear algebra tools — that is exactly why we use it for perturbation analysis and statistics of rotations.

For $SE(3)$, the Lie algebra $\mathfrak{se}(3)$ consists of $4\times 4$ matrices of the form ${\mathbf{\xi }}^{\wedge }$ (8.4), where $\mathbf{\xi }=[{\mathbf{\rho }}^T,{\mathbf{\varphi }}^T{]}^T\in {\mathbb{R}}^6$. The pair $(\cdot {)}^{\wedge }$ / $(\cdot {)}^{\vee }$ converts between the 6-vector $\mathbf{\xi }$ and the matrix ${\mathbf{\xi }}^{\wedge }$.

---

8.1.3 指数映射 / Exponential Map §

中文

核心联系：李群与李代数通过指数映射（exponential map）相连：

\mathbf{C} = \exp(\boldsymbol{\phi}^\wedge) \in SO(3), \quad \boldsymbol{\phi} \in \mathbb{R}^3. \tag{8.5}

\mathbf{T} = \exp(\boldsymbol{\xi}^\wedge) \in SE(3), \quad \boldsymbol{\xi} \in \mathbb{R}^6. \tag{8.6}

$SO(3)$ 指数映射的闭合形式（Rodrigues 公式推导）：

设 $\mathbf{\varphi }=\varphi \mathbf{a}$（$\varphi =|\mathbf{\varphi }|$，$\mathbf{a}$ 为单位轴），利用幂级数展开和恒等式 ${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-1+\mathbf{a}{\mathbf{a}}^T$、${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge }$，可得：

\exp(\boldsymbol{\phi}^\wedge) = \cos\phi\,\mathbf{1} + (1-\cos\phi)\,\mathbf{a}\mathbf{a}^T + \sin\phi\,\mathbf{a}^\wedge. \tag{8.7}

这正是 Rodrigues 公式！

$SE(3)$ 指数映射：

\exp(\boldsymbol{\xi}^\wedge) = \begin{bmatrix}\mathbf{C} & \mathbf{J}\boldsymbol{\rho} \\ \mathbf{0}^T & 1\end{bmatrix} = \mathbf{T} \in SE(3), \tag{8.8}

其中 $\mathbf{C}=\exp ({\mathbf{\varphi }}^{\wedge })$，左雅可比矩阵（left Jacobian）$\mathbf{J}$ 为：

\mathbf{J} = \int_0^1 \mathbf{C}^\alpha\, d\alpha = \sum_{n=0}^\infty \frac{1}{(n+1)!}(\boldsymbol{\phi}^\wedge)^n = \frac{\sin\phi}{\phi}\mathbf{1} + \left(1 - \frac{\sin\phi}{\phi}\right)\mathbf{a}\mathbf{a}^T + \frac{1-\cos\phi}{\phi}\mathbf{a}^\wedge. \tag{8.9}

逆映射（对数映射）：

\boldsymbol{\phi} = \ln(\mathbf{C})^\vee, \quad \boldsymbol{\xi} = \ln(\mathbf{T})^\vee. \tag{8.10}

逆映射不唯一（满射非单射）：$\exp ((\mathbf{\varphi }+2\pi m\mathbf{a}{)}^{\wedge })=\mathbf{C}$。约定 $|\mathbf{\varphi }|<\pi$ 时每个 $\mathbf{C}$ 对应唯一的 $\mathbf{\varphi }$。

$SE(3)$ 直接展开公式：利用 $\mathfrak{se}(3)$ 的最小多项式 $({\mathbf{\xi }}^{\wedge }{)}^4+{\varphi }^2({\mathbf{\xi }}^{\wedge }{)}^2=0$：

\mathbf{T} = \mathbf{1} + \boldsymbol{\xi}^\wedge + \frac{1-\cos\phi}{\phi^2}(\boldsymbol{\xi}^\wedge)^2 + \frac{\phi-\sin\phi}{\phi^3}(\boldsymbol{\xi}^\wedge)^3. \tag{8.11}

关键性质：$\det (\exp ({\mathbf{\varphi }}^{\wedge }))=\exp (\mathrm{tr}({\mathbf{\varphi }}^{\wedge }))=\exp (0)=1$，因为反对称矩阵对角线为零。这说明指数映射自动保证行列式为 +1。

---

English

The exponential map $\exp ({\mathbf{\varphi }}^{\wedge })\in SO(3)$ maps any $\mathbf{\varphi }\in {\mathbb{R}}^3$ to a valid rotation matrix — it is surjective (every rotation can be generated) but not injective (adding $2\pi$ to the angle gives the same rotation). Restricting to $|\mathbf{\varphi }|<\pi$ restores injectivity.

For $SE(3)$, $\exp ({\mathbf{\xi }}^{\wedge })$ produces a $\mathbf{T}$ with rotation $\mathbf{C}=\exp ({\mathbf{\varphi }}^{\wedge })$ and translation $\mathbf{r}=\mathbf{J}\mathbf{\rho }$, where $\mathbf{J}$ (8.9) is the (left) Jacobian of $SO(3)$ — the “stretching factor” that converts the Lie algebra translation $\mathbf{\rho }$ to the Lie group translation $\mathbf{r}$.

---

8.1.4 伴随表示 / Adjoint §

中文

$SE(3)$ 的伴随映射（adjoint）将 $SE(3)$ 的元素表示为 ${\mathbb{R}}^6$ 上的线性变换：

\mathrm{Ad}(\mathbf{T})\,\mathbf{x}^\wedge = \mathbf{T}\,\mathbf{x}^\wedge\,\mathbf{T}^{-1} = (\mathcal{T}\mathbf{x})^\wedge, \tag{8.12}

其中 $6\times 6$ 矩阵 $\mathcal{T}$ 为：

\mathcal{T} = \mathrm{Ad}(\mathbf{T}) = \begin{bmatrix}\mathbf{C} & \mathbf{r}^\wedge\mathbf{C} \\ \mathbf{0} & \mathbf{C}\end{bmatrix}. \tag{8.13}

$\mathrm{Ad}(SE(3))$ 本身也是一个矩阵李群，具有相同的群公理。

伴随的作用：将”在一个坐标系中描述的速度/扰动”变换到另一个坐标系中。在概率部分（§8.3）中，当对位姿施加确定性变换时，协方差的变换恰好通过伴随矩阵完成：${\mathbf{\Sigma }}^{\prime }=\mathcal{T}\mathbf{\Sigma }{\mathcal{T}}^T$。

---

English

The adjoint $\mathcal{T}=\mathrm{Ad}(\mathbf{T})$ (8.13) is a $6\times 6$ matrix that linearly maps the Lie algebra vector $\mathbf{x}\in {\mathbb{R}}^6$ under conjugation by $\mathbf{T}$. It arises naturally when “transporting” uncertainty across coordinate frames: if ${\mathbf{T}}^{\prime }=\mathbf{RT}$ is a deterministically transformed pose, then the associated perturbation transforms as ${\mathbf{ϵ}}^{\prime }=\mathcal{R}\mathbf{ϵ}$, where $\mathcal{R}=\mathrm{Ad}(\mathbf{R})$.

---

8.1.5 Baker–Campbell–Hausdorff 公式与雅可比矩阵 / BCH Formula and Jacobians §

中文

问题：如何合并两个矩阵指数？对标量 $\exp (a)\exp (b)=\exp (a+b)$，但矩阵情形需要 BCH 公式：

\ln(\exp(\mathbf{A})\exp(\mathbf{B})) = \mathbf{A} + \mathbf{B} + \frac{1}{2}[\mathbf{A},\mathbf{B}] + \frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]] - \frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]] + \cdots \tag{8.14}

其中 $[\mathbf{A},\mathbf{B}]=\mathbf{AB}-\mathbf{BA}$ 是李括号。

当 $\mathbf{A}$ 或 $\mathbf{B}$ 中有一个很小时，BCH 公式简化为（以 $SO(3)$ 为例）：

\ln(\mathbf{C}_1\mathbf{C}_2)^\vee \approx \begin{cases} \mathbf{J}(\boldsymbol{\phi}_2)^{-1}\boldsymbol{\phi}_1 + \boldsymbol{\phi}_2 & \text{若 } \boldsymbol{\phi}_1 \text{ 小} \\ \boldsymbol{\phi}_1 + \mathbf{J}(-\boldsymbol{\phi}_1)^{-1}\boldsymbol{\phi}_2 & \text{若 } \boldsymbol{\phi}_2 \text{ 小} \end{cases} \tag{8.15}

其中 $\mathbf{J}={\mathbf{J}}_{\ell }$（左雅可比矩阵，见式 (8.9)）。类似地，对 $SE(3)$：

\ln(\mathbf{T}_1\mathbf{T}_2)^\vee \approx \begin{cases} \mathcal{J}(\boldsymbol{\xi}_2)^{-1}\boldsymbol{\xi}_1 + \boldsymbol{\xi}_2 & \text{若 } \boldsymbol{\xi}_1 \text{ 小} \\ \boldsymbol{\xi}_1 + \mathcal{J}(-\boldsymbol{\xi}_1)^{-1}\boldsymbol{\xi}_2 & \text{若 } \boldsymbol{\xi}_2 \text{ 小} \end{cases} \tag{8.16}

其中 $\mathcal{J}={\mathcal{J}}_{\ell }$ 是 $SE(3)$ 的（左）雅可比矩阵，具有分块结构：

\mathcal{J} = \int_0^1 \mathcal{T}^\alpha\,d\alpha = \begin{bmatrix}\mathbf{J} & \mathbf{Q} \\ \mathbf{0} & \mathbf{J}\end{bmatrix}, \tag{8.17}

其中 $\mathbf{Q}$ 是一个仅依赖于 $\mathbf{\xi }$ 的 $3\times 3$ 矩阵（见式 (8.91a)）。

直觉：BCH 公式是旋转/位姿的”加法法则”——将两个李代数元素的指数相乘，等价于在李代数中”加”这两个元素，但需要用雅可比矩阵修正（因为旋转不可交换）。当其中一个元素很小时，修正量是一阶近似。

---

English

The BCH formula (8.14) tells us how to compose two matrix exponentials. When one of the arguments is small, it simplifies to the approximate forms (8.15)–(8.16), involving the left Jacobian $\mathbf{J}$ of $SO(3)$ and $\mathcal{J}$ of $SE(3)$. These appear throughout:

• In §8.1.9 (perturbation calculus), to relate a perturbation in $\mathbf{\varphi }$ to a left-perturbation on $\mathbf{C}$
• In §8.3.3 (compounding uncertain poses), to propagate covariance through ${\mathbf{T}}_1{\mathbf{T}}_2$
• In Chapter 9 (pose estimation), as the linearized measurement Jacobian

---

8.1.6 插值 / Interpolation §

中文

普通线性插值 $(1-\alpha ){\mathbf{C}}_1+\alpha {\mathbf{C}}_2$ 不在 $SO(3)$ 中（违反封闭性）。正确的 $SO(3)$ 插值方案为：

\mathbf{C}(\alpha) = (\mathbf{C}_2\mathbf{C}_1^T)^\alpha\mathbf{C}_1, \quad \alpha \in [0,1], \tag{8.18}

其中 ${\mathbf{C}}_{21}^{\alpha }=\exp (\alpha {\mathbf{\varphi }}^{\wedge })\in SO(3)$（简单地缩放旋转角度）。物理意义：以恒定角速度 $\mathbf{\omega }=\mathbf{\varphi }/(t_2-t_1)$ 旋转，这正是泊松方程的解。

类似地，$SE(3)$ 插值：

\mathbf{T}(\alpha) = (\mathbf{T}_2\mathbf{T}_1^{-1})^\alpha\mathbf{T}_1. \tag{8.19}

扰动插值：若两端点有小扰动 $\delta {\mathbf{\varphi }}_1,\delta {\mathbf{\varphi }}_2$，中间点的扰动为：

\delta\boldsymbol{\varphi} = (\mathbf{1} - \mathbf{A}(\alpha,\boldsymbol{\phi}))\,\delta\boldsymbol{\phi}_1 + \mathbf{A}(\alpha,\boldsymbol{\phi})\,\delta\boldsymbol{\phi}_2, \tag{8.20}

其中 $\mathbf{A}(\alpha ,\mathbf{\varphi })=\alpha \mathbf{J}(\alpha \mathbf{\varphi })\mathbf{J}(\mathbf{\varphi }{)}^{-1}$（当 $\mathbf{\varphi }$ 很小时 $\mathbf{A}\approx \alpha 1$，退化为普通线性插值）。

---

English

Naive linear interpolation leaves $SO(3)$. The geodesic interpolation scheme (8.18)–(8.19) stays on the manifold by scaling the rotation angle at a constant rate — equivalent to constant angular velocity motion. The perturbed interpolation (8.20) shows how endpoint uncertainties propagate to an intermediate point, with $\mathbf{A}(\alpha ,\mathbf{\varphi })\approx \alpha 1$ for small $\mathbf{\varphi }$, recovering ordinary linear interpolation of perturbations.

---

8.1.7 齐次点算子 / Homogeneous Point Operators §

中文

为处理位姿与点的联合优化，定义 $4\times 1$ 齐次坐标向量 $\mathbf{p}=[{\mathbf{\epsilon }}^T,\eta {]}^T$ 上的两个算子：

\mathbf{p}^\odot = \begin{bmatrix}\eta\mathbf{1} & -\boldsymbol{\varepsilon}^\wedge \\ \mathbf{0}^T & \mathbf{0}^T\end{bmatrix} \in \mathbb{R}^{4\times6}, \qquad \mathbf{p}^\circledcirc = \begin{bmatrix}\mathbf{0} & \boldsymbol{\varepsilon} \\ -\boldsymbol{\varepsilon}^\wedge & \mathbf{0}\end{bmatrix} \in \mathbb{R}^{6\times4}. \tag{8.21}

关键恒等式：

\boldsymbol{\xi}^\wedge\mathbf{p} = \mathbf{p}^\odot\boldsymbol{\xi}, \quad \mathbf{p}^T\boldsymbol{\xi}^\wedge = \boldsymbol{\xi}^T\mathbf{p}^\circledcirc. \tag{8.22}

这将”对 $\mathbf{\xi }$ 的线性”从 $4\times 4$ 矩阵形式转化为 $4\times 6$ 矩阵形式，使得位姿雅可比矩阵的计算极为简洁。

---

English

The operators ${\mathbf{p}}^{\odot }$ and ${\mathbf{p}}^{⊚}$ (8.21) rewrite the action of the Lie algebra on a homogeneous point as a matrix-vector product. Identity (8.22) means that instead of differentiating through the $4\times 4$ matrix product ${\mathbf{\xi }}^{\wedge }\mathbf{p}$, we can directly read off the Jacobian as ${\mathbf{p}}^{\odot }$ — a crucial simplification in bundle adjustment and SLAM (Chapter 10).

---

8.1.8 优化与微积分 / Optimization and Calculus §

中文

扰动方案（左扰动）：在当前估计点 ${\mathbf{C}}_{\mathrm{op}}$ 附近，定义左扰动：

\mathbf{C} = \exp(\boldsymbol{\psi}^\wedge)\,\mathbf{C}_\mathrm{op}. \tag{8.23}

则旋转点 $\mathbf{Cv}$ 关于左扰动的雅可比矩阵（即左李导数）为：

\frac{\partial(\mathbf{C}\mathbf{v})}{\partial\boldsymbol{\psi}} = -(\mathbf{C}_\mathrm{op}\mathbf{v})^\wedge. \tag{8.24}

类似地，对位姿变换：

\mathbf{T} = \exp(\boldsymbol{\epsilon}^\wedge)\,\mathbf{T}_\mathrm{op}, \quad \frac{\partial(\mathbf{T}\mathbf{p})}{\partial\boldsymbol{\epsilon}} = (\mathbf{T}_\mathrm{op}\mathbf{p})^\odot. \tag{8.25}

高斯-牛顿迭代（$SE(3)$ 版本）：

给定代价函数 $J(\mathbf{T})=\frac{1}{2}{\sum }_m(u_m(\mathbf{T}{\mathbf{p}}_m){)}^2$：

1. 线性化：$u_m(\mathbf{T}{\mathbf{p}}_m)\approx \underset{{\beta }_m}{\underbrace{u_m({\mathbf{T}}_{\mathrm{op}}{\mathbf{p}}_m)}}+\underset{{\mathbf{\delta }}_m^T}{\underbrace{\frac{\partial u_m}{\partial \mathbf{x}}{|}_{{\mathbf{T}}_{\mathrm{op}}{\mathbf{p}}_m}({\mathbf{T}}_{\mathrm{op}}{\mathbf{p}}_m{)}^{\odot }}}\mathbf{ϵ}$
2. 求解最优扰动：$\left({\sum }_m{\mathbf{\delta }}_m{\mathbf{\delta }}_m^T\right){\mathbf{ϵ}}^{\star }=-{\sum }_m{\beta }_m{\mathbf{\delta }}_m$
3. 更新：${\mathbf{T}}_{\mathrm{op}}\leftarrow \exp ({\mathbf{ϵ}}^{\star \wedge }){\mathbf{T}}_{\mathrm{op}}$

三个关键性质：

1. 无奇点：始终将位姿存储为矩阵（${\mathbf{T}}_{\mathrm{op}}\in SE(3)$），避免了欧拉角的奇点问题
2. 无约束优化：每步求解的 $\mathbf{ϵ}$ 是普通向量，不受任何约束
3. 矩阵级操作：无需手动对三角函数求导，减少出错概率

这与黎曼流形上的优化等价：扰动方案对应黎曼梯度下降，指数映射作为收缩映射（retraction）将更新量投影回流形。

---

English

The left-perturbation scheme (8.23) is the practical method for Gauss-Newton on Lie groups. The Lie derivative (8.24)–(8.25) gives the Jacobian cleanly: for $SO(3)$, $\partial (\mathbf{Cv})/\partial \mathbf{\psi }=-({\mathbf{C}}_{\mathrm{op}}\mathbf{v}{)}^{\wedge }$; for $SE(3)$, $\partial (\mathbf{Tp})/\partial \mathbf{ϵ}=({\mathbf{T}}_{\mathrm{op}}\mathbf{p}{)}^{\odot }$. No trigonometric derivatives required — just the rotated/transformed point with the skew or $\odot$ operator applied.

The Gauss-Newton update (8.25) guarantees ${\mathbf{T}}_{\mathrm{op}}\in SE(3)$ at every iteration, solves an unconstrained $6\times 6$ linear system, and is equivalent to Riemannian gradient descent on the $SE(3)$ manifold.

---

8.2 运动学 / Kinematics §

8.2.1 旋转运动学 / Rotational Kinematics §

中文

Lie 群形式（泊松方程）：

\dot{\mathbf{C}} = \boldsymbol{\omega}^\wedge\mathbf{C}, \quad \boldsymbol{\omega}^\wedge = \dot{\mathbf{C}}\mathbf{C}^T. \tag{8.26}

Lie 代数形式（等价，无约束）：

\boldsymbol{\omega} = \mathbf{J}\dot{\boldsymbol{\phi}}, \quad \text{或} \quad \dot{\boldsymbol{\phi}} = \mathbf{J}^{-1}\boldsymbol{\omega}. \tag{8.27}

数值积分（恒定角速度假设）：

\mathbf{C}(t_2) = \exp(\boldsymbol{\omega}^\wedge\Delta t)\,\mathbf{C}(t_1) = \mathbf{C}_{21}\,\mathbf{C}(t_1), \tag{8.28}

其中 ${\mathbf{C}}_{21}=\exp ({\mathbf{\omega }}^{\wedge }\Delta t)\in SO(3)$——保证结果在 $SO(3)$ 中。

当角速度线性变化时，可使用Magnus 展开：

\mathbf{C}(t_2) \approx \exp\left(\left(\boldsymbol{\omega}_1\Delta t + \frac{1}{2}\boldsymbol{\alpha}_1\Delta t^2 + \frac{1}{12}\boldsymbol{\alpha}_1^\wedge\boldsymbol{\omega}_1\Delta t^3 + \cdots\right)^\wedge\right)\mathbf{C}(t_1). \tag{8.29}

线性化旋转运动学：在名义解 $\mathbf{C}(t)$ 附近做左扰动 ${\mathbf{C}}^{\prime }=\exp (\delta {\mathbf{\varphi }}^{\wedge })\mathbf{C}$，得到扰动运动学：

\delta\dot{\boldsymbol{\phi}} = \boldsymbol{\omega}^\wedge\delta\boldsymbol{\phi} + \delta\boldsymbol{\omega}. \tag{8.30}

此为线性时变（LTV）方程，状态转移矩阵：

\boldsymbol{\Phi}(t,s) = \mathbf{C}(t)\mathbf{C}(s)^T. \tag{8.31}

---

English

Poisson’s equation (8.26) — in Lie group form — is singularity-free but constrained ($\mathbf{C}{\mathbf{C}}^T=1$). The Lie algebra form (8.27) trades the constraint for potential singularities at $|\mathbf{\varphi }|=2\pi m$, but these are avoidable in practice.

Numerical integration using the piecewise-constant $\mathbf{\omega }$ scheme (8.28) guarantees the integrated rotation stays in $SO(3)$ at every step. The Magnus expansion (8.29) provides higher-order accuracy for smoothly varying $\mathbf{\omega }$.

The linearized kinematics (8.30) are the foundation for the process noise propagation in the EKF: given nominal trajectory $\mathbf{C}(t)$, the perturbation $\delta \mathbf{\varphi }$ evolves linearly, with transition matrix $\mathbf{\Phi }(t,s)=\mathbf{C}(t)\mathbf{C}(s{)}^T$ (8.31).

---

8.2.2 位姿运动学 / Pose Kinematics §

中文

Lie 群形式（无奇点）：

\dot{\mathbf{T}} = \boldsymbol{\varpi}^\wedge\mathbf{T}, \quad \boldsymbol{\varpi}^\wedge = \dot{\mathbf{T}}\mathbf{T}^{-1}, \tag{8.32}

其中广义速度 $\mathbf{\varpi }=[{\mathbf{\nu }}^T,{\mathbf{\omega }}^T{]}^T\in {\mathbb{R}}^6$（线速度 + 角速度）。

Lie 代数形式：

\boldsymbol{\varpi} = \mathcal{J}\dot{\boldsymbol{\xi}}, \quad \dot{\boldsymbol{\xi}} = \mathcal{J}^{-1}\boldsymbol{\varpi}. \tag{8.33}

数值积分（恒定广义速度）：

\mathbf{T}(t_2) = \exp(\boldsymbol{\varpi}^\wedge\Delta t)\,\mathbf{T}(t_1), \tag{8.34}

结果保证在 $SE(3)$ 中。

线性化位姿运动学：左扰动 ${\mathbf{T}}^{\prime }=\exp (\delta {\mathbf{\xi }}^{\wedge })\mathbf{T}$，扰动方程：

\delta\dot{\boldsymbol{\xi}} = \boldsymbol{\varpi}^\curlywedge\delta\boldsymbol{\xi} + \delta\boldsymbol{\varpi}, \quad \boldsymbol{\Phi}(t,s) = \mathcal{T}(t)\mathcal{T}(s)^{-1}. \tag{8.35}

其中 $\mathcal{T}=\mathrm{Ad}(\mathbf{T})$ 是 $6\times 6$ 伴随矩阵。

传输定理（Transport Theorem）：对静止点 ${\dot{\mathbf{p}}}_i=0$，在运动参考系中的坐标变化为：

\dot{\mathbf{p}}_v = \mathbf{p}_v^\odot\boldsymbol{\varpi}_v^{iv}. \tag{8.36}

---

English

Pose kinematics (8.32)–(8.34) parallels rotation kinematics. The $SE(3)$ constant-velocity integration (8.34) guarantees the result is in $SE(3)$, naturally combining the rotation and translation integrations. The $6\times 6$ transition matrix $\mathbf{\Phi }(t,s)=\mathcal{T}(t)\mathcal{T}(s{)}^{-1}$ drives both the nominal and perturbation dynamics — a key ingredient for deriving the process noise covariance in the IMU-aided pose estimators of Chapter 9.

---

8.3 概率与统计 / Probability and Statistics §

8.3.1 李群上的高斯随机变量 / Gaussian Random Variables on Lie Groups §

中文

问题：普通高斯分布 $\mathbf{x}=\mathbf{\mu }+\mathbf{ϵ}$（$\mathbf{ϵ}\sim \mathcal{N}(0,\mathbf{\Sigma })$）利用向量加法；但 $SO(3)$ 不支持向量加法。

解决方案（左扰动方案）：对 $SO(3)$ 上的随机变量 $\mathbf{C}$，定义：

\mathbf{C} = \exp(\boldsymbol{\epsilon}^\wedge)\bar{\mathbf{C}}, \quad \boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}). \tag{8.37}

其中 $\bar{\mathbf{C}}\in SO(3)$ 是”大的、无噪声的”均值旋转，$\mathbf{ϵ}\in {\mathbb{R}}^3$ 是”小的、有噪声的”扰动（普通向量！）。类似地，对 $SE(3)$：

\mathbf{T} = \exp(\boldsymbol{\epsilon}^\wedge)\bar{\mathbf{T}}, \quad \boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}),\quad \boldsymbol{\epsilon}\in\mathbb{R}^6. \tag{8.38}

验证均值：取 $\mathbf{M}=\bar{\mathbf{C}}$，可以证明 $\int \ln (\mathbf{C}{\bar{\mathbf{C}}}^T{)}^{\vee }p(\mathbf{ϵ})d\mathbf{ϵ}=E[\mathbf{ϵ}]=0$，说明 $\bar{\mathbf{C}}$ 确实是”均值旋转”。

协方差：$E[\mathbf{ϵ}{\mathbf{ϵ}}^T]=\mathbf{\Sigma }$（与普通向量高斯的协方差定义完全一致）。

核心思想（“两全其美”）：

• 大分量（均值 $\bar{\mathbf{C}}$）保存在李群中——无奇点、满足旋转约束
• 小分量（扰动 $\mathbf{ϵ}$）保存在李代数中——无约束、可做线性代数

由于 $\mathbf{ϵ}$ 小，它远离 $3\times 1$ 旋转参数化的奇点（$|\mathbf{ϵ}|=2\pi$ 附近）。

纯旋转映射下的变换：若 ${\mathbf{C}}^{\prime }=\mathbf{RC}$（$\mathbf{R}$ 确定），则：

\bar{\mathbf{C}}' = \mathbf{R}\bar{\mathbf{C}}, \quad \boldsymbol{\epsilon}' = \mathbf{R}\boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0}, \mathbf{R}\boldsymbol{\Sigma}\mathbf{R}^T). \tag{8.39}

对于 $SE(3)$：若 ${\mathbf{T}}^{\prime }=\mathbf{RT}$（$\mathbf{R}$ 确定），则：

\bar{\mathbf{T}}' = \mathbf{R}\bar{\mathbf{T}}, \quad \boldsymbol{\epsilon}' = \mathcal{R}\boldsymbol{\epsilon} \sim \mathcal{N}(\mathbf{0}, \mathcal{R}\boldsymbol{\Sigma}\mathcal{R}^T), \tag{8.40}

其中 $\mathcal{R}=\mathrm{Ad}(\mathbf{R})$。这些变换完全精确——无需线性化！

---

English

The key insight: define a rotation random variable as $\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}}$ where $\mathbf{ϵ}$ is an ordinary Gaussian 3-vector. The nominal rotation $\bar{\mathbf{C}}$ lives in $SO(3)$ (singularity-free), while the noise $\mathbf{ϵ}$ lives in ${\mathbb{R}}^3$ (unconstrained). Under a deterministic rotation $\mathbf{R}$, the new random variable transforms as (8.39): mean rotates by $\mathbf{R}$, covariance transforms by $\mathbf{R}\mathbf{\Sigma }{\mathbf{R}}^T$ — exactly, no approximation.

---

8.3.2 旋转向量上的不确定性 / Uncertainty on a Rotated Vector §

中文

设 $\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}}$，$\mathbf{ϵ}\sim \mathcal{N}(0,\mathbf{\Sigma })$，则旋转向量 $\mathbf{y}=\mathbf{Cx}$ 的期望（四阶近似）为：

E[\mathbf{y}] \approx \left(\mathbf{1} + \frac{1}{2}\left(-\mathrm{tr}(\boldsymbol{\Sigma})\mathbf{1} + \boldsymbol{\Sigma}\right) + O(\boldsymbol{\Sigma}^2)\right)\bar{\mathbf{C}}\mathbf{x}. \tag{8.41}

注意 $E[\mathbf{y}]\ne \bar{\mathbf{C}}\mathbf{x}$（有偏！）——这是旋转非线性引入的”Jensen 偏差”。

---

English

Even with zero-mean rotation noise, the rotated vector $E[\mathbf{Cx}]\ne \bar{\mathbf{C}}\mathbf{x}$ due to the nonlinearity of the exponential map — this is a second-order bias term proportional to $\mathrm{tr}(\mathbf{\Sigma })$. This is the same phenomenon seen in Chapter 6 (variational inference): MAP/linearized methods underestimate the true mean and overestimate confidence.

---

8.3.3 位姿复合中的协方差传播 / Covariance Propagation Under Pose Compounding §

中文

问题：给定两个含噪声的位姿 $\{{\bar{\mathbf{T}}}_1,{\mathbf{\Sigma }}_1\}$ 和 $\{{\bar{\mathbf{T}}}_2,{\mathbf{\Sigma }}_2\}$，求复合位姿 $\mathbf{T}={\mathbf{T}}_1{\mathbf{T}}_2$ 的均值和协方差。

一阶近似结果：

\bar{\mathbf{T}} = \bar{\mathbf{T}}_1\bar{\mathbf{T}}_2, \quad \boldsymbol{\Sigma} \approx \boldsymbol{\Sigma}_1 + \bar{\mathcal{T}}_1\boldsymbol{\Sigma}_2\bar{\mathcal{T}}_1^T, \tag{8.42}

其中 ${\bar{\mathcal{T}}}_1=\mathrm{Ad}({\bar{\mathbf{T}}}_1)$。

直觉：位姿复合的协方差传播类似于线性系统 $\mathbf{y}=\mathbf{Ax}$ 的协方差传播 ${\mathbf{\Sigma }}_y=\mathbf{A}{\mathbf{\Sigma }}_x{\mathbf{A}}^T$，但这里”$\mathbf{A}$“是伴随矩阵 ${\bar{\mathcal{T}}}_1$——它将第二个位姿的协方差”旋转”到第一个位姿的坐标系中。

这是位姿图优化（Pose-Graph Optimization）中传播不确定性的基础公式（第九章）。

---

English

Pose compounding (8.42) is the $SE(3)$ analogue of linear error propagation ${\mathbf{\Sigma }}_y=\mathbf{A}{\mathbf{\Sigma }}_x{\mathbf{A}}^T$, with the adjoint ${\bar{\mathcal{T}}}_1=\mathrm{Ad}({\bar{\mathbf{T}}}_1)$ playing the role of the Jacobian. This formula is used directly in Chapter 9 for pose-graph optimization: when chaining many pose estimates together, uncertainties accumulate according to (8.42).

---

8.4 本章小结 / Chapter Summary §

中文

工具	$SO(3)$	$SE(3)$
群	${\mathbf{C}}^{-1}={\mathbf{C}}^T$	${\mathbf{T}}^{-1}=[{\mathbf{C}}^T,-{\mathbf{C}}^T\mathbf{r};0^T,1]$
李代数	${\mathbf{\varphi }}^{\wedge }\in \mathfrak{so}(3)$（$3\times 3$ 反对称）	${\mathbf{\xi }}^{\wedge }\in \mathfrak{se}(3)$（$4\times 4$）
指数映射	$\mathbf{C}=\exp ({\mathbf{\varphi }}^{\wedge })$（Rodrigues）	$\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge })$（含 $\mathbf{J}$）
左雅可比	$\mathbf{J}={\int }_0^1{\mathbf{C}}^{\alpha }d\alpha$（$3\times 3$）	$\mathcal{J}={\int }_0^1{\mathcal{T}}^{\alpha }d\alpha$（$6\times 6$）
BCH 近似	$\ln ({\mathbf{C}}_1{\mathbf{C}}_2{)}^{\vee }\approx {\mathbf{J}}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2$	$\ln ({\mathbf{T}}_1{\mathbf{T}}_2{)}^{\vee }\approx {\mathcal{J}}^{-1}{\mathbf{\xi }}_1+{\mathbf{\xi }}_2$
左李导数	$\partial (\mathbf{Cv})/\partial \mathbf{\psi }=-(\mathbf{Cv}{)}^{\wedge }$	$\partial (\mathbf{Tp})/\partial \mathbf{ϵ}=(\mathbf{Tp}{)}^{\odot }$
高斯随机变量	$\mathbf{C}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{C}}$，$\mathbf{ϵ}\in {\mathbb{R}}^3$	$\mathbf{T}=\exp ({\mathbf{ϵ}}^{\wedge })\bar{\mathbf{T}}$，$\mathbf{ϵ}\in {\mathbb{R}}^6$
协方差传播	${\mathbf{\Sigma }}^{\prime }=\mathbf{R}\mathbf{\Sigma }{\mathbf{R}}^T$	${\mathbf{\Sigma }}^{\prime }=\mathcal{R}\mathbf{\Sigma }{\mathcal{R}}^T$

本章统一了第七章的几何工具与第一部分的估计算法：

• EKF：用左李导数（8.24）–（8.25）代替普通雅可比矩阵；用 $\exp ({\mathbf{ϵ}}^{\wedge })$ 替代 $\mathbf{\mu }+\mathbf{ϵ}$ 作为状态更新
• MAP/GN：用左扰动方案（8.23）在 $SO(3)/SE(3)$ 上迭代；避免奇点、无约束
• 概率：用 $(\bar{\mathbf{C}},\mathbf{\Sigma })$ 或 $(\bar{\mathbf{T}},\mathbf{\Sigma })$ 描述旋转/位姿的不确定性；协方差通过伴随矩阵传播

---

English

This chapter has built the complete mathematical toolkit for doing state estimation with rotation and pose states. Key takeaways:

1. $SO(3)$ and $SE(3)$ are Lie groups — not vector spaces, but they support multiplication, inversion, and smooth calculus via the exponential map.
2. The exponential map bridges Lie algebra (vector space — suitable for linear algebra) and Lie group (manifold — correct for rotations/poses). The surjective-only property means perturbations in ${\mathbb{R}}^3$ / ${\mathbb{R}}^6$ always produce valid rotations/poses.
3. Left-perturbation Gauss-Newton keeps estimates on the manifold at every iteration, requires no constraint handling, and produces clean Jacobians via Lie derivatives.
4. Gaussian uncertainty on $SO(3)/SE(3)$ is defined via the left-perturbation scheme: mean in Lie group, covariance in Lie algebra. Deterministic frame changes transform covariance exactly via the adjoint.

These tools will be applied directly in Chapters 9–11 to point-cloud alignment, pose-graph optimization, bundle adjustment, SLAM, and inertial navigation.

---

下一章将第一部分的估计算法（MAP、EKF、平滑器）与本章的李群工具结合，求解三维位姿估计的经典问题。/ The next chapter combines the estimation algorithms from Part I with the Lie group tools from this chapter to solve classic 3D pose estimation problems.