SLAM 三维空间刚体运动（2）

前言

上一册把 SLAM 拆成了前端、后端、回环和建图四块，并反复用到一个词：位姿。机器人在哪、朝哪个方向，就是位姿。书的第 2 讲把它写进了运动方程

x_k = f(x_{k-1}, u_k, w_k)

里的那个 x_k（第 k 时刻的状态）。可是直到现在，我们都没有说清 x_k 到底是个什么东西：它是几个数？这些数怎么排？为什么不能像位置那样用三个坐标简单地加减？

这一章就是为了回答这个问题。它会告诉你：位姿由平移 和旋转两部分组成，平移是简单的，旋转是麻烦的；描述旋转有四种常用语言------旋转矩阵、旋转向量、欧拉角、四元数------每一种都有它的长处和致命缺陷；SLAM 系统在不同环节会切换使用这些语言，而理解切换的理由，比背下任何一个公式都重要。

这一章几乎没有"SLAM 专属"的内容，它是经典的三维几何。但请不要因此轻视它。后面所有的章节------李群、相机模型、视觉里程计、后端优化------每一行公式里都躺着一个旋转矩阵或一个四元数。这一章没吃透，后面每一步都会多花三倍的时间。

如果你做过 IMU 姿态解算（例如 Mahony、Madgwick 这类互补滤波），那么四元数对你不陌生。本章会在相应位置把你已经熟悉的东西和 SLAM 的用法接起来，并解释一些你在 AHRS 里可能"照着做但没想过为什么"的细节，比如四元数为什么每一步都要归一化、为什么 q 和 −q 是同一个旋转。

1.1 三个容易混淆的概念

我们先把三个词分清楚：点 、向量 、坐标。

点是空间中的一个位置，它本身没有长度、没有方向。两个点相减得到一个向量，向量是"从一个点指向另一个点的箭头"，它有长度和方向，但没有固定的起点------把一支箭头平移到任何地方，它还是同一个向量。

坐标是另一回事。向量是一个几何对象，它独立于任何坐标系存在；但要用数字把它写下来，就必须先选一组基。在三维空间里选定三个线性无关的向量 (e₁, e₂, e₃) 作为基，任何向量 a 都可以唯一地写成

a = a₁·e₁ + a₂·e₂ + a₃·e₃

这三个数 (a₁, a₂, a₃) 才是坐标。换一组基，同一个向量的坐标就变了。这件事看起来是常识，却是整章的根基：所谓"坐标变换"，变的是坐标，不变的是向量本身。相机看到一个点，激光测到一个点，机器人估计自己的位置------这些都是同一个物理世界里的同一个点，只不过被写在了不同的坐标系下。SLAM 的大量工作就是在不同坐标系之间来回搬运坐标。

书中约定：如果不特别说明，坐标系都是右手系，三个基向量两两正交且长度为 1，即标准正交基。右手系的判别方法是：右手四指从 x 轴转向 y 轴，大拇指指向 z 轴。

1.2 内积与外积

两个三维向量 a、b 之间有两种乘法。

内积（点积）的结果是一个标量：

a · b = aᵀ b = a₁b₁ + a₂b₂ + a₃b₃ = |a| |b| cos⟨a, b⟩

它度量两个向量"有多平行"：同向时最大，垂直时为零，反向时最小。几何上它是一个向量在另一个向量方向上的投影长度乘以后者的长度。

外积（叉积）的结果是一个向量：

a × b = [ a₂b₃ − a₃b₂ ,

a₃b₁ − a₁b₃ ,

a₁b₂ − a₂b₁ ]

它的方向垂直于 a 和 b 张成的平面（按右手定则），长度等于 |a| |b| sin⟨a, b⟩，也就是两向量围成的平行四边形的面积。外积度量两个向量"有多垂直"，平行时为零。

1.3 一个重要的记号：反对称矩阵

请仔细看上面外积的三个分量。它们对 b 是线性的------每个分量都是 b₁、b₂、b₃ 的线性组合，系数由 a 决定。既然是线性的，就一定能写成"一个矩阵乘以 b"的形式：

┌ 0 −a₃ a₂ ┐ ┌ b₁ ┐

a × b = │ a₃ 0 −a₁ │ │ b₂ │ ≜ a^ b

└ −a₂ a₁ 0 ┘ └ b₃ ┘

这个由 a 构造出来的 3×3 矩阵记作 a^（读作"a hat"或"a 尖"），它满足 (a^)ᵀ = −a^，因此叫反对称矩阵。反过来，任何一个 3×3 反对称矩阵只有三个独立的非零元素，可以唯一地还原出一个三维向量，这个逆操作记作 ∨（读作"vee"）：(a^)∨ = a。

于是 ^ 和 ∨ 建立了三维向量与三维反对称矩阵之间的一一对应。

请把这个记号刻在脑子里。它现在看起来只是外积的一个花哨写法，但到了第 4 讲，你会发现旋转的"微小变化"恰恰就是一个反对称矩阵，整个李代数 so(3) 就是全体三维反对称矩阵的集合，而 ^ 和 ∨ 就是在"三维向量"和"李代数元素"之间来回翻译的工具。书在这一讲就把 ^ 引进来，不是为了讲外积，是在为下一讲埋线。

