ch07_3d_geometry

第七章　三维几何基础 §

Chapter 7   A Primer on Three-Dimensional Geometry §

---

本章目标 / Chapter Goals

本书第一部分假设状态是普通向量。但对真实机器人来说，**姿态（位置 + 朝向）**是必不可少的状态量，而三维旋转不是向量——它不满足向量加法的封闭性。本章系统介绍三维几何工具：旋转矩阵、欧拉角、四元数、位姿变换矩阵，以及常见传感器的观测模型。这些工具是第三部分应用的直接基础。

Part I assumed states were ordinary vectors. Real robots need pose (position + orientation) as a state, yet 3D rotations are not vectors — they are not closed under ordinary addition. This chapter builds the geometric toolkit: rotation matrices, Euler angles, quaternions, pose transforms, and standard sensor models. These tools feed directly into the applications of Part III.

---

7.1 参考坐标系与基本运算 / Reference Frames and Basic Operations §

中文

坐标系 §

一个参考坐标系（reference frame）${\underline{\mathcal{F}}}_a$ 由一个原点和三个单位正交基向量确定。向量 ${\mathbf{r}}_a^{pq}$ 表示”从点 $q$ 到点 $p$ 的向量，在 $a$ 坐标系下的坐标表示”。这个记法看起来繁琐，但它在多帧问题中能自动追踪”哪个坐标系”，避免混淆。

常用坐标系：

• 惯性系（inertial frame） ${\underline{\mathcal{F}}}_i$：固定在地面或宇宙空间中的参考系
• 车体系（vehicle frame） ${\underline{\mathcal{F}}}_v$：固定在机器人上，随机器人运动
• 传感器系（sensor frame） ${\underline{\mathcal{F}}}_s$：固定在某个传感器上

向量运算 §

设 $\mathbf{u},\mathbf{v}\in {\mathbb{R}}^3$。

点积（内积）： $\mathbf{u}\cdot \mathbf{v}={\mathbf{u}}^T\mathbf{v}=u_1{v}_1+u_2{v}_2+u_3{v}_3.$

叉积（外积）： $\mathbf{u}\times \mathbf{v}={\mathbf{u}}^{\wedge }\mathbf{v},$

其中 ${\mathbf{u}}^{\wedge }$（或 ${\mathbf{u}}^{\times }$）是 $\mathbf{u}$ 对应的反对称矩阵（skew-symmetric matrix）：

\mathbf{u}^\wedge = \begin{bmatrix} 0 & -u_3 & u_2 \\ u_3 & 0 & -u_1 \\ -u_2 & u_1 & 0 \end{bmatrix}. \tag{7.1}

反对称矩阵满足 $({\mathbf{u}}^{\wedge }{)}^T=-{\mathbf{u}}^{\wedge }$，且有恒等式：

\mathbf{u}^\wedge \mathbf{v} = -\mathbf{v}^\wedge \mathbf{u}. \tag{7.2}

直觉：把叉积写成矩阵乘法 ${\mathbf{u}}^{\wedge }\mathbf{v}$ 的好处是：对旋转等线性运算可以直接操作矩阵，而不必记忆叉积的具体计算规则。

---

English

Reference Frames §

A reference frame ${\underline{\mathcal{F}}}_a$ is defined by an origin and three orthonormal basis vectors. The vector ${\mathbf{r}}_a^{pq}$ denotes the vector from point $q$ to point $p$, expressed in frame $a$. This notation tracks both the geometric quantity (which pair of points) and the coordinate frame (which basis is used), avoiding the common source of confusion where the same physical vector takes different numerical values in different frames.

Common frames:

• Inertial ${\underline{\mathcal{F}}}_i$: fixed to Earth or to an inertial reference
• Vehicle ${\underline{\mathcal{F}}}_v$: fixed to the robot body
• Sensor ${\underline{\mathcal{F}}}_s$: fixed to a particular sensor

Cross Product as Matrix Multiplication §

The cross product $\mathbf{u}\times \mathbf{v}$ can always be written as ${\mathbf{u}}^{\wedge }\mathbf{v}$, where ${\mathbf{u}}^{\wedge }$ is the skew-symmetric matrix in (7.1). Key identities:

$({\mathbf{u}}^{\wedge }{)}^T=-{\mathbf{u}}^{\wedge },{\mathbf{u}}^{\wedge }\mathbf{v}=-{\mathbf{v}}^{\wedge }\mathbf{u}.$

This matrix form allows cross-product terms to be manipulated algebraically like any matrix product — essential for the Jacobians that appear throughout state estimation.

---

7.2 旋转 / Rotations §

7.2.1 旋转矩阵 / Rotation Matrix §

中文

设坐标系 ${\underline{\mathcal{F}}}_2$ 相对于 ${\underline{\mathcal{F}}}_1$ 发生了旋转。同一向量在两个坐标系中的坐标满足：

