第1章：数学基础回顾（上）——三维空间刚体变换
各位同学好，我是你们这门课的主讲。今天咱们先不急着上手写代码，把数学底子打牢了，后面做配准才不会发懵。三维空间里的刚体变换，说白了就是研究一个物体怎么在空间里「动」——旋转、平移，但不发生形变。你想想看，激光雷达扫了一帧点云，下一帧车往前开了几米，这两帧数据怎么对齐？靠的就是这套数学工具。
1.1 旋转矩阵：最直观的旋转表达
旋转矩阵，我习惯叫它「方向描述器」。一个3x3的矩阵，就能把一个三维向量转到另一个方向。它的核心性质是正交且行列式为+1——说白了就是旋转不改变向量的长度和手性。
举个例子，绕Z轴旋转θ角，旋转矩阵长这样：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]我在项目里遇到过一个问题：用旋转矩阵做连续旋转时，矩阵乘法顺序特别容易搞错。记住，左乘是绕固定坐标系转，右乘是绕自身坐标系转。这个坑我踩过不止一次，后来养成了习惯——每次写代码前先画个坐标系草图。
重要性质：
- R^T = R^{-1}（转置等于逆，求逆超快）
- det(R) = 1（保证是纯旋转，不含镜像）
- 三个列向量两两正交，且都是单位向量
1.2 欧拉角：直观但暗藏杀机
欧拉角大家应该不陌生，就是绕三个轴依次转三个角度。常用的有ZYX顺序（偏航-俯仰-滚转），在无人机和自动驾驶里很常见。
但我要提醒你——万向锁（Gimbal Lock）是欧拉角最大的坑。当第二个旋转角达到±90°时，第一个和第三个轴会重合，丢失一个自由度。我曾经在调试一个机械臂时，就因为没处理万向锁，导致末端执行器突然「卡住」了。排查了半天才发现是欧拉角表达的问题。
避坑指南：
我曾经在融合IMU和激光雷达数据时，直接用欧拉角做插值，结果姿态出现了剧烈跳变。后来改用四元数才解决。所以我的建议是：欧拉角只适合给人看，不适合做计算。
1.3 四元数：计算界的瑞士军刀
四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk。它没有万向锁，插值平滑，计算效率高。在SLAM里，我几乎所有的姿态优化都用四元数。
四元数乘法对应旋转的复合，但注意顺序——和矩阵一样，不满足交换律。单位四元数（模长为1）表示纯旋转，它的逆就是共轭。
// 四元数旋转向量
// 输入：四元数 q，三维向量 v
// 输出：旋转后的向量 v'
// 公式：v' = q * v * q^{-1}
// 伪代码实现
function rotateByQuaternion(q, v):
// 把v写成纯四元数
p = (0, v.x, v.y, v.z)
// 旋转
p_rotated = q * p * conjugate(q)
// 取虚部
return (p_rotated.x, p_rotated.y, p_rotated.z)我的习惯：
在代码里，我通常用Eigen库的Quaterniond类。初始化时注意顺序——Eigen用的是(w, x, y, z)，而有些库用的是(x, y, z, w)。这个细节搞反了，整个旋转就错了。
1.4 齐次坐标与变换矩阵
齐次坐标，就是在三维坐标后面加一个1，变成四维。为什么这么做？因为这样就能用4x4矩阵统一表示旋转和平移了。
变换矩阵T长这样：
T = [R t]
[0 1]其中R是3x3旋转矩阵，t是3x1平移向量。用这个矩阵左乘一个齐次坐标点，就能同时完成旋转和平移。
我记得刚入行时，总搞不清变换矩阵的逆怎么求。其实很简单：
T^{-1} = [R^T -R^T * t]
[0 1 ]不需要真的去求4x4矩阵的逆，直接用这个公式，效率高得多。
齐次坐标的好处：
- 用矩阵乘法统一处理旋转+平移
- 可以方便地表示无穷远点（最后一个分量为0）
- 变换的复合就是矩阵乘法，非常直观
1.5 知识体系总览
下面这张图是我自己整理的，把本章的核心逻辑串起来了。你仔细看看，就能明白这些概念之间的关系。
1.6 实战中的选择建议
说了这么多，到底该用哪种表达？我根据经验给个参考：
| 应用场景 | 推荐表达 | 原因 |
|---|---|---|
| 点云配准优化 | 四元数 + 平移向量 | 无奇异点，插值平滑，优化效率高 |
| 人机交互/可视化 | 欧拉角 | 直观，容易理解 |
| 坐标变换链 | 变换矩阵 | 统一表达，复合方便 |
| 滤波/融合 | 四元数 | 避免万向锁，适合EKF等滤波器 |
我的建议：
刚开始学的时候，先把旋转矩阵和变换矩阵搞透。这两个是基础中的基础。四元数可以后面再深入，但一定要会用。欧拉角嘛——知道怎么转成其他表达就够了，别用它做核心计算。
嗯，这一章的内容就到这儿。数学基础是枯燥了点，但后面做配准时你会发现，今天花的时间绝对值。下一章我们继续聊李群李代数，那是优化配准的核心工具。