还有两条外积的性质后面会反复用到：

• 反交换：a × b = −(b × a)，对应到矩阵就是 a^ b = −b^ a；

• 自己叉自己为零：a × a = 0，即 a^ a = 0。

第二章　坐标系之间的欧氏变换

2.1 刚体运动只有两种

现在考虑一个真实的场景。机器人在房间里移动，房间固定不动，我们在房间里钉一个坐标系，叫世界坐标系 （world frame），记作 W。机器人身上装着一台相机，相机也带着一个坐标系，叫相机坐标系（camera frame），记作 C。相机坐标系会随着机器人一起运动。

现在房间里有一个点 p，比如墙角。它在世界系下有一个坐标 p_W，在相机系下也有一个坐标 p_C。两个坐标描述的是同一个墙角。问题是：这两组数字之间是什么关系？

相机是一个刚体 ：它在运动过程中，自身各部分之间的距离和夹角都不变。一个刚体从一个位置运动到另一个位置，只可能经历两种变化的组合：旋转 （朝向变了）和平移 （位置变了）。这种"旋转加平移、不拉伸不扭曲"的变换叫欧氏变换。所以相机系与世界系之间的关系，就由一个欧氏变换完全确定。

2.2 旋转矩阵是怎么来的

先只考虑旋转。设两个坐标系原点重合，只是朝向不同。世界系的基是 (e₁, e₂, e₃)，相机系的基是 (e₁′, e₂′, e₃′)。同一个向量 a 在两组基下的坐标分别是 (a₁, a₂, a₃) 和 (a₁′, a₂′, a₃′)。由于向量本身不变，有

a₁·e₁ + a₂·e₂ + a₃·e₃ = a₁′·e₁′ + a₂′·e₂′ + a₃′·e₃′

两边同时用 e₁、e₂、e₃ 分别做内积。左边因为基是标准正交的，eᵢ · eⱼ 在 i = j 时为 1、否则为 0，所以左边分别只剩下 a₁、a₂、a₃。右边则得到

┌ a₁ ┐ ┌ e₁·e₁′ e₁·e₂′ e₁·e₃′ ┐ ┌ a₁′ ┐

│ a₂ │ = │ e₂·e₁′ e₂·e₂′ e₂·e₃′ │ │ a₂′ │ ≜ R a′

└ a₃ ┘ └ e₃·e₁′ e₃·e₂′ e₃·e₃′ ┘ └ a₃′ ┘

中间这个 3×3 矩阵就是旋转矩阵 R。它的每个元素是两组基向量两两之间的内积；由于基向量长度都是 1，这些内积就是两两夹角的余弦，所以旋转矩阵也叫方向余弦矩阵（Direction Cosine Matrix，DCM）。这个名字你在 IMU 资料里一定见过。

这个推导告诉我们一件事：旋转矩阵的三列，分别是新基 e₁′、e₂′、e₃′ 在旧基下的坐标。想象你要把相机系"画"在世界系里：相机的 x 轴指向哪、y 轴指向哪、z 轴指向哪------这三个方向向量并排一放，就是 R。这是理解旋转矩阵最直观的方式。

这个推导告诉我们一件事：旋转矩阵的三列，分别是新基 e₁′、e₂′、e₃′ 在旧基下的坐标。想象你要把相机系"画"在世界系里：相机的 x 轴指向哪、y 轴指向哪、z 轴指向哪------这三个方向向量并排一放，就是 R。这是理解旋转矩阵最直观的方式。

2.3 旋转矩阵的性质与特殊正交群

旋转矩阵不是任意的 3×3 矩阵。由于它是两组标准正交基之间的内积表，它一定满足：

1. 正交：Rᵀ R = I，等价地 R⁻¹ = Rᵀ；

2. 行列式为 1：det R = 1。

正交性的意思是它的三列（或三行）互相垂直且长度为 1------这正是"新基也是标准正交基"的翻译。行列式为 +1 排除了镜像：行列式为 −1 的正交矩阵也保持长度和夹角，但会把右手系变成左手系，那不是任何刚体能够通过物理运动做到的。

满足这两条的所有 3×3 矩阵组成一个集合，叫特殊正交群（Special Orthogonal Group）：

SO(3) = { R ∈ ℝ³ˣ³ | R Rᵀ = I, det R = 1 }

"特殊"指行列式为 1，"正交"指 Rᵀ R = I，"群"这个词先放着，下一讲再解释。同样地，SO(n) 表示 n 维空间的旋转，SLAM 里只用 SO(2)（平面）和 SO(3)。

正交性带来一个非常实用的结论：旋转的逆就是转置。要把坐标从相机系变回世界系，不用求逆矩阵，转置一下就行：

a′ = R⁻¹ a = Rᵀ a

Rᵀ 描述的是一个相反的旋转。

2.4 加上平移

现在让两个坐标系的原点也不重合。相机系原点在世界系下的坐标记作 t。一个点在相机系下的坐标是 a′，要得到它在世界系下的坐标 a，先把它旋转到与世界系同向，再加上原点的偏移：