\mathbf{v}_2 = \mathbf{C}_{21}\, \mathbf{v}_1, \tag{7.3}

其中 ${\mathbf{C}}_{21}\in {\mathbb{R}}^{3\times 3}$ 称为旋转矩阵（direction cosine matrix, DCM）。其下标”21”表示”从坐标系 1 变换到坐标系 2”。

性质（可由定义直接推导）：

性质	公式
正交性	${\mathbf{C}}^T\mathbf{C}=1$
逆等于转置	${\mathbf{C}}^{-1}={\mathbf{C}}^T$
行列式为 +1	$\det \mathbf{C}=+1$
复合（链式法则）	${\mathbf{C}}_{31}={\mathbf{C}}_{32}{\mathbf{C}}_{21}$
旋转叉积	$(\mathbf{Cv}{)}^{\wedge }=\mathbf{C}{\mathbf{v}}^{\wedge }{\mathbf{C}}^T$

满足上述性质的矩阵构成特殊正交群 $SO(3)$（Special Orthogonal Group）。

主轴旋转矩阵（Principal Rotations）——绕 $x,y,z$ 轴旋转 $\theta$ 角：

\mathbf{C}_1(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta & \cos\theta \end{bmatrix}, \tag{7.4}

\mathbf{C}_2(\theta) = \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{bmatrix}, \tag{7.5}

\mathbf{C}_3(\theta) = \begin{bmatrix} \cos\theta & \sin\theta & 0 \\ -\sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}. \tag{7.6}

---

English

A rotation matrix ${\mathbf{C}}_{21}$ transforms the coordinate representation of a vector when the frame changes from ${\underline{\mathcal{F}}}_1$ to ${\underline{\mathcal{F}}}_2$: ${\mathbf{v}}_2={\mathbf{C}}_{21}{\mathbf{v}}_1$. It belongs to $SO(3)$, defined as:

$SO(3)\triangleq \{\mathbf{C}\in {\mathbb{R}}^{3\times 3}\mid {\mathbf{C}}^T\mathbf{C}=1,\det \mathbf{C}=+1\}.$

The inverse is the transpose, ${\mathbf{C}}^{-1}={\mathbf{C}}^T$, and frames compose by matrix multiplication: ${\mathbf{C}}_{31}={\mathbf{C}}_{32}{\mathbf{C}}_{21}$.

The three principal rotations ${\mathbf{C}}_1,{\mathbf{C}}_2,{\mathbf{C}}_3$ (7.4)–(7.6) each represent a rotation about one coordinate axis and are the building blocks for all parameterizations.

---

7.2.2 旋转的参数化 / Parameterizing Rotations §

中文

旋转矩阵是 $3\times 3$ 的正交矩阵，但 $SO(3)$ 的自由度只有 3（需满足 6 个约束：${\mathbf{C}}^T\mathbf{C}=1$，共 6 个独立方程）。因此可以用 3 个参数来描述任意旋转，下面介绍几种常用方案。

轴角与 Rodrigues 公式 / Axis-Angle and Rodrigues’ Formula §

任意旋转等价于”绕某单位轴 $\mathbf{a}$ 旋转角度 $\varphi$“。对应的旋转矩阵由 Rodrigues 公式给出：

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

验证：将 $\mathbf{v}$ 分解为平行于 $\mathbf{a}$ 的分量和垂直分量，可以直接验证此公式将 $\mathbf{v}$ 绕 $\mathbf{a}$ 旋转 $\varphi$ 角。

欧拉角 / Euler Angles §

把任意旋转分解为三次主轴旋转的复合，所用的三个角度称为欧拉角（Euler angles）。常见方案：

• 3-1-3（ZXZ）：航空动力学、陀螺仪常用
• 1-2-3（XYZ）= 横滚-俯仰-偏航（roll-pitch-yaw）：机器人、导航常用

以 1-2-3 为例： \mathbf{C}(\psi,\theta,\phi) = \mathbf{C}_1(\phi)\,\mathbf{C}_2(\theta)\,\mathbf{C}_3(\psi). \tag{7.8}

奇点问题（Gimbal Lock）：所有三参数旋转表示在某些角度处会退化，无法唯一确定旋转。对于 1-2-3 方案，当俯仰角 $\theta =\pm 90°$ 时发生奇点，此时横滚和偏航无法区分。这就是不能直接用欧拉角做状态向量加法的原因。

欧拉参数（单位四元数）/ Euler Parameters (Unit Quaternions) §

用四个实数 $(\eta ,\mathbf{\epsilon })$ 表示旋转：

\eta = \cos\frac{\phi}{2}, \quad \boldsymbol{\varepsilon} = \mathbf{a}\sin\frac{\phi}{2}, \quad \eta^2 + \boldsymbol{\varepsilon}^T\boldsymbol{\varepsilon} = 1. \tag{7.9}

这就是单位四元数，满足单位长度约束。旋转矩阵与四元数的关系：

