• 从旋转的物理意义

• 到旋转矩阵的数学性质

• 再到刚体变换

• 最后才到李群李代数

欧拉角（Euler Angle）——最直观的旋转表示

欧拉角 = 把任意 3D 旋转，拆成"先绕 Z 转、再绕 Y 转、再绕 X 转"，用三个角度来描述。

三个角度有专用名字（航空/无人机常用）：

英文	中文	绕哪个轴	想象成
Yaw（ψ）​	偏航角	Z 轴	飞机左右转头（看左边/右边）
Pitch（θ）​	俯仰角	Y 轴	飞机抬头/低头（上看天/下看地）
Roll（φ）​	滚转角	X 轴	飞机侧倾/翻滚（向左歪/向右歪）

Z（上） ↑ |__ Y（右） / X（前，机头朝向）

你坐飞机时机长说的"偏航、俯仰、滚转"就是这三个。

组合旋转（SLAM 常用 Z→Y→X 顺序）

Yaw = 30°, Pitch = 15°, Roll = 10°

按顺序做三次旋转：

1. 先绕自身 Z 轴转 30°（Yaw）

2. 再绕转完后的 Y 轴转 15°（Pitch）

3. 最后绕转完后的 X 轴转 10°（Roll）

⚠️顺序不能乱！​

Yaw→Pitch→Roll 和 Roll→Pitch→Yaw 结果不一样

绕各轴转角度 α 的基本旋转矩阵：

绕 X（Roll）：

Rx = │ 1 0 0 │ │ 0 cosα -sinα │ │ 0 sinα cosα │

绕 Y（Pitch）：

Ry = │ cosα 0 sinα │ │ 0 1 0 │ │ -sinα 0 cosα │

绕 Z（Yaw）：

Rz = │ cosα -sinα 0 │ │ sinα cosα 0 │ │ 0 0 1 │

完整旋转矩阵（ZYX 顺序）：

R = Rz(Yaw) · Ry(Pitch) · Rx(Roll)

→ 先算 Rx，再 Ry，最后 Rz，左乘。

欧拉角的两大缺点（记住这俩）

❌ 1. 顺序依赖

三组角度必须说清旋转顺序，否则无意义。

常用约定：ZYX（先 Yaw，再 Pitch，最后 Roll）

---

❌ 2. 万向锁（Gimbal Lock）——重点理解！

什么时候出事？​

当Pitch = ±90°​ 时，Roll 轴和 Yaw 轴重合，三个自由度退化成两个。

形象比喻——三环陀螺仪：

• 正常：三个金属环互相垂直，各自独立转 ✅

• Pitch 转 90° 后：内环和外环在同一个平面 → 两个环锁在一起 ❌

• 这时你转"Roll"和转"Yaw"效果一样，丢了一个自由度

旋转矩阵 R（SO(3)）——欧拉角之后自然出现的那个

一、为什么欧拉角之后一定是它？

你刚才已经看到欧拉角有两个大坑：

1. 顺序乱：先转谁后转谁，结果不一样

2. 万向锁：Pitch = 90° 时直接废一个自由度

二、旋转矩阵一句话是什么？

旋转矩阵 = 把“旋转后的三个坐标轴”并排放在一起

形象理解

假设你站在房间里，手里拿着一个坐标系：

• 红箭头：X 轴

• 绿箭头：Y 轴

• 蓝箭头：Z 轴

你转了一下身体（旋转），然后问自己三个问题：

1. 原来的 X 轴，现在指向哪儿？

2. 原来的 Y 轴，现在指向哪儿？

3. 原来的 Z 轴，现在指向哪儿？

把这三个答案写成列向量，并排放在一起，就是一个3×3 矩阵。

三、第一个具体数字例子（超重要）

例①：什么都不转（单位旋转）

你没动，原来的轴还在原地：

原轴	新方向
X 轴	(1, 0, 0)
Y 轴	(0, 1, 0)
Z 轴	(0, 0, 1)

拼成矩阵：

R = │ 1 0 0 │ │ 0 1 0 │ │ 0 0 1 │

这就是单位矩阵 I。

👉 含义：

没转，原样。

---

例②：绕 Z 轴转 90°（Yaw = 90°）

你站在地上，原地左转 90°。

原轴	转完之后指向
X 轴	原来 Y 方向 (0, 1, 0)
Y 轴	原来 -X 方向 (-1, 0, 0)
Z 轴	还是朝上 (0, 0, 1)

拼成矩阵：

R_z90 = │ 0 -1 0 │ │ 1 0 0 │ │ 0 0 1 │