a = R a′ + t

这就是完整的欧氏变换：一个旋转矩阵 R 加一个平移向量 t，一共 3 + 3 = 6 个自由度。

这里要特别小心下标的含义。书中后文采用这样的记法：R₁₂ 表示"把坐标系 2 的坐标变换到坐标系 1 的旋转"，即

a₁ = R₁₂ a₂ + t₁₂

读的时候从右往左读："从 2 到 1"。这个约定在全书保持一致，也和 Eigen、Sophus、g2o 的习惯一致。t₁₂ 则是坐标系 2 的原点在坐标系 1 下的坐标，也就是"从 1 指向 2 的向量，在 1 系下表示"。

一个常见的错误是以为 t₂₁ = −t₁₂。这是不对的。t₂₁ 是坐标系 1 的原点在坐标系 2 下的坐标，它等于

t₂₁ = −R₂₁ t₁₂ = −R₁₂ᵀ t₁₂

中间多了一次旋转。方向相反没错，但还要换到另一个坐标系下表示。写代码时这个地方错得最多。

2.5 变换矩阵与齐次坐标

欧氏变换 a = R a′ + t 有一个不方便的地方：它不是线性的（多了一个加法），连续做两次就会很啰嗦。设从 a 到 b 再到 c：

b = R₁ a + t₁

c = R₂ b + t₂ = R₂ (R₁ a + t₁) + t₂

变换十次之后这个式子就没法看了。解决办法是一个数学技巧：齐次坐标。把三维点 a 后面补一个 1，变成四维向量 ã = (a, 1)，然后把 R 和 t 拼成一个 4×4 矩阵：

c̃ = T₂ T₁ ã

齐次坐标的"齐次"是说：把四维向量整体乘以一个非零常数 k，(k·a₁, k·a₂, k·a₃, k) 和 (a₁, a₂, a₃, 1) 表示的是同一个三维点。要还原三维坐标，把前三维除以第四维即可。这个性质在第 5 讲的相机投影里会再次出现，到时候第四维不一定是 1。

全体变换矩阵也构成一个群，叫特殊欧氏群：

SE(3) = { T = R t ; 0ᵀ 1 ∈ ℝ⁴ˣ⁴ | R ∈ SO(3), t ∈ ℝ³ }

2.6 SLAM 里最常见的两个变换矩阵

在视觉 SLAM 中，最常用的是相机系和世界系之间的变换。书里约定：

• T_cw：把世界系坐标变到相机系，p_c = T_cw p_w。它回答的问题是"这个路标在相机眼里的位置"，投影到图像时用它。

• T_wc：把相机系坐标变到世界系，p_w = T_wc p_c。它的平移部分 t_wc 就是相机在世界中的位置，画轨迹时用它。

两者互为逆：T_wc = T_cw⁻¹。很多初学者画出来的轨迹是错的，原因就是拿 T_cw 的平移部分当相机位置画了------那个量是世界原点在相机系下的坐标，不是相机的位置。

从现在起，当书里说"估计相机位姿"，指的就是估计 T_cw 或 T_wc 中的一个；二者知其一即知其二。

第三章　实践：Eigen 库

3.1 为什么是 Eigen

Eigen 是一个开源的 C++ 线性代数模板库，全部由头文件组成，不需要编译链接任何 .so 或 .a，装好后在 CMake 里加一句 include_directories("/usr/include/eigen3") 就能用。Sophus、g2o、Ceres、ORB-SLAM、VINS 等几乎所有 SLAM 相关的 C++ 库都把 Eigen 作为底层矩阵类型，所以它不是"可选工具"，而是必须掌握的基础设施。

3.2 Eigen 的核心类型

Eigen 里所有矩阵和向量都是同一个模板类 Eigen::Matrix<Scalar, Rows, Cols> 的特化。三个模板参数依次是元素类型、行数、列数。例如：

Eigen::Matrix<float, 2, 3> matrix_23; // 2 行 3 列的 float 矩阵

Eigen::Matrix<double, 3, 1> v_3d; // 3 维 double 列向量

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic; // 动态大小

为了少打字，Eigen 预定义了许多别名：Eigen::Vector3d 就是 Matrix<double, 3, 1>，Eigen::Matrix3d 就是 Matrix<double, 3, 3>，Eigen::MatrixXd 是动态大小的 double 矩阵。命名规律是：Vector/Matrix + 维度 + 类型后缀（d 双精度、f 单精度、i 整数，X 表示动态）。

操作	写法
初始化为零 / 单位 / 随机	Matrix3d::Zero()、Matrix3d::Identity()、Matrix3d::Random()
逗号初始化	matrix_23 << 1, 2, 3, 4, 5, 6;
元素访问	matrix_23(i, j)
转置、求和、迹、行列式、逆	.transpose()、.sum()、.trace()、.determinant()、.inverse()
数乘	10 * matrix_33
特征值分解（对称阵）	Eigen::SelfAdjointEigenSolver<Matrix3d> es(A); 然后 es.eigenvalues()、es.eigenvectors()
解线性方程 A x = b	A.inverse() * b（慢）、A.colPivHouseholderQr().solve(b)（QR 分解）、A.ldlt().solve(b)（正定阵用 Cholesky）