\mathbf{C}(\eta, \boldsymbol{\varepsilon}) = (2\eta^2 - 1)\mathbf{1} - 2\eta\,\boldsymbol{\varepsilon}^\wedge + 2\boldsymbol{\varepsilon}\boldsymbol{\varepsilon}^T. \tag{7.10}

四元数的复合（对应旋转矩阵相乘 ${\mathbf{C}}_{32}={\mathbf{C}}_{31}{\mathbf{C}}_{12}$）通过复合算子实现：

\mathbf{q}_{32}^+ = \mathbf{q}_{31} \odot \mathbf{q}_{12}, \tag{7.11}

其中 $\mathbf{q}=[\eta {\mathbf{\epsilon }}^T{]}^T$，复合运算可写成矩阵乘法：

\mathbf{q}_{ba}^+ = \mathbf{q}_b^\oplus \mathbf{q}_a = \mathbf{q}_b^+ \mathbf{q}_a, \tag{7.12}

\mathbf{q}^+ = \begin{bmatrix} \eta & -\boldsymbol{\varepsilon}^T \\ \boldsymbol{\varepsilon} & \eta\mathbf{1} + \boldsymbol{\varepsilon}^\wedge \end{bmatrix}, \quad \mathbf{q}^\oplus = \begin{bmatrix} \eta & -\boldsymbol{\varepsilon}^T \\ \boldsymbol{\varepsilon} & \eta\mathbf{1} - \boldsymbol{\varepsilon}^\wedge \end{bmatrix}. \tag{7.13}

四元数的逆（对应逆旋转）： \mathbf{q}^{-1} = \begin{bmatrix} \eta \\ -\boldsymbol{\varepsilon} \end{bmatrix}. \tag{7.14}

注意：四元数表示是双值的——$\mathbf{q}$ 与 $-\mathbf{q}$ 表示同一旋转。在状态估计中需要处理这一歧义性（通常约定 $\eta \ge 0$）。

Gibbs 向量 / Gibbs Vector (Rodrigues Parameters) §

\mathbf{g} = \mathbf{a}\tan\frac{\phi}{2} = \frac{\boldsymbol{\varepsilon}}{\eta}. \tag{7.15}

Gibbs 向量将旋转压缩为 3 个参数（无约束），但在 $\varphi =\pi$（180° 旋转）处发生奇点。适用于小旋转或局部线性化。

---

English

Axis-Angle / Rodrigues’ Formula §

Any rotation is equivalent to a rotation by angle $\varphi$ about a unit axis $\mathbf{a}$. The rotation matrix is given by Rodrigues’ formula (7.7). The three principal rotations are special cases with $\mathbf{a}$ aligned with a coordinate axis.

Euler Angles §

Three successive principal-axis rotations, e.g., the 1-2-3 (roll-pitch-yaw) sequence $\mathbf{C}={\mathbf{C}}_1(\varphi ){\mathbf{C}}_2(\theta ){\mathbf{C}}_3(\psi )$. All three-parameter representations have singularities (gimbal lock) where the parameterization degenerates — for 1-2-3 this occurs at $\theta =\pm 90°$.

Unit Quaternions (Euler Parameters) §

A four-vector $\mathbf{q}=[\eta ,{\mathbf{\epsilon }}^T{]}^T$ with ${\eta }^2+\Vert \mathbf{\epsilon }{\Vert }^2=1$, encoding a rotation by $\varphi$ about axis $\mathbf{a}$ via (7.9). Composition uses the matrix operators ${\mathbf{q}}^{+}$ and ${\mathbf{q}}^{\oplus }$ in (7.13). The representation is double-valued: $\mathbf{q}$ and $-\mathbf{q}$ describe the same rotation.

Gibbs Vector §

$\mathbf{g}=\mathbf{\epsilon }/\eta$, a minimal unconstrained 3-vector representation, singular at $\varphi =\pi$. Useful for local linearization and small-angle approximations.

---

7.2.3 旋转运动学 / Rotational Kinematics §

中文

当坐标系随时间变化时，我们需要描述旋转矩阵随时间的演化。

泊松方程（Poisson’s Equation）：设 ${\mathbf{C}}_{21}(t)$ 是从坐标系 1 到坐标系 2 的旋转矩阵，随时间变化。则：

\dot{\mathbf{C}}_{21} = -\boldsymbol{\omega}_2^{21\wedge}\,\mathbf{C}_{21}, \tag{7.16}

其中 ${\mathbf{\omega }}_2^{21}$ 是坐标系 2 相对于坐标系 1 的角速度，在坐标系 2 中表示。

推导直觉：若 $\mathbf{C}(t)\mathbf{C}(t{)}^T=1$ 对所有 $t$ 成立，对时间求导得 $\dot{\mathbf{C}}{\mathbf{C}}^T+\mathbf{C}{\dot{\mathbf{C}}}^T=0$，即 $\dot{\mathbf{C}}{\mathbf{C}}^T$ 是反对称矩阵，故可写为 $\dot{\mathbf{C}}{\mathbf{C}}^T=-{\mathbf{\omega }}^{\wedge }$，得到 $\dot{\mathbf{C}}=-{\mathbf{\omega }}^{\wedge }\mathbf{C}$。