你可以这样验证自己：

• 原来朝前的 X 轴 → 现在朝左（Y 正）

• 原来朝左的 Y 轴 → 现在朝后（X 负）

✅ 完全符合直觉。

---

例③：欧拉角 vs 旋转矩阵（对照理解）

欧拉角：

Yaw = 90°, Pitch = 0°, Roll = 0°

对应的旋转矩阵就是上面那个R_z90。

👉 到这里你就明白一句话了：

**欧拉角是“怎么转的过程”，

旋转矩阵是“转完的结果”。**

六、旋转矩阵的优缺点（笔记重点）

优点	缺点
✅ 没有万向锁	❌ 9 个数，冗余
✅ 旋转可以连乘	❌ 不能加
✅ 数学性质好	❌ 不适合优化

👉 所以 SLAM 里：

• 存位姿、算变换：用旋转矩阵

• 做优化、求导：不用它

四元数（Quaternion）——旋转世界的“稳定老司机”

刚经历过欧拉角，还记得它的两个致命伤吗：

1. 顺序乱（先转谁后转谁结果不一样）

2. 万向锁（Pitch = 90° 直接废一个自由度）

于是数学家开始想：

有没有一种方式，可以：

• 不用“分步转”

• 没有奇点

• 还能稳定插值、稳定计算

答案就是：四元数

四元数 = 绕某根轴，转某个角度

就这么简单。

它不是“三步转”，而是：

一步到位：往哪个方向转、转多少

四元数最大的杀手锏：插值（SLAM / 动画最爱）

场景：

• 相机在时间 t₀ 的朝向

• 相机在时间 t₁ 的朝向

• 想知道中间某一帧的朝向

用欧拉角：

❌ 会乱跳、穿模、万向锁

用四元数：

✅球面线性插值（SLERP）

• 路径最短

• 速度均匀

• 非常丝滑

这就是为什么：

• Unity / Unreal

• IMU / 无人机

• AR / VR

全都在用四元数。

四元数 vs 欧拉角（对比记）

对比项	欧拉角	四元数
直观	✅ 非常	❌ 一般
万向锁	❌ 有	✅ 无
插值	❌ 差	✅ 非常好
计算稳定性	❌ 一般	✅ 很高
参数数量	3	4

四元数在 SLAM 里干嘛用？

场景	用不用
给人看姿态	❌（转成欧拉角）
IMU 积分	✅ 常用
滤波（EKF）	✅ 常用
BA 优化	❌（用李代数）

一句话记住：

**四元数是“旋转的运动员”，

李代数是“优化的工具人”。**

李代数 so(3) —— 旋转的“小增量 / 微调”

一、为什么四元数之后一定是 so(3)？

你现在已经见过三种旋转表示：

表示	特点
欧拉角	人看得懂，但会炸
旋转矩阵 R	稳，但不能加
四元数	稳，但也不是向量

现在问题来了，一个非常工程化的问题：

如果我猜了一个位姿，发现不太对，我想“改一点点”，怎么办？

比如：

• 相机往左挪 2 cm

• 再往左转 0.5°

这时候你就需要：

“旋转的小增量”

这就是so(3)​ 登场的理由。

二、一句话理解 so(3)

so(3) = 一个三维向量，表示“往哪边转、转多少”

它本质上就是：

✅轴角（Axis-Angle）

三、so(3) 长什么样？（只看这个）

ϕ = (ϕ₁, ϕ₂, ϕ₃)

几何含义是：

分量	含义
ϕ₁	绕 X 轴转多少
ϕ₂	绕 Y 轴转多少
ϕ₃	绕 Z 轴转多少

👉 你可以把它当成：

“旋转速度的瞬时版本”

---

四、第一个具体数字例子（一定要看）

例①：什么都不动

ϕ = (0, 0, 0)

👉 对应：

• 没转

• 单位旋转矩阵

---

例②：绕 Z 轴转 5°

注意，这里是小角度。

ϕ = (0, 0, 5°)

如果你把它“用掉”，就会得到一个接近单位矩阵的 R：

R ≈ │ cos5° -sin5° 0 │ │ sin5° cos5° 0 │ │ 0 0 1 │

👉 这就是 so(3) 的直觉：

它不是一个“完整旋转”，而是“旋转的微分”

例③：相机往左歪一点点

ϕ = (0, 0, 0.5°)

在 SLAM 里，这种东西会出现在：

• 迭代优化

• 梯度下降

• 位姿微调

---

五、为什么 so(3) 这么香？

✅ 1️⃣ 它是向量，可以加！

假设你现在有两个小调整：