3.3 三个必须知道的坑

第一，Eigen 不做隐式类型转换。 Matrix<float, 2, 3> 和 Matrix<double, 3, 1> 不能直接相乘，哪怕维度是对的。必须显式写 matrix_23.cast<double>() * v_3d。这是编译期错误，提示信息会很长很吓人，看到 YOU_MIXED_DIFFERENT_NUMERIC_TYPES 之类的字样就是这个问题。

第二，维度必须匹配。 对固定大小的矩阵，维度不匹配是编译期错误（YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES）；对动态大小的矩阵，则是运行期断言失败。这其实是好事------越早报错越好。

第三，固定大小的 Eigen 对象放进 STL 容器要加对齐分配器。 Eigen 为了用 SSE/AVX 指令加速，要求固定大小的向量化类型（Vector4d、Matrix4d、Quaterniond、Isometry3d 等）在内存里 16 字节对齐。C++17 之前，std::vector<Eigen::Isometry3d> 的默认分配器不保证这一点，会在某些平台上随机崩溃。正确写法是

std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;

书里 plotTrajectory 例子的轨迹容器就是这么写的。如果你用的编译器支持 C++17 并且开启了对齐 new，这个问题会自动消失，但为了可移植性，照书上写最保险。

3.4 关于效率

书里给了一个实验：解一个 50×50 的线性方程组，用求逆的方法和用 QR 分解的方法比较耗时。结论是在数值计算中几乎永远不要显式求逆，用分解法解方程更快也更稳定。这个结论在后面的优化章节极为重要：高斯牛顿法每一步都要解一个增量方程 H Δx = g，这个 H 在 BA 里可能是上万维的，求逆是不可想象的，必须用 Cholesky 分解加稀疏结构。第 9 讲会回到这里。

另一条经验：对固定大小的小矩阵（不超过 4×4），用 Matrix3d 这类固定大小类型比 MatrixXd 快很多，因为编译器可以把循环完全展开、不需要堆内存分配。SLAM 里到处都是 3×3 和 4×4，所以能用固定大小就用固定大小。

第四章　旋转向量与欧拉角

4.1 旋转矩阵的两个缺点

旋转矩阵能够完整地描述旋转，而且复合、求逆都很方便。但它有两个明显的缺点。

第一，冗余。SO(3) 的旋转只有 3 个自由度（绕三个轴的转角），但旋转矩阵用了 9 个数。多出来的 6 个数被 Rᵀ R = I（6 个独立约束）锁住了。同理，变换矩阵用 16 个数表达 6 个自由度。

第二，带约束。正因为有约束，旋转矩阵不能被当作普通的 9 维向量来处理。你不能对它做"加一点扰动"这种事：R + ΔR 一般不再是正交矩阵，不再是一个合法的旋转。这给优化带来了巨大的麻烦------优化算法的基本动作就是"沿着梯度方向加一个增量"，而这里偏偏不能加。

这两个缺点推动我们去寻找更紧凑、无约束的表示。本章介绍两种：旋转向量和欧拉角。下一讲的李代数则是对这个问题最终的、系统性的回答。

4.2 旋转向量：用一根轴和一个角描述旋转

欧拉旋转定理说：三维空间中任意一个旋转，都可以看作绕某一个固定轴转过某一个角度。这个结论不显然，但你可以这样说服自己：旋转矩阵作为实正交矩阵，一定有一个特征值为 1 的特征向量 n（满足 R n = n），这个向量在旋转下不动，它就是转轴。

于是我们可以用一个单位向量 n（轴）和一个标量 θ（角）来描述旋转。更紧凑一些，把二者合成一个向量

φ = θ n

它的方向是转轴，长度是转角。这个三维向量叫旋转向量 （也叫轴角，Axis-Angle）。3 个数，3 个自由度，没有约束------至少表面上是这样。对应地，一个变换可以用一个旋转向量加一个平移向量共 6 个数表示。

如果你做过 IMU 积分，你其实已经用过它：陀螺仪一个采样周期内测到的角速度 ω 乘以 Δt，就是一个小旋转向量 ω·Δt------方向是瞬时转轴，长度是这一小段时间转过的角度。

4.3 罗德里格斯公式：从旋转向量到旋转矩阵

旋转向量和旋转矩阵描述的是同一个旋转，它们之间一定有换算关系。从旋转向量到旋转矩阵的公式叫罗德里格斯公式（Rodrigues' Formula）：

R = cos θ · I + (1 − cos θ) · n nᵀ + sin θ · n^

其中 n nᵀ 是 n 与自身的外积（3×3 矩阵，不是叉积），n^ 是 n 的反对称矩阵。

书中直接给出了这个公式。这里补一个几何推导，因为推导过程本身就是对"绕轴旋转"最好的理解。

设单位轴 n，任意向量 v，要把 v 绕 n 转 θ 得到 v′。把 v 分解成平行于轴和垂直于轴的两部分：

v∥ = (n · v) n = n nᵀ v          （平行分量：v 在轴上的投影）
v⊥ = v − v∥                       （垂直分量）