无穷小旋转：对小角度变化 $\delta \mathbf{\varphi }$，

\mathbf{C}(\delta\boldsymbol{\phi}) \approx \mathbf{1} - \delta\boldsymbol{\phi}^\wedge, \tag{7.17}

即旋转矩阵在单位矩阵附近的一阶展开。

欧拉角运动学：若旋转矩阵由欧拉角 $\mathbf{\theta }=[{\theta }_1,{\theta }_2,{\theta }_3{]}^T$ 参数化，则角速度与欧拉角时间导数的关系为：

\boldsymbol{\omega}_2^{21} = \mathbf{S}(\theta_2, \theta_3)\,\dot{\boldsymbol{\theta}}, \tag{7.18}

其中 $\mathbf{S}({\theta }_2,{\theta }_3)$ 是一个 $3\times 3$ 的矩阵，依赖中间两个欧拉角。在奇点处 $\mathbf{S}$ 不可逆，这正是欧拉角奇点的来源。

---

English

Poisson’s equation (7.16) governs how ${\mathbf{C}}_{21}(t)$ evolves when the frame rotates at angular velocity ${\mathbf{\omega }}_2^{21}$. This is the continuous-time kinematic equation for rotations — the analogue of $\dot{\mathbf{x}}=f(\mathbf{x})$ but constrained to $SO(3)$.

The small-angle approximation (7.17), $\mathbf{C}(\delta \mathbf{\varphi })\approx 1-\delta {\mathbf{\varphi }}^{\wedge }$, is the linearization of $SO(3)$ about the identity and is fundamental to EKF-style perturbation analysis.

Euler angle kinematics (7.18) relate $\dot{\mathbf{\theta }}$ to $\mathbf{\omega }$. The matrix $\mathbf{S}({\theta }_2,{\theta }_3)$ becomes singular at gimbal-lock angles, confirming that Euler angles are not suitable for unrestricted integration.

---

7.2.4 扰动旋转与雅可比矩阵 / Perturbing Rotations and Jacobians §

中文

在状态估计中，我们经常需要对旋转函数求导。设旋转矩阵 $\mathbf{C}(\mathbf{\theta })$ 作用在向量 $\mathbf{v}$ 上，则关键雅可比矩阵为：

\frac{\partial(\mathbf{C}(\boldsymbol{\theta})\,\mathbf{v})}{\partial \boldsymbol{\theta}} = (\mathbf{C}(\boldsymbol{\theta})\,\mathbf{v})^\wedge\,\mathbf{S}(\theta_2, \theta_3). \tag{7.19}

对于扰动旋转，考虑在当前旋转 $\mathbf{C}(\bar{\mathbf{\theta }})$ 附近的微小扰动 $\delta \mathbf{\theta }$：

\mathbf{C}(\bar{\boldsymbol{\theta}} + \delta\boldsymbol{\theta}) \approx (\mathbf{1} - \delta\boldsymbol{\phi}^\wedge)\,\mathbf{C}(\bar{\boldsymbol{\theta}}), \tag{7.20}

其中扰动角 $\delta \mathbf{\varphi }$ 与欧拉角增量的关系为： \delta\boldsymbol{\phi} = \mathbf{S}(\bar{\theta}_2, \bar{\theta}_3)\,\delta\boldsymbol{\theta}. \tag{7.21}

关键思想：(7.20) 的含义是，对当前旋转施加一个小旋转 $(1-\delta {\mathbf{\varphi }}^{\wedge })$，等价于在欧拉角上加上 $\delta \mathbf{\theta }$（在一阶近似意义下）。这是后面 EKF 和 MAP 优化中线性化旋转的基础。

---

English

Equation (7.19) is the Jacobian of a rotated vector with respect to Euler-angle parameters. Equation (7.20) is the left-perturbation model: a small change in Euler angles corresponds to pre-multiplying the rotation matrix by a near-identity rotation. This factored perturbation form is used extensively in the EKF (Ch. 4) when the state includes orientation: instead of adding a perturbation directly to $\mathbf{C}$ (which is not a vector space operation), we pre-multiply by $(1-\delta {\mathbf{\varphi }}^{\wedge })$.

---

7.3 位姿 / Poses §

中文

在三维空间中，机器人的**位姿（pose）**包含位置和朝向两部分：

$\text{pose of vehicle }v\text{ in frame }i:\left\{{\mathbf{r}}_i^{vi},{\mathbf{C}}_{iv}\right\},$

其中 ${\mathbf{r}}_i^{vi}$ 是车体原点在惯性系中的位置向量，${\mathbf{C}}_{iv}$ 是从惯性系到车体系的旋转矩阵。

