1. 坐标系基础：刚体运动与旋转矩阵
做SLAM这么多年，我越来越觉得坐标系这东西，就像空气一样——平时感觉不到它的存在，但一旦出问题，整个系统就崩了。今天咱们就来聊聊刚体运动和旋转矩阵，这是整个坐标变换的基石。
1.1 刚体运动到底是什么？
说白了，刚体运动就是物体在空间里又转又移，但形状大小一点不变。你想想看，你手里的手机从桌上拿到耳边，这个过程就是刚体运动——手机没变形，只是位置和朝向变了。
在SLAM里，相机、机器人、路标点，我们都把它们当成刚体来处理。这样做的好处是：我们只需要关心6个自由度——3个平移 + 3个旋转。
核心要点：刚体运动 = 旋转 + 平移，两者互不干扰，可以分开处理。
1.2 旋转矩阵——我见过最多的坑
旋转矩阵是个3×3的方阵，它满足两个性质：
- 正交性：RTR = I，也就是说矩阵的逆等于它的转置
- 行列式为+1：保证了是纯旋转，没有镜像
我在项目中遇到过好几次，有人把旋转矩阵写错了，结果点云全飞到天上去。嗯，这里要注意：旋转矩阵的每一列，代表的是原坐标系的基向量在新坐标系下的表示。
1.3 绕坐标轴的旋转
最基本的三种旋转：
| 旋转轴 | 旋转矩阵（绕θ角） |
|---|---|
| X轴 | Rx(θ) = [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]] |
| Y轴 | Ry(θ) = [[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]] |
| Z轴 | Rz(θ) = [[cosθ, -sinθ, 0], [sinθ, cosθ, 0], [0, 0, 1]] |
我个人习惯用右手定则来判断正方向——拇指指向轴的正方向，四指弯曲的方向就是正角度方向。这个习惯帮我避免了很多低级错误。
1.4 复合旋转与顺序问题
这里有个大坑：旋转不满足交换律。也就是说，先绕X转再绕Y转，跟先绕Y转再绕X转，结果完全不一样。
警告：复合旋转时，矩阵乘法顺序是从右往左读。比如 R = Rz · Ry · Rx，表示先绕X转，再绕Y转，最后绕Z转。
我曾经在标定外参时，把乘法顺序搞反了，结果标出来的外参误差大得离谱。排查了两天才发现是这个问题——从那以后，我每次写旋转矩阵乘法都会在注释里标清楚顺序。
1.5 旋转矩阵的代码实现
用Python和NumPy实现起来很简单：
import numpy as np
def rotation_x(theta):
"""绕X轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[1, 0, 0],
[0, c, -s],
[0, s, c]
])
def rotation_y(theta):
"""绕Y轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, 0, s],
[0, 1, 0],
[-s, 0, c]
])
def rotation_z(theta):
"""绕Z轴旋转矩阵"""
c, s = np.cos(theta), np.sin(theta)
return np.array([
[c, -s, 0],
[s, c, 0],
[0, 0, 1]
])
# 复合旋转：先绕X转30°，再绕Z转45°
theta_x = np.radians(30)
theta_z = np.radians(45)
R = rotation_z(theta_z) @ rotation_x(theta_x)
print("复合旋转矩阵:\n", R)
小技巧：调试时可以用 R @ R.T 检查是否为单位矩阵，用 np.linalg.det(R) 检查行列式是否为1。这两个检查能帮你快速发现旋转矩阵的拼写错误。
1.6 刚体变换与齐次坐标
把旋转和平移合在一起，就得到了刚体变换：
p' = R · p + t
为了表示方便，我们引入齐次坐标，把3D点变成4D向量：
# 齐次变换矩阵
T = np.array([
[R[0,0], R[0,1], R[0,2], t[0]],
[R[1,0], R[1,1], R[1,2], t[1]],
[R[2,0], R[2,1], R[2,2], t[2]],
[0, 0, 0, 1 ]
])
# 点p的齐次坐标
p_homo = np.array([p[0], p[1], p[2], 1.0])
# 变换后的点
p_prime_homo = T @ p_homo
p_prime = p_prime_homo[:3] / p_prime_homo[3]
为什么用齐次坐标？说白了就是为了把旋转和平移统一成一个矩阵乘法，方便连续变换和求逆。这在SLAM后端优化里特别有用。
1.7 知识体系总览
下面这张图帮你理清本章的核心逻辑：
1.8 避坑指南
最后分享几个我踩过的坑：
- 旋转矩阵的维度：我曾经在C++里把3×3矩阵当成4×4来用，结果内存越界，程序直接崩了。检查了半小时才发现。
- 角度单位：NumPy的三角函数默认用弧度，但很多人习惯用角度。我建议在代码开头就统一用弧度，或者在函数名里标明单位。
- 矩阵乘法顺序：R1 @ R2 和 R2 @ R1 结果完全不同。写代码时一定要在注释里写清楚旋转顺序。
我的习惯：每次写完旋转矩阵相关代码，都会用单位向量做一次测试——把(1,0,0)转一下，看看结果对不对。这个小测试帮我省了无数调试时间。
好了，这一章就到这里。旋转矩阵是坐标变换的基石，理解透了后面学四元数、李代数都会轻松很多。记住：多动手写代码，多画图验证，比死记公式管用一万倍。