从 旋转的物理意义
到 旋转矩阵的数学性质
再到 刚体变换
最后才到 李群李代数
欧拉角（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°
按顺序做三次旋转：
先绕自身 Z 轴转 30°（Yaw）
再绕转完后的 Y 轴转 15°（Pitch）
最后绕转完后的 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)）------欧拉角之后自然出现的那个
一、为什么欧拉角之后一定是它？
你刚才已经看到欧拉角有两个大坑：
顺序乱：先转谁后转谁，结果不一样
万向锁：Pitch = 90° 时直接废一个自由度
二、旋转矩阵一句话是什么？
旋转矩阵 = 把"旋转后的三个坐标轴"并排放在一起
形象理解
假设你站在房间里，手里拿着一个坐标系：
红箭头：X 轴
绿箭头：Y 轴
蓝箭头：Z 轴
你转了一下身体（旋转），然后问自己三个问题：
原来的 X 轴，现在指向哪儿？
原来的 Y 轴，现在指向哪儿？
原来的 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）------旋转世界的"稳定老司机"
刚经历过欧拉角，还记得它的两个致命伤吗：
顺序乱（先转谁后转谁结果不一样）
万向锁（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 步
够了 → 结束