齐次变换矩阵 / Transformation Matrix §

将位置和旋转合并为一个 $4\times 4$ 矩阵，称为变换矩阵（transformation matrix）：

\mathbf{T}_{iv} = \begin{bmatrix} \mathbf{C}_{iv} & \mathbf{r}_i^{vi} \\ \mathbf{0}^T & 1 \end{bmatrix} \in SE(3), \tag{7.22}

其中 $SE(3)$（特殊欧氏群）是所有有效位姿变换的集合。

齐次坐标：将三维点 $\mathbf{p}$ 扩展为四维向量 $\underline{\mathbf{p}}=[{\mathbf{p}}^T,1{]}^T$，则坐标系间的点变换为：

\underline{\mathbf{p}}_i = \mathbf{T}_{iv}\,\underline{\mathbf{p}}_v. \tag{7.23}

逆变换： \mathbf{T}_{iv}^{-1} = \mathbf{T}_{vi} = \begin{bmatrix} \mathbf{C}_{iv}^T & -\mathbf{C}_{iv}^T\mathbf{r}_i^{vi} \\ \mathbf{0}^T & 1 \end{bmatrix}. \tag{7.24}

链式复合（对应多帧间的变换）： \mathbf{T}_{iv} = \mathbf{T}_{ia}\,\mathbf{T}_{ab}\,\mathbf{T}_{bv}. \tag{7.25}

直觉：${\mathbf{T}}_{iv}$ 中下标的顺序遵循”消去法”——$iv$ 的 $i$ 和 $ia$ 的 $i$ 匹配消去，剩下的顺序正确拼接，类似于分数化简。这让复合规则易于记忆和验证。

位姿运动学 / Pose Kinematics §

位姿矩阵随时间的演化：

\dot{\mathbf{T}}_{iv} = \mathbf{T}_{iv}\begin{bmatrix} \boldsymbol{\omega}_v^{vi\wedge} & \mathbf{v}_v^{vi} \\ \mathbf{0}^T & 0 \end{bmatrix}, \tag{7.26}

其中 ${\mathbf{\omega }}_v^{vi}$ 是车体的角速度（在车体系中表示），${\mathbf{v}}_v^{vi}={\mathbf{C}}_{iv}{\dot{\mathbf{r}}}_i^{vi}$ 是车体的线速度（在车体系中表示）。

独轮车模型（Unicycle Model，2D 特例）：约束在平面内运动，自由度退化为 $(x,y,\theta )$：

\dot{x} = v\cos\theta, \quad \dot{y} = v\sin\theta, \quad \dot{\theta} = \omega, \tag{7.27}

其中 $v$ 是线速度，$\omega$ 是角速度。这是移动机器人中最常用的运动模型。

Frenet–Serret 坐标系 §

沿曲线运动时，可以在曲线上定义一个随运动点移动的自然坐标系（Frenet–Serret frame），由切向量、法向量和副法向量构成。曲线的曲率 $\kappa$ 和挠率 $\tau$ 描述曲线的弯曲程度。位姿矩阵沿曲线的演化为：

\dot{\mathbf{T}}_{iv} = \mathbf{T}_{iv}\begin{bmatrix} 0 & -v\kappa & 0 & v \\ v\kappa & 0 & -v\tau & 0 \\ 0 & v\tau & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix}, \tag{7.28}

其中 $v$ 是沿曲线的速率。

---

English

The transformation matrix ${\mathbf{T}}_{iv}\in SE(3)$ (7.22) compactly encodes both position ${\mathbf{r}}_i^{vi}$ and rotation ${\mathbf{C}}_{iv}$ in a single $4\times 4$ matrix. Composition and inversion follow standard matrix rules ((7.24)–(7.25)). The subscript convention is mnemonic: intermediate indices cancel like fractions.

Pose kinematics (7.26) is the $SE(3)$ analogue of Poisson’s equation, governing how the full pose evolves under translational and angular velocity. The planar unicycle model (7.27) is the standard differential drive robot model and a key test case for nonlinear estimation in 2D.

The Frenet–Serret frame (7.28) describes how a body moves along an arbitrary space curve and is used in continuous-time trajectory estimation (Chapter 11).

---

7.4 传感器模型 / Sensor Models §

中文

第一部分的估计算法需要测量方程 $\mathbf{y}=\mathbf{g}(\mathbf{x})+\mathbf{n}$。本节推导三类常见三维传感器的测量函数 $\mathbf{g}(\cdot )$。

---

7.4.1 透视相机 / Perspective Camera §

中文

归一化图像坐标：设点 $P$ 在相机坐标系中的坐标为 $\mathbf{\rho }=[x,y,z{]}^T$（$z>0$ 表示在相机前方）。透视投影给出归一化图像坐标：

x_n = \frac{x}{z}, \quad y_n = \frac{y}{z}. \tag{7.29}

直觉：相机就像一个”小孔成像”装置——所有沿同一射线的点都映射到同一个图像点。归一化坐标对应焦距为 1、光轴交点在原点的假想相机。