平行分量在转轴上，旋转时纹丝不动。垂直分量在与轴垂直的平面内做平面旋转。在这个平面里，v⊥ 和 n × v⊥ 是两个互相垂直、长度相等的向量（因为 n 是单位向量且与 v⊥ 垂直），正好构成这个平面的一组正交基。平面内转 θ 的结果是

v⊥′ = cos θ · v⊥ + sin θ · (n × v⊥)

又因为 n × v∥ = 0（n 和 v∥ 平行），所以 n × v⊥ = n × v。合起来：

v′ = v∥ + v⊥′
   = n nᵀ v + cos θ · (v − n nᵀ v) + sin θ · n^ v
   = [ cos θ · I + (1 − cos θ) · n nᵀ + sin θ · n^ ] v

方括号里的矩阵对任何 v 都成立，它就是 R。推导里三项的含义一清二楚：第一项是"原向量缩放 cos θ"，第二项把轴向分量补回来，第三项是垂直平面内转出来的 sin θ 分量。注意 n^ 的出现------反对称矩阵又来了。

4.4 反过来：从旋转矩阵求旋转向量

**求转角。**对罗德里格斯公式两边取迹（主对角线元素之和）。tr(I) = 3，tr(n nᵀ) = nᵀ n = 1，反对称矩阵对角线全是零所以 tr(n^) = 0。于是

tr(R) = 3 cos θ + (1 − cos θ) = 1 + 2 cos θ
​
θ = arccos( (tr(R) − 1) / 2 )

**求转轴。**转轴上的向量旋转后不变，即 R n = n，所以 n 是 R 对应特征值 1 的特征向量。解这个特征值问题并归一化即可。

这两个式子在下一讲会以"对数映射"的名义重新出现。实际上，旋转向量就是 SO(3) 的李代数，罗德里格斯公式就是指数映射，这里的反解就是对数映射。书在本讲先用初等的方式把它们讲一遍，下一讲再揭晓它们的身份。

4.5 旋转向量的隐藏问题

旋转向量看上去完美：3 个数、无约束。但它也有自己的问题。

θ 和 θ + 2π 是同一个旋转，所以旋转向量到旋转的映射不是一一的，通常约定 θ 取 0 到 π 之间。而当 θ = π 时，绕 n 转 π 和绕 −n 转 π 是同一个旋转，表示不唯一。当 θ 趋近 0 时，转轴 n 变得无从定义（没转，哪来的轴），从旋转矩阵反解转轴会出现数值问题。

此外，两个旋转向量不能简单相加来表示复合旋转：绕 x 转 90° 再绕 y 转 90°，不等于绕 (1, 1, 0)/√2 转 90√2 度。只有在两个旋转都很小时，相加才是一个好的近似------这一点正是下一讲 BCH 公式要处理的。

4.6 欧拉角：最符合直觉、最不适合计算

欧拉角是对人类最友好的表示：把一个旋转拆成绕三个坐标轴依次转三个角 。航空和航海上习惯用 偏航---俯仰---滚转（yaw---pitch---roll）：

1. 绕物体的 Z 轴旋转，得到偏航角 ψ（机头朝左还是朝右）；

2. 绕旋转之后的 Y 轴旋转，得到俯仰角 θ（机头朝上还是朝下）；

3. 绕旋转之后的 X 轴旋转，得到滚转角 φ（机身向左倾还是向右倾）。

这种顺序叫 ZYX 欧拉角，三个角组成的 ψ, θ, φ 就是一种三维旋转表示。你在飞控、AHRS 的调试界面上看到的三个角就是它。

欧拉角有一堆需要约定的细节：旋转顺序是 ZYX 还是 XYZ 还是 ZXZ，每次是绕"旋转之后的轴"（内旋）还是绕"固定的世界轴"（外旋），角度的正方向怎么定。不同的软件、不同的教材、不同的厂商约定各不相同，所以当你拿到一组欧拉角时，第一件事是确认它的定义，否则换算出来的结果一定是错的。

第五章　四元数

5.1 为什么需要四元数

到目前为止我们有三种表示：旋转矩阵（9 个数，冗余且有约束）、旋转向量和欧拉角（3 个数，紧凑但有奇异性）。有没有一种表示既紧凑又没有奇异性？

答案是四元数：4 个数，1 个约束（单位长度），无奇异性。它的代价是不太直观，运算规则需要专门记。

四元数的历史动机来自复数。在二维平面上，复数可以表示旋转：把一个复数乘以 e^(iθ) = cos θ + i sin θ，就是把它逆时针转 θ。单位复数的集合是一个圆（SO(2)），它没有奇异性。哈密顿在 1843 年问：能不能把这个思路推广到三维？他发现三维"超复数"做不到，但四维可以------这就是四元数。

5.2 四元数的定义

一个四元数 q 有一个实部和三个虚部：

q = q₀ + q₁ i + q₂ j + q₃ k

其中 i、j、k 是三个虚数单位，满足

i² = j² = k² = −1
ij = k,   ji = −k
jk = i,   kj = −i
ki = j,   ik = −j