ϕ₁ = (0, 0, 0.5°) ϕ₂ = (0.2°, 0, 0)

你可以直接加：

ϕ = ϕ₁ + ϕ₂

👉 这在旋转矩阵、四元数上是做不到的。

✅ 2️⃣ 非常适合优化（BA 的核心）

SLAM 在做的事情，本质是：

不断给位姿加一个小增量 ϕ，让误差变小

流程是这样的：

当前位姿 T ↓ 算误差 ↓ 得到一个 se(3) / so(3) 增量 ξ ↓ T_new = exp(ξ) · T_old ↓ 重复

你不需要手算 exp，只要知道：

优化 = 在李代数上加向量

六、so(3) 和前面几个东西的关系（重点）

东西	层级
欧拉角	人用
旋转矩阵 R	最终结果
四元数 q	稳定表示
so(3)	优化用的增量

一句话记住：

R / q 是“位置”，so(3) 是“速度 / 微调”

SE(3) —— 3D 世界里“相机/机器人的位姿”

一、一句话是什么（先记住这个）

SE(3) = 旋转 + 平移，打包成一个整体

换句话说：

SE(3) 就是“相机在哪、朝哪看”的完整描述

二、它是怎么被逼出来的？

① 旋转用 R（SO(3)）

R ∈ SO(3)

② 平移用 t

t ∈ ℝ³

最朴素的想法是：

(R, t)

但马上出问题了：

问题	原因
怎么复合？	要先转再移，顺序不能乱
怎么求逆？	手算麻烦
怎么统一运算？	不是一个东西

👉 数学家就想：

能不能把 R 和 t 塞进同一个数学对象里？

三、齐次坐标：关键转折点（形象版）

普通坐标

p = (x, y, z)

齐次坐标

p_h = (x, y, z, 1)

多加一个 1，看起来多余，但它干了一件大事：

让“旋转 + 平移”变成一次矩阵乘法

四、SE(3) 长什么样？（重点）

SE(3) 是一个4×4 矩阵：

T = │ R t │ │ 0 1 │

展开写出来就是：

T = │ r11 r12 r13 tx │ │ r21 r22 r23 ty │ │ r31 r32 r33 tz │ │ 0 0 0 1 │

部分	含义
左上 3×3	旋转 R（SO(3)）
右上 3×1	平移 t
最后一行	固定 (0 0 0 1)

👉 这就是SE(3)。

五、第一个具体数字例子（一定要看）

例①：相机在原地，没动

R = I t = (0, 0, 0)

对应的 SE(3)：

T = │ 1 0 0 0 │ │ 0 1 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │

👉 世界坐标系 = 相机坐标系。

---

例②：相机往前走了 1 米

“前”是 Z 轴负方向（SLAM 常用约定）。

R = I t = (0, 0, 1)

T = │ 1 0 0 0 │ │ 0 1 0 0 │ │ 0 0 1 1 │ │ 0 0 0 1 │

👉 相机在世界里，沿 Z 轴走了 1 m。

---

例③：相机左转 90°，再往前走

• 旋转：绕 Z 转 90°（你之前见过的 R_z90）

• 平移：t = (0, 0, 1)

T = │ 0 -1 0 0 │ │ 1 0 0 0 │ │ 0 0 1 1 │ │ 0 0 0 1 │

你可以这样读它：

这个相机：左转 90°，然后往前走 1 米

一句话就描述完了 ✅

---

六、SE(3) 在干的两件正经事

✅ 1️⃣ 把一个点从相机坐标系变到世界坐标系

p_world = T · p_camera

不用你管先转还是先移，矩阵一次搞定。

---

✅ 2️⃣ 把两个位姿拼起来

比如：

• 先动一次 T₁

• 再动一次 T₂

T = T₂ · T₁

👉 这就是 SLAM 里每天都在干的事。

---

七、SE(3) 的“性格”（优缺点）

优点	缺点
✅ 表达完整	❌ 16 个数
✅ 可复合	❌ 不能加
✅ 数学漂亮	❌ 不适合优化

👉 所以：

存位姿用 SE(3)，优化用 se(3)

se(3) —— 位姿的“微调旋钮”

一、一句话是什么（先记住）

se(3) = 位姿的小增量 = 平移微调 + 旋转微调

它是一个6 维向量：

ξ = (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)

二、它从哪里来？（非常顺）

这条线：

SE(3) = 位姿本身 ↓ 但 SE(3) 不能加 ↓ 那“改一点点位姿”怎么表示？ ↓ se(3)

👉 所以 se(3) 的存在理由只有一个：