内参矩阵（Intrinsic Parameter Matrix）：将归一化坐标映射到实际像素坐标 $(u,v)$：

\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \underbrace{\begin{bmatrix} f_u & 0 & c_u \\ 0 & f_v & c_v \\ 0 & 0 & 1 \end{bmatrix}}_{\mathbf{K}} \begin{bmatrix} x_n \\ y_n \\ 1 \end{bmatrix}, \tag{7.30}

其中 $f_u,f_v$ 是水平/垂直方向的焦距（像素为单位），$(c_u,c_v)$ 是主点偏移。

完整相机模型：

\begin{bmatrix} u \\ v \end{bmatrix} = \mathbf{s}(\boldsymbol{\rho}) = \mathbf{P}\mathbf{K}\frac{1}{z}\boldsymbol{\rho}, \tag{7.31}

其中 $\mathbf{P}=[1_{2\times 2}0]$ 是去掉最后一行的投影矩阵。单目相机丢失深度信息——从 $(u,v)$ 无法恢复 $z$。

本质矩阵（Essential Matrix）：设同一点 $P$ 从两个相机位姿 $a,b$ 分别观测到归一化图像坐标 ${\mathbf{p}}_a,{\mathbf{p}}_b$，则：

\mathbf{p}_a^T\,\mathbf{E}_{ab}\,\mathbf{p}_b = 0, \tag{7.32}

其中本质矩阵： \mathbf{E}_{ab} = \mathbf{C}_{ba}^T\,\mathbf{r}_b^{ab\wedge}. \tag{7.33}

基本矩阵（Fundamental Matrix）：若使用实际像素坐标 ${\mathbf{q}}_i={\mathbf{K}}_i{\mathbf{p}}_i$，对应约束：

\mathbf{q}_a^T\,\mathbf{F}_{ab}\,\mathbf{q}_b = 0, \quad \mathbf{F}_{ab} = \mathbf{K}_a^{-T}\mathbf{E}_{ab}\mathbf{K}_b^{-1}. \tag{7.34}

对极几何（Epipolar Geometry）：在一幅图像中观测到点 ${\mathbf{q}}_a$，则在另一幅图像中对应点 ${\mathbf{q}}_b$ 必须位于对极线上（由 ${\mathbf{F}}_{ab}{\mathbf{q}}_b=0$ 定义的直线）。这将特征匹配从全图搜索缩减为一维搜索。

单应矩阵（Homography）：若点 $P$ 位于已知平面上，则两幅图像中的齐次像素坐标满足：

\mathbf{q}_b = \mathbf{K}_b\,\mathbf{H}_{ba}\,\mathbf{K}_a^{-1}\,\mathbf{q}_a, \tag{7.35}

其中单应矩阵： \mathbf{H}_{ba} = \frac{z_a}{z_b}\,\mathbf{C}_{ba}\left(\mathbf{1} + \frac{1}{d_a}\mathbf{r}_a^{ba}\mathbf{n}_a^T\right). \tag{7.36}

$\{d_a,{\mathbf{n}}_a\}$ 是平面参数（距相机 $a$ 的距离和法向量）。当纯旋转时 ${\mathbf{r}}_a^{ba}=0$，单应矩阵退化为旋转矩阵本身。

---

English

The perspective camera (pinhole model) maps a 3D point $\mathbf{\rho }=[x,y,z{]}^T$ to pixel coordinates via (7.31). A single camera loses depth — the measurement function is not injective. The essential matrix ${\mathbf{E}}_{ab}$ (7.33) captures the geometric relationship between two views: any corresponding pair of normalized image points must satisfy the epipolar constraint ${\mathbf{p}}_a^T{\mathbf{E}}_{ab}{\mathbf{p}}_b=0$, restricting feature-matching to a 1D epipolar line. The fundamental matrix ${\mathbf{F}}_{ab}$ (7.34) is the same constraint expressed in pixel coordinates. The homography ${\mathbf{H}}_{ba}$ (7.35)–(7.36) applies when the scene is planar, enabling full pose estimation from a single image.

---

7.4.2 双目相机 / Stereo Camera §

中文

双目相机由两个刚性连接的相机组成，基线（baseline）为 $b$（沿 $x$ 轴方向）。以左相机为传感器坐标系原点，视差（disparity）定义为：

d = u_\ell - u_r = \frac{f_u b}{z}. \tag{7.37}

视差与深度成反比：越近的点视差越大。双目模型将三个未知数 $(x,y,z)$ 映射到三个观测量 $(u_{\ell },v_{\ell },d)$：

\begin{bmatrix} u_\ell \\ v_\ell \\ d \end{bmatrix} = \mathbf{s}(\boldsymbol{\rho}) = \begin{bmatrix} f_u & 0 & c_u & 0 \\ 0 & f_v & c_v & 0 \\ 0 & 0 & 0 & f_u b \end{bmatrix} \frac{1}{z} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}. \tag{7.38}