注意 ij = k 但 ji = −k：虚数单位之间的乘法不满足交换律。这正好对应三维旋转也不可交换（先绕 x 转再绕 y 转，和先绕 y 再绕 x，结果不同）。如果你观察 i、j、k 之间的乘法表，会发现它和三维单位向量的外积表 e₁ × e₂ = e₃ 一模一样，这不是巧合。

为了书写方便，把四元数写成一个标量加一个向量：

q = [s, v],    s = q₀（实部）,    v = [q₁, q₂, q₃]（虚部）

虚部为零的叫实四元数 ，实部为零的叫虚四元数。

5.3 四元数与旋转的对应

单位四元数（|q| = 1）可以表示三维旋转，而且和旋转向量之间有一个极其简洁的对应。绕单位轴 n 转 θ 的旋转，对应的四元数是

q = [ cos(θ/2),  n · sin(θ/2) ]

请注意是 θ/2 而不是 θ。这个"半角"是四元数最反直觉的地方，也是理解它的关键。

半角带来一个直接后果：q 和 −q 表示同一个旋转。把 θ 换成 θ + 2π，旋转本身没变，但 cos((θ + 2π)/2) = −cos(θ/2)，sin((θ + 2π)/2) = −sin(θ/2)，四元数整体变号。所以单位四元数到旋转的映射是二对一的，数学上叫"双重覆盖"。

这在工程上意味着什么？比较两个姿态是否接近时，不能直接算 |q₁ − q₂|，因为 q 和 −q 相差很远但姿态相同。正确的做法是先看两者点积的符号，若为负则把其中一个取反。做姿态插值（slerp）时也必须先做这一步，否则会插出"绕远路"的结果。如果你在 AHRS 里见过姿态突然"翻一下"又回来的现象，很可能就是这里出了问题。

反过来，从单位四元数读出转角和转轴：

θ = 2 arccos(q₀)

n = q₁, q₂, q₃ / sin(θ/2)

5.4 四元数的运算

设 q_a = s_a, v_a，q_b = s_b, v_b。

加减法：对应分量相加减，q_a ± q_b = s_a ± s_b, v_a ± v_b。

乘法：把每一项按分配律展开，再用 i、j、k 的乘法表化简。结果用向量形式写出来非常漂亮：

q_a q_b = [ s_a s_b − v_a · v_b ,   s_a v_b + s_b v_a + v_a × v_b ]

实部是"实部乘实部减去虚部的内积"，虚部是"交叉的实部乘虚部，再加上虚部的外积"。那个外积项就是不可交换性的来源：交换 a、b 后 v_a × v_b 变号，所以一般 q_a q_b ≠ q_b q_a。只有当两个虚部平行（即两个旋转绕同一根轴）时乘法才可交换。四元数乘法满足结合律。

模长：|q| = √(s² + v · v) = √(q₀² + q₁² + q₂² + q₃²)。乘积的模等于模的乘积：|q_a q_b| = |q_a| |q_b|。这保证了两个单位四元数相乘还是单位四元数------复合两个旋转仍是旋转。

共轭：虚部取反，q* = s, −v。四元数与自己的共轭相乘得到实四元数 q q* = s² + v · v, 0 = \|q\|², 0。

逆 ：q⁻¹ = q* / |q|²。对单位四元数，逆就是共轭，这和"旋转矩阵的逆就是转置"是同一件事的不同写法。乘积的逆要交换顺序：(q_a q_b)⁻¹ = q_b⁻¹ q_a⁻¹。

数乘：k q = k s, k v。

5.5 用四元数旋转一个向量

有了这些运算，就可以用四元数来转动一个三维向量了。设空间点 p = (x, y, z)，旋转由单位四元数 q 给出。步骤是：

1. 把三维点包装成一个虚四元数：p = 0, x, y, z；

2. 做一次"三明治"乘法：

   p′ = q p q⁻¹

3. 结果 p′ 仍然是一个虚四元数（可以证明），把它的虚部取出来，就是旋转后的点。

为什么要左乘一次右乘一次？直观地说，左乘 q 转了 θ/2，右乘 q⁻¹ 又转了 θ/2，合起来正好 θ------这就是半角的来历。更严格的解释要用到四元数在四维空间中的几何，这里不展开，知道"三明治公式对应一次完整的旋转"就够了。

5.6 四元数与旋转矩阵的换算

四元数和旋转矩阵描述同一个旋转，它们之间可以互相换算。从单位四元数 q = q₀, q₁, q₂, q₃ 到旋转矩阵：

      ┌ 1 − 2q₂² − 2q₃²     2q₁q₂ − 2q₀q₃      2q₁q₃ + 2q₀q₂  ┐
  R = │ 2q₁q₂ + 2q₀q₃      1 − 2q₁² − 2q₃²     2q₂q₃ − 2q₀q₁  │
      └ 2q₁q₃ − 2q₀q₂      2q₂q₃ + 2q₀q₁      1 − 2q₁² − 2q₂² ┘

这个公式不需要背，需要时查表或调库。但请注意它的结构：每个元素都是四元数分量的二次型（两个分量相乘）。这解释了为什么 q 和 −q 给出同一个 R------所有分量同时变号，两两相乘后符号抵消。

