3、刚体变换基础：旋转矩阵、平移向量、齐次坐标与齐次变换矩阵
各位同学好，我是你们的老朋友。今天我们来聊聊多传感器融合里最基础、也最绕不开的一块内容——刚体变换。
说实话，我刚入行那会儿，觉得这东西不就是矩阵乘来乘去嘛，有啥好学的？结果第一次做激光雷达和相机的联合标定，就被齐次坐标给坑了一把。嗯，从那以后我再也不敢小看这章内容了。
你想想看，我们车上装了激光雷达、摄像头、IMU、GPS，每个传感器都有自己的坐标系。怎么把激光点云投影到图像上？怎么把IMU的加速度转换到车体坐标系？说白了，全靠刚体变换在背后撑腰。
核心思想：刚体变换 = 旋转 + 平移。物体形状不变，只是位置和朝向变了。
3.1 旋转矩阵——描述“朝向”的数学工具
先聊旋转矩阵。我个人习惯把它理解成“坐标系的映射关系”。
假设有个点P，在坐标系A下的坐标是P_A。现在我想知道它在坐标系B下的坐标P_B。如果两个坐标系原点重合，只是旋转了，那这个关系就是：
P_B = R * P_A这里的R就是旋转矩阵。它是一个3x3的矩阵，有几个重要性质：
- 正交性：R^T * R = I，也就是说它的逆等于它的转置
- 行列式为+1：这是右手系的要求，如果是-1那就是反射变换了
- 三个自由度：虽然9个元素，但受正交约束，实际只有3个独立参数
我在项目中遇到过一个问题：有人直接用欧拉角做插值，结果出现了万向锁。那场景真是...无人机姿态控制直接乱跳。所以后来我学乖了，能用旋转矩阵就用旋转矩阵，别偷懒用欧拉角。
小技巧：验证一个矩阵是不是合法的旋转矩阵，直接算R^T * R，看是不是单位阵。误差在1e-6以内就算合格。
3.2 平移向量——描述“位置”的偏移
旋转搞定了，那平移就简单多了。平移向量t就是两个坐标系原点之间的位移。
完整的刚体变换公式：
P_B = R * P_A + t这里t是3x1的向量，表示从坐标系A原点到坐标系B原点的向量，在坐标系B下的表示。
嗯，这句话容易搞混。我刚开始也经常弄反。你记住一点：t的坐标系是目标坐标系，也就是等号左边的那个坐标系。
注意：平移向量和旋转矩阵是“绑定”的。你不能随便换一个旋转矩阵，还用原来的平移向量。它们必须来自同一个变换关系。
3.3 齐次坐标——为什么要引入它？
好，问题来了。上面那个公式P_B = R * P_A + t，写起来不优雅。你想想看，如果连续做多次变换：
P_C = R2 * (R1 * P_A + t1) + t2这式子越来越长，而且旋转和平移混在一起，看着就烦。
齐次坐标就是来解决这个问题的。它给三维点加一个维度，变成四维：
P_homogeneous = [x, y, z, 1]^T为什么是1？因为这样平移就能用矩阵乘法表示了。如果是方向向量（比如法线），最后一位是0。
记住这个口诀：点用1，向量用0。点平移后位置变，向量平移后不变。
3.4 齐次变换矩阵——把旋转和平移打包
有了齐次坐标，我们就可以把旋转和平移合并成一个4x4的矩阵T：
T = [R t]
[0 1]这样，一次变换就变成了一个矩阵乘法：
P_B_homo = T * P_A_homo连续变换就更简洁了：
T_AC = T_BC * T_AB注意顺序！矩阵乘法不满足交换律。先转A到B，再转B到C，这个顺序不能乱。
我记得有一次帮同事排查标定问题，他死活说外参不对。我一看代码，他把变换矩阵乘反了。嗯，这种错误我犯过不止一次，现在每次写变换之前都会先画个坐标系关系图。
3.5 知识体系总览
下面这张图是我自己整理的，把本章的核心逻辑串起来了：
3.6 实战中的注意事项
讲完理论，说点实际的。在做外参标定时，有几个坑我替你们踩过了：
- 坐标系定义要统一：前左上还是右前上？不同传感器厂商习惯不同。我见过一个项目，激光雷达用前左上，相机用右前下，结果标定出来全是错的。
- 变换顺序不能错：先旋转后平移，这个顺序是固定的。你要是先平移后旋转，结果完全不一样。
- 齐次坐标最后一维：做投影时记得归一化，把最后一维变成1。我见过有人忘了这步，点云投影到图像上全跑到画面外面去了。
我的习惯：每次写变换代码之前，先在纸上画清楚坐标系关系图。标清楚每个变换是从哪到哪，再动手写代码。这步花5分钟，能省你2小时调试时间。
3.7 代码示例：用Python实现刚体变换
最后给一段我常用的代码模板，你们可以直接拿去用：
import numpy as np
def build_transform(R, t):
"""构建齐次变换矩阵"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t.flatten()
return T
def transform_point(T, point):
"""对单个点做刚体变换"""
p_homo = np.append(point, 1.0)
p_transformed = T @ p_homo
return p_transformed[:3] / p_transformed[3]
def transform_points(T, points):
"""对多个点做刚体变换（Nx3矩阵）"""
N = points.shape[0]
points_homo = np.hstack([points, np.ones((N, 1))])
points_transformed = (T @ points_homo.T).T
return points_transformed[:, :3] / points_transformed[:, 3:4]
# 示例：绕Z轴旋转90度，再平移(1, 2, 3)
theta = np.pi / 2
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
t = np.array([[1], [2], [3]])
T = build_transform(R, t)
point = np.array([1, 0, 0])
result = transform_point(T, point)
print(f"变换结果: {result}") # 应该输出 [1, 3, 3]这段代码我用了好几年，基本没出过问题。唯一要注意的是，传入的平移向量t要是列向量，形状是(3,1)，不是(3,)。
好了，刚体变换这块就聊到这儿。说白了就是旋转加平移，齐次坐标让表达更优雅。你们自己动手写写代码，跑几个例子，很快就熟了。