与单目相机不同，双目模型是可逆的（三个方程，三个未知数），可以从观测量直接恢复三维坐标：

z = \frac{f_u b}{d}, \quad x = \frac{(u_\ell - c_u)z}{f_u}, \quad y = \frac{(v_\ell - c_v)z}{f_v}. \tag{7.39}

以中点为原点的模型（Midpoint Model）：将传感器坐标系放在两相机中点，左右相机模型对称：

\begin{bmatrix} u_\ell \\ v_\ell \\ u_r \\ v_r \end{bmatrix} = \mathbf{M} \frac{1}{z} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}, \quad \mathbf{M} = \begin{bmatrix} f_u & 0 & c_u & f_u\frac{b}{2} \\ 0 & f_v & c_v & 0 \\ f_u & 0 & c_u & -f_u\frac{b}{2} \\ 0 & f_v & c_v & 0 \end{bmatrix}. \tag{7.40}

由基本矩阵约束可以证明，水平基线双目相机的对极线是水平的，即 $v_r=v_{\ell }$——左右图像中同一点的垂直坐标必须相等，这将匹配搜索限制在水平方向。

---

English

A stereo camera recovers depth via the disparity $d=u_{\ell }-u_r=f_{u}b/z$ (7.37). The left-camera stereo model (7.38) maps $(x,y,z)$ bijectively to $(u_{\ell },v_{\ell },d)$, allowing direct 3D reconstruction via (7.39). The fundamental matrix constraint for a horizontal stereo baseline reduces to $v_r=v_{\ell }$, confining matching searches to horizontal epipolar lines — a key computational benefit.

---

7.4.3 距离-方位-俯仰传感器 / Range-Azimuth-Elevation Sensor §

中文

激光雷达（lidar）等传感器以球坐标（spherical coordinates）观测点 $P$。设点坐标为 $\mathbf{\rho }=[x,y,z{]}^T$，测量方程为：

\begin{bmatrix} r \\ \alpha \\ \epsilon \end{bmatrix} = \mathbf{s}(\boldsymbol{\rho}) = \begin{bmatrix} \sqrt{x^2+y^2+z^2} \\ \tan^{-1}(y/x) \\ \sin^{-1}\!\left(z/\sqrt{x^2+y^2+z^2}\right) \end{bmatrix}, \tag{7.41}

其中 $r$ 是距离，$\alpha$ 是方位角，$ϵ$ 是俯仰角。

二维简化（距离-方位模型）：当点在 $xy$ 平面内（$z=0,ϵ=0$）：

\begin{bmatrix} r \\ \alpha \end{bmatrix} = \mathbf{s}(\boldsymbol{\rho}) = \begin{bmatrix} \sqrt{x^2+y^2} \\ \tan^{-1}(y/x) \end{bmatrix}, \tag{7.42}

这是移动机器人中最常用的激光雷达/声呐模型。

---

English

The RAE model (7.41) describes lidar (and ultrasonic) sensors that measure a point in spherical coordinates — range $r$, azimuth $\alpha$, elevation $ϵ$. The 2D range-bearing simplification (7.42) is the standard sensor model for planar mobile robot localization (used throughout Chapter 4 examples).

---

7.4.4 惯性测量单元 / Inertial Measurement Unit (IMU) §

中文

IMU 包含三轴加速度计和三轴陀螺仪，固定在传感器坐标系 ${\underline{\mathcal{F}}}_s$ 中（可能与车体系 ${\underline{\mathcal{F}}}_v$ 不重合，标定参数为 ${\mathbf{r}}_v^{sv},{\mathbf{C}}_{sv}$）。

陀螺仪模型：陀螺仪测量车体在惯性系中的角速度，在传感器系中表示：

\boldsymbol{\omega} = \mathbf{C}_{sv}\,\boldsymbol{\omega}_v^{vi}. \tag{7.43}

由于传感器系与车体系刚性连接（${\dot{\mathbf{C}}}_{sv}=0$），陀螺模型比较简单。

加速度计模型：加速度计本质上是测量比力（specific force）——惯性加速度减去重力：

\mathbf{a} = \mathbf{C}_{si}\left(\ddot{\mathbf{r}}_i^{si} - \mathbf{g}_i\right), \tag{7.44}

其中 ${\mathbf{g}}_i$ 是重力加速度向量（在惯性系中）。

关键直觉：

• 自由下落时：${\ddot{\mathbf{r}}}_i^{si}={\mathbf{g}}_i$，加速度计输出 $\mathbf{a}=0$——完全没有测量到重力！
• 静止时：${\ddot{\mathbf{r}}}_i^{si}=0$，加速度计输出 $\mathbf{a}=-{\mathbf{C}}_{si}{\mathbf{g}}_i$——测量的是负的重力（在传感器系中）。