从旋转矩阵到四元数，可以先用迹求出 q₀：tr(R) = 4q₀² − 1，所以

q₀ = √(tr(R) + 1) / 2

再用非对角元素的差求出另外三个（R_ij 表示 R 的第 i 行第 j 列）：

q₁ = (R₃₂ − R₂₃) / (4q₀)
q₂ = (R₁₃ − R₃₁) / (4q₀)
q₃ = (R₂₁ − R₁₂) / (4q₀)

当 q₀ 接近零（转角接近 180°）时这个公式会除以一个很小的数，数值上不稳定。工程实现（包括 Eigen）会先判断 R 对角线上哪个元素最大，从对应的分量开始求，以保证分母不小。这又是一个"公式在数学上对、在数值上要绕路"的例子。

5.7 与 AHRS 的接口：为什么四元数每步都要归一化

如果你写过 Mahony 或 Madgwick 滤波器，核心的一步是用陀螺仪角速度 ω（机体系下）更新四元数，代码大致是这三行：

q̇ = (1/2) · q ⊗ 0, ω （四元数的导数）

q_{k+1} = q_k + q̇ · Δt （欧拉积分一步）

q_{k+1} ← q_{k+1} / |q_{k+1}| （归一化）

现在可以说清楚这三行的含义了。第一行是四元数的运动学方程，那个 1/2 来自半角；第二行是一阶欧拉积分，它在数学上是一个近似------真正的更新应该是右乘一个小旋转四元数：

q_{k+1} = q_k ⊗ cos(\|ω\|Δt / 2), (ω/\|ω\|) · sin(\|ω\|Δt / 2)

欧拉积分是这个精确公式对小角度的一阶展开。正因为是近似，积分之后的 q_{k+1} 的模长会稍微偏离 1，偏离会随时间累积，几千步之后四元数就不再表示一个旋转了，所以第三行把它拉回单位球面。

这件事在 SLAM 的后端里以另一种面貌出现：优化器每次迭代给四元数加一个增量之后，也必须归一化；或者更干脆地，不对四元数的 4 个分量直接优化，而是对它切空间里的 3 维小旋转向量优化，再通过"右乘小旋转"更新回四元数。后一种做法就是下一讲李代数扰动模型的四元数版本，也是 Ceres 的 QuaternionParameterization 和 g2o 的 VertexSE3Expmap 内部在做的事。换句话说：你在 AHRS 里被迫做的"积分后归一化"，是 SLAM 后端"流形上优化"的一个朴素版本。

第七章　实践：Eigen 几何模块与一个坐标变换例题

7.1 几何模块里的类型

Eigen 的几何模块（#include <Eigen/Geometry>）为本章的每一种表示都提供了对应的类：

数学对象	Eigen 类型	说明
旋转矩阵 R	Eigen::Matrix3d	就是普通 3×3 矩阵
旋转向量 θn	Eigen::AngleAxisd	构造参数 (角度, 单位轴)，底层不是矩阵但可以参与运算
欧拉角	Eigen::Vector3d	用 R.eulerAngles(2, 1, 0) 得到，参数表示轴的顺序（2=Z, 1=Y, 0=X，即 ZYX）
四元数 q	Eigen::Quaterniond	可以用 AngleAxisd 或 Matrix3d 直接构造
变换矩阵 T	Eigen::Isometry3d	名字叫 3d，实际是 4×4 矩阵
仿射变换	Eigen::Affine3d	4×4
射影变换	Eigen::Projective3d	4×4

各类型之间的转换非常自然：rotation_vector.toRotationMatrix() 得到矩阵，Quaterniond q(rotation_vector) 或 Quaterniond q(rotation_matrix) 得到四元数，Isometry3d T; T.rotate(rotation_vector); T.pretranslate(t); 构造变换矩阵。

用它们旋转一个向量也直接写乘法：rotation_vector * v、rotation_matrix * v、q * v、T * v。最后一个要特别说明：T * v 在数学上是 R v + t，Eigen 帮你隐式地补了齐次坐标的 1 并去掉了结果的第四维。q * v 在数学上则是 q v q⁻¹，Eigen 重载了乘号把三明治公式藏起来了。

7.2 四元数的两个坑

存储顺序。 Eigen 的 Quaterniond 构造函数的参数顺序是 (w, x, y, z)，即实部在前；但它内部存储和 .coeffs() 返回的顺序是 (x, y, z, w)，即实部在后。打印 q.coeffs() 看到四个数，最后一个才是实部。TUM 轨迹文件格式也是 qx qy qz qw 实部在后。不同的库（ROS 的 tf、Matlab、各种 IMU 固件）在这里约定不同，混用之前务必核对。

必须归一化。 当你从文件里读入一个四元数，或者用四个数手动构造一个四元数，它不一定是单位的。拿它去旋转向量之前必须 q.normalize()。书中坐标变换例题的第一句注释就是提醒这一点。不归一化的后果不是报错，而是算出一个悄悄错了的结果。

7.3 例题：两个机器人之间的坐标变换

书中 3.6.2 节给了一道题，它把本章所有内容串在了一起。