为了优化位姿

三、se(3) 各部分是什么意思？

ξ = (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)

部分	名字	含义
ρ	平移增量	往哪个方向挪多少
ϕ	旋转增量	往哪边转多少

👉 你可以直接把它当成：

“给当前相机拧的两个小旋钮”

四、第一个具体数字例子（一定要看）

例①：完全不动

ξ = (0, 0, 0, 0, 0, 0)

👉 对应单位变换矩阵。

---

例②：相机往左挪 2 cm

ξ = (0, 0.02, 0, 0, 0, 0)

• 前三个数：平移

• 后三个数：旋转（没有）

---

例③：相机往左转 0.5°

ξ = (0, 0, 0, 0, 0, 0.5°)

👉 这就是 so(3)，现在嵌在 se(3) 里。

例④：真实 SLAM 里最常见的那种

ξ = (0.01, 0.005, 0, 0, 0, 0.3°)

含义是：

• 往右挪 1 cm

• 往前挪 0.5 cm

• 往左转 0.3°

✅直接加​

✅不用管约束​

✅不会坏

五、se(3) 在 SLAM 里干嘛用？

✅ 核心用途：BA（Bundle Adjustment）

BA 的本质是：

不断给每个位姿加一个小增量 ξ，让误差变小

流程是：

当前位姿 T ↓ 算误差 ↓ 求一个 se(3) 增量 ξ ↓ T_new = exp(ξ) · T_old ↓ 重复

👉 你永远不会直接改 R / t

👉 你永远是改se(3)

---

六、se(3) vs SE(3)（对比记）

对比	SE(3)	se(3)
是什么	位姿	位姿的增量
形式	4×4 矩阵	6 维向量
能不能加	❌	✅
优化用	❌	✅
约束	很多	几乎没有

一句话记住：

SE(3) 是“答案”，se(3) 是“步子”

📒 笔记：se(3) 里面的相关计算（工程直觉版）

---

一、先给你一个“总览表”（心里有底）

计算	在干嘛
向量 → 矩阵	把 6 维向量变成 4×4 矩阵
矩阵 → 向量	把 4×4 矩阵变回 6 维向量
指数映射 exp	把“微调”变成“真实位姿”
对数映射 log	把“真实位姿”变成“微调”
李括号 [·,·]	描述“两次微调的差别”

👉你写代码几乎只会用到 exp / log

---

二、① 向量 → 矩阵（∧ 运算）

输入：se(3) 向量

ξ = (ρ₁, ρ₂, ρ₃, ϕ₁, ϕ₂, ϕ₃)

输出：4×4 矩阵

ξ^ = │ ϕ^ ρ │ │ 0 0 │

具体数字例子

ξ = (1, 2, 3, 0, 0, 0.5)

意思是：

• 平移：(1, 2, 3)

• 绕 Z 转 0.5 rad

对应的矩阵：

ξ^ = │ 0 -0.5 0 1 │ │ 0.5 0 0 2 │ │ 0 0 0 3 │ │ 0 0 0 0 │

✅ 左上角是 so(3)

✅ 右上角是平移

✅ 最后一行全是 0

👉 这一步只是为了数学结构完整，你平时很少手写。

---

三、② 矩阵 → 向量（∨ 运算）

这是 ∧ 的逆操作。

ξ^ → ξ

👉 写代码时一般由库帮你做（Sophus / Eigen）。

你只需要知道：

• 左上角 → 旋转向量

• 右上角 → 平移向量

---

四、③ 指数映射 exp（最重要）

一句话是什么

exp：把“微调”变成“真实位姿”

数学位置

exp(ξ) = T ∈ SE(3)

具体数字例子（经典）

输入：纯平移

ξ = (1, 0, 0, 0, 0, 0)

exp 后是：

T = │ 1 0 0 1 │ │ 0 1 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │

👉 平移 1 米，没旋转。

---

输入：纯旋转

ξ = (0, 0, 0, 0, 0, 90°)

exp 后是：

T = │ 0 -1 0 0 │ │ 1 0 0 0 │ │ 0 0 1 0 │ │ 0 0 0 1 │

👉 左转 90°，没平移。

---

输入：旋转 + 平移

ξ = (1, 0, 0, 0, 0, 90°)

exp 后：

• 先转 90°

• 再走 1 米

👉 平移方向会被旋转“掰弯”。

---

五、④ 对数映射 log（第二重要）

一句话是什么

log：把“真实位姿”变成“微调”

数学位置

log(T) = ξ ∈ se(3)

什么时候用？

比如：