这是加速度计最反直觉的特性：它测量的不是绝对加速度，而是与自由落体的偏差。

考虑传感器系相对车体系的偏置（lever arm effect）：

\ddot{\mathbf{r}}_i^{si} = \ddot{\mathbf{r}}_i^{vi} + \mathbf{C}_{vi}^T\left(\dot{\boldsymbol{\omega}}_v^{vi\wedge}\,\mathbf{r}_v^{sv} + \boldsymbol{\omega}_v^{vi\wedge}\boldsymbol{\omega}_v^{vi\wedge}\,\mathbf{r}_v^{sv}\right). \tag{7.45}

完整 IMU 模型（陀螺 + 加速度计联合）：

\begin{bmatrix} \mathbf{a} \\ \boldsymbol{\omega} \end{bmatrix} = \mathbf{s}\!\left(\mathbf{r}_i^{vi}, \mathbf{C}_{vi}, \boldsymbol{\omega}_v^{vi}, \ddot{\mathbf{r}}_i^{vi}, \dot{\boldsymbol{\omega}}_v^{vi}\right) = \begin{bmatrix} \mathbf{C}_{sv}\!\left(\mathbf{C}_{vi}\!\left(\ddot{\mathbf{r}}_i^{vi} - \mathbf{g}_i\right) + \dot{\boldsymbol{\omega}}_v^{vi\wedge}\mathbf{r}_v^{sv} + \boldsymbol{\omega}_v^{vi\wedge}\boldsymbol{\omega}_v^{vi\wedge}\mathbf{r}_v^{sv}\right) \\ \mathbf{C}_{sv}\,\boldsymbol{\omega}_v^{vi} \end{bmatrix}. \tag{7.46}

若传感器-车体偏置 ${\mathbf{r}}_v^{sv}$ 足够小，可忽略最后两项。

---

English

An IMU integrates gyroscopes and accelerometers into a single package. The gyro model (7.43) is straightforward — it measures the body angular rate ${\mathbf{\omega }}_v^{vi}$ rotated into the sensor frame. The accelerometer model (7.44) is more subtle: it measures specific force (inertial acceleration minus gravity), not absolute acceleration. In free fall the accelerometer reads zero; at rest it reads $-{\mathbf{C}}_{si}{\mathbf{g}}_i$.

When the sensor is offset from the vehicle center, centripetal and Euler acceleration terms appear (7.45), giving the full combined model (7.46). If the lever arm ${\mathbf{r}}_v^{sv}$ is small, these terms can often be neglected. The IMU model (7.46) serves as the process model in inertial navigation systems: it provides high-rate predictions of vehicle motion between lower-rate exteroceptive measurements (camera, GPS).

---

7.5 本章小结 / Chapter Summary §

中文

概念	核心结论
旋转矩阵 $\mathbf{C}\in SO(3)$	${\mathbf{C}}^{-1}={\mathbf{C}}^T$，$\det \mathbf{C}=+1$；链式乘法；旋转叉积 $(\mathbf{Cv}{)}^{\wedge }=\mathbf{C}{\mathbf{v}}^{\wedge }{\mathbf{C}}^T$
旋转的参数化	欧拉角（有奇点）、四元数（无奇点但双值）、Gibbs向量（局部用）；所有3参数表示都有奇点
泊松方程	${\dot{\mathbf{C}}}_{21}=-{\mathbf{\omega }}_2^{21\wedge }{\mathbf{C}}_{21}$；旋转的连续时间运动学
位姿矩阵 $\mathbf{T}\in SE(3)$	将旋转和平移合并为 $4\times 4$ 矩阵；逆、复合均为标准矩阵运算
透视相机	丢失深度；本质矩阵 / 基本矩阵约束；单应矩阵（平面假设）
双目相机	通过视差 $d=f_{u}b/z$ 恢复深度
RAE 传感器	球坐标测量；2D 简化为距离-方位
IMU	比力模型；陀螺测角速度；加速度计测比力

旋转不是向量：$SO(3)$ 不是线性空间——旋转不可交换（顺序重要），不能直接相加。这正是第八章需要引入李群工具的原因：李代数提供了一种在旋转”附近”做线性代数的方法，从而将第一部分的估计算法扩展到旋转状态。

---

English

Three-dimensional rotations challenge all the estimation techniques in Part I because $SO(3)$ is not a vector space: rotations do not commute, and adding two rotation matrices does not yield a rotation matrix. The tools developed here — rotation matrices, quaternions, transformation matrices, sensor models — are the prerequisites for Part III applications. Chapter 8 will equip us with matrix Lie groups to do calculus on $SO(3)$ and $SE(3)$, enabling MAP, EKF, and GVI estimators to handle rotation states rigorously.

---

下一章将深入研究 $SO(3)$ 和 $SE(3)$ 的代数结构，引入李代数和扰动微积分。/ The next chapter examines the algebraic structure of $SO(3)$ and $SE(3)$, introducing Lie algebras and perturbation calculus.