小萝卜一号和二号位于世界坐标系中。一号的位姿为 q₁ = 0.35, 0.2, 0.3, 0.1（实部在前），t₁ = 0.3, 0.1, 0.1；二号的位姿为 q₂ = −0.5, 0.4, −0.1, 0.2，t₂ = −0.1, 0.5, 0.3。这里的位姿指 T_RW，即世界到机器人的变换。小萝卜一号看到一个点，在自己坐标系下的坐标为 p_R1 = 0.5, 0, 0.2。求该点在小萝卜二号坐标系下的坐标。

思路只有一句话：先从一号变到世界，再从世界变到二号。

已知 T_R1W，它把世界坐标变成一号坐标。我们手上是一号坐标，要去世界，需要它的逆：

p_W = T_R1W⁻¹ p_R1

再用 T_R2W 变到二号：

p_R2 = T_R2W · T_R1W⁻¹ · p_R1

代码按这个式子直译：

Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
q1.normalize();            // 题给的四元数不是单位的，必须先归一化
q2.normalize();
Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
Eigen::Vector3d p1(0.5, 0, 0.2);
​
Eigen::Isometry3d T1w(q1), T2w(q2);   // 用四元数构造 Isometry，旋转部分就位
T1w.pretranslate(t1);                 // 再放平移
T2w.pretranslate(t2);
​
Eigen::Vector3d p2 = T2w * T1w.inverse() * p1;

程序输出约为 p_R2 ≈ −0.0309731, 0.73499, 0.296108。

这个例子里值得反复体会的有三点：第一，题目给的"位姿"是 T_RW 还是 T_WR，决定了第一步要不要求逆，题目不说清楚就必须问清楚；第二，变换矩阵的连乘从右往左读，p_R1 先经过 T_R1W⁻¹ 再经过 T_R2W；第三，pretranslate 和 translate 的区别------前者是在现有变换之后 加平移（左乘平移矩阵），后者是在之前 加（右乘），这里我们要的是"先旋转再平移"，所以用 pretranslate。

第八章　可视化演示

8.1 显示运动轨迹

书的 3.7 节用 Pangolin 库画了一条相机轨迹。Pangolin 是一个基于 OpenGL 的轻量级三维可视化库，ORB-SLAM 等项目也用它。这一节的重点不是 OpenGL，而是轨迹文件的格式和"轨迹"到底是什么。

轨迹文件 trajectory.txt 每一行是一个位姿，格式为

time  tx ty tz  qx qy qz qw

即时间戳、平移的三个分量、四元数的四个分量（实部在最后）。这是 TUM 数据集的格式，也是 SLAM 领域事实上的通用格式，评估工具 evo 直接读它。

读入之后，每一行构成一个 T_WC（相机到世界），其平移部分 t_WC 就是相机在世界中的位置。把相邻两个位置用线段连起来，就是轨迹。书里还为每个位姿画了三根短轴（红绿蓝对应 x、y、z），这三根轴的方向就是 R_WC 的三列------还记得第 2.2 节说的吗，旋转矩阵的三列就是新坐标系的三个轴在旧坐标系下的方向。

所以"轨迹"在数学上是一条 SE(3) 上的离散曲线，画出来的每一个带小坐标架的点就是这条曲线上的一个采样。第 10 讲的位姿图优化，优化的就是这条曲线上的所有点。

8.2 显示相机位姿

visualizeGeometry 程序提供了一个可以用鼠标拖动的三维相机模型，同时实时显示它当前的旋转矩阵、平移向量、欧拉角和四元数。这是一个很好的直觉训练工具：拖着相机转一转，看四种表示怎么同步变化。特别建议做两件事------把相机俯仰到接近 90°，观察欧拉角如何剧烈跳动（万向锁现身）；再把相机转一整圈回到原处，观察四元数是否变成了原来的相反数（双重覆盖现身）。

小结：四种语言的对照表与使用场合

表示	参数个数	约束	奇异性	复合	适合做什么
旋转矩阵 R	9	6 个（正交 + 行列式）	无	矩阵乘	旋转向量、计算投影、作为中间量
旋转向量 θn	3	无	θ = π 处	不能直接加	表示小旋转、作为优化增量（下一讲的李代数）
欧拉角	3	无	俯仰 ±90° 处	不能直接加	只用于显示和调试
单位四元数 q	4	1 个（单位长度）	无（但二对一）	四元数乘	存储、传输、插值、作为状态变量

一个典型的 SLAM 系统会这样使用它们：用四元数（或 SE(3) 矩阵）存储和传递位姿，用旋转矩阵做几何计算，用李代数（旋转向量的推广）做优化增量，用欧拉角打印给人看。

本章留下了一个明确的问题给下一讲：旋转矩阵不能加增量，四元数加了增量要归一化，旋转向量在小角度下可以近似相加------那么到底有没有一种严格的方式，让我们能对一个旋转"加一点点"，而且这个"加"有良好的数学性质可以用来求导和优化？下一讲的李群与李代数就是答案。你会看到，本章出现过的三样东西------反对称矩阵 a^、罗德里格斯公式、从 R 反解转角转轴------分别就是李代数 so(3)、指数映射和对数映射。它们已经全部登场了，只是还没有被叫出名字。