1. 坐标系基础

做SLAM这么多年，我越来越觉得坐标系这东西，就像空气一样——平时感觉不到它的存在，但一旦出问题，整个系统就崩了。今天咱们就来聊聊刚体运动和旋转矩阵，这是整个坐标变换的基石。

1.1 刚体运动到底是什么？

说白了，刚体运动就是物体在空间里又转又移，但形状大小一点不变。你想想看，你手里的手机从桌上拿到耳边，这个过程就是刚体运动——手机没变形，只是位置和朝向变了。

在SLAM里，相机、机器人、路标点，我们都把它们当成刚体来处理。这样做的好处是：我们只需要关心6个自由度——3个平移 + 3个旋转。

1.2 旋转矩阵——我见过最多的坑

旋转矩阵是个3×3的方阵，它满足两个性质：

• 正交性：R^TR = 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转，结果完全不一样。

我曾经在标定外参时，把乘法顺序搞反了，结果标出来的外参误差大得离谱。排查了两天才发现是这个问题——从那以后，我每次写旋转矩阵乘法都会在注释里标清楚顺序。

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)

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的三角函数默认用弧度，但很多人习惯用角度。我建议在代码开头就统一用弧度，或者在函数名里标明单位。
• 矩阵乘法顺序：R₁ @ R₂ 和 R₂ @ R₁ 结果完全不同。写代码时一定要在注释里写清楚旋转顺序。

好了，这一章就到这里。旋转矩阵是坐标变换的基石，理解透了后面学四元数、李代数都会轻松很多。记住：多动手写代码，多画图验证，比死记公式管用一万倍。