• 你有两个位姿 T₁、T₂

• 想知道它们之间的“差值”

做法是：

ξ = log(T₂ · T₁⁻¹)

👉 得到的就是从 T₁ 到 T₂ 的微调

---

六、⑤ 李括号 [·,·]（高级，但要知道）

一句话是什么

李括号 = 两次微调的顺序差

直觉

[ξ₁, ξ₂] ≈ 先 ξ₁ 再 ξ₂ − 先 ξ₂ 再 ξ₁

具体数字感受

ξ₁ = (1, 0, 0, 0, 0, 0) ξ₂ = (0, 1, 0, 0, 0, 0)

• 先走 X 1 米，再走 Y 1 米

• 先走 Y 1 米，再走 X 1 米

👉 结果一样 → 李括号很小

但如果加入旋转：

ξ₁ = (0, 0, 0, 0, 0, 90°) ξ₂ = (1, 0, 0, 0, 0, 0)

👉 顺序不同，结果完全不同

👉 李括号 ≠ 0

---

七、写代码时你真正会用到的（重点）

C++（Sophus）

Sophus::SE3d T; // 优化变量 Eigen::Vector6d xi = T.log(); // 更新位姿 T = T * Sophus::SE3d::exp(xi);

👉你永远不会手写 exp / log 公式

本文系统介绍了3D旋转和位姿表示的核心概念：欧拉角、旋转矩阵、四元数、李群SE(3)和李代数se(3)。欧拉角直观但存在顺序依赖和万向锁问题；旋转矩阵描述结果但冗余；四元数无奇点且插值稳定；李代数so(3)/se(3)作为优化工具提供增量式调整。SE(3)整合旋转和平移，而se(3)是其对应的微调空间。实际应用中，欧拉角用于人机交互，四元数用于稳定计算，李代数用于优化迭代，旋转矩阵和SE(3)用于存储最终位姿。这些表示各具特点，在SLAM、机器人等领域形成互补使用模式。

BA 的本质，不是“优化位姿”，

而是“在 se(3) 上不断给位姿加微调”。

🎬 BA 的一次完整迭代（故事线版）

我们把主角设定好：

• 一个相机

• 一张图像

• 一个 3D 点

• 一个“猜得不太准”的初始位姿

---

第 0 步：初始状态（世界长这样）

① 相机位姿（SE(3)）

T = 单位矩阵

意思是：

相机在世界原点，朝 Z 轴负方向看

---

② 3D 点在世界上

P_world = (5, 0, 10)

意思是：

这个点在前方 10 m，偏右 5 m

---

③ 投影到图像上（理想情况）

相机看到这个点，像素坐标应该是：

u = 320 v = 240

---

第 1 步：现实很骨感（误差出现了）

但你实际在图像里检测到的像素是：

u_obs = 310 v_obs = 245

👉 差了 10 个像素

这说明一件事：

要么点不准，要么相机位姿不准

BA 的假设是：

• 点是对的

• 位姿是错的

---

第 2 步：把误差“翻译”成语言

你现在的问题是：

“相机要怎么动，才能让投影误差变小？”

但相机位姿是 SE(3)，你不能直接加。

所以你必须：

✅先把 SE(3) 变成 se(3)

---

第 3 步：对数映射 log（位姿 → 微调）

你做一件事：

ξ = log(T)

结果是：

ξ = (0, 0, 0, 0, 0, 0)

👉 因为你现在是单位位姿。

---

第 4 步：优化器算出“该怎么改”

优化器（比如高斯–牛顿）算出一个增量：

Δξ = (0.01, 0, 0, 0, 0, -0.3°)

含义是：

相机往右挪 1 cm，往左转 0.3°

✅ 这是一个se(3)​

✅ 它是一个向量

✅ 可以直接加

---

第 5 步：更新微调（向量加法）

ξ_new = ξ + Δξ

这一步：

• 在旋转矩阵上做不到

• 在四元数上也做不到

• 只有在 se(3) 上能做到

👉 这就是李代数存在的最大意义。

---

第 6 步：指数映射 exp（微调 → 位姿）

你现在有了新的微调 ξ_new，但你要用的是位姿 T。

所以你做：

T_new = exp(ξ_new)

得到一个新的 SE(3) 位姿。

---

第 7 步：用新位姿重新投影

你把同一个 3D 点，用新的 T_new 再投一次：

u_proj = 315 v_proj = 243

误差从：

10 px → 5 px

👉 变小了 ✅

---

第 8 步：判断是否收敛

• 误差还够小吗？

  • 不够 → 回到第 4 步

  • 够了 → 结束