4. 点云坐标变换：刚体变换、旋转矩阵与四元数、欧拉角与变换矩阵

点云处理里，坐标变换可以说是最基础也最绕不开的一环。我刚开始做SLAM那会儿，就被旋转矩阵和四元数搞得晕头转向。说白了，你拿激光雷达扫了一堆点，这些点都是在传感器坐标系下的。但你要做建图、做定位，就得把它们转到世界坐标系下。这个「转」的过程，就是坐标变换。

今天咱们就把这块硬骨头啃下来。我会结合自己踩过的坑，把刚体变换、旋转矩阵、四元数、欧拉角这些概念串起来讲清楚。

4.1 刚体变换：点云对齐的核心

先问个问题：一个点云怎么从一个坐标系挪到另一个坐标系？

答案是刚体变换。它包含两部分：旋转和平移。旋转解决「朝向」问题，平移解决「位置」问题。刚体变换有个重要性质——变换前后，点与点之间的距离不变。你想想看，一个杯子从桌上拿到地上，杯子的形状没变，只是位置和朝向变了。这就是刚体变换。

我在项目中遇到过一个问题：两个不同时刻采集的点云，怎么拼到一起？其实就是求它们之间的刚体变换。这个在SLAM里叫「帧间匹配」，后面我们会详细讲。

4.2 旋转矩阵：最直观的旋转表示

旋转矩阵是个3x3的矩阵，它描述了一个坐标系相对于另一个坐标系的旋转关系。我个人的习惯是，先理解二维旋转，再推广到三维。

二维旋转很简单：

// 绕原点逆时针旋转θ角度
R(θ) = [cosθ  -sinθ]
       [sinθ   cosθ]

三维旋转就复杂一些。绕X、Y、Z轴分别旋转，对应的矩阵是：

旋转轴	旋转矩阵
绕X轴	[1, 0, 0; 0, cosθ, -sinθ; 0, sinθ, cosθ]
绕Y轴	[cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]
绕Z轴	[cosθ, -sinθ, 0; sinθ, cosθ, 0; 0, 0, 1]

4.3 四元数：避免万向锁的利器

旋转矩阵有9个参数，但自由度只有3。这就有冗余，而且容易产生数值问题。欧拉角虽然直观，但有个致命问题——万向锁。我记得第一次做无人机姿态估计时，俯仰角到了90度，偏航和滚转突然就耦合了，控制直接乱掉。

四元数就是为了解决这个问题而生的。它用4个参数表示旋转，没有奇异性，插值也平滑。

四元数的形式：

q = w + xi + yj + zk
其中 w^2 + x^2 + y^2 + z^2 = 1

用四元数旋转一个点P：

P' = q * P * q^(-1)

这里P要写成纯四元数形式 (0, x, y, z)。

4.4 欧拉角与变换矩阵

欧拉角用三个角度表示旋转：偏航(Yaw)、俯仰(Pitch)、滚转(Roll)。不同的旋转顺序会得到不同的结果。常用的顺序是ZYX，也就是先绕Z轴转Yaw，再绕Y轴转Pitch，最后绕X轴转Roll。

从欧拉角到旋转矩阵的转换：

R = Rz(Yaw) * Ry(Pitch) * Rx(Roll)

而完整的变换矩阵是4x4的齐次矩阵：

T = [R  t]
    [0  1]

这样就把旋转和平移统一到一个矩阵里了。点云的齐次坐标是 (x, y, z, 1)，变换起来非常方便：

P' = T * P

4.5 知识体系总览

下面这张图是我自己总结的坐标变换知识结构，帮你理清思路：

4.6 实战：点云变换的代码实现

说了这么多理论，咱们直接上代码。下面是用Python实现点云刚体变换的示例：

import numpy as np

def transform_point_cloud(points, R, t):
    """
    对点云进行刚体变换
    points: Nx3 的点云数组
    R: 3x3 旋转矩阵
    t: 3x1 平移向量
    """
    # 方法1：直接计算
    transformed = (R @ points.T).T + t.reshape(1, 3)
    
    # 方法2：使用齐次坐标（推荐）
    # 构建4x4变换矩阵
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t.flatten()
    
    # 转成齐次坐标
    ones = np.ones((points.shape[0], 1))
    homo_points = np.hstack([points, ones])
    
    # 变换
    transformed_homo = (T @ homo_points.T).T
    transformed = transformed_homo[:, :3]
    
    return transformed

# 示例：绕Z轴旋转45度，平移(1,2,3)
theta = np.pi / 4
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])

# 生成测试点云
points = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]])

# 变换
transformed = transform_point_cloud(points, R, t)
print("原始点云:\n", points)
print("变换后点云:\n", transformed)

4.7 不同表示方式的相互转换

实际开发中，经常需要在旋转矩阵、四元数、欧拉角之间来回转换。这里给出常用的转换公式：

转换方向	方法
旋转矩阵 → 四元数	从矩阵的迹和元素值推导，注意处理特殊情况
四元数 → 旋转矩阵	直接用四元数元素构造矩阵
欧拉角 → 旋转矩阵	按顺序连乘三个基本旋转矩阵
旋转矩阵 → 欧拉角	从矩阵元素反解角度，注意万向锁判断

我个人建议直接用现成的库，比如scipy.spatial.transform或者Eigen（C++）。手写转换容易出bug，尤其是处理边界情况的时候。

嗯，坐标变换这块内容就讲到这里。记住一点：理解物理意义比死记公式更重要。你只要想清楚「点要从哪个坐标系转到哪个坐标系」，剩下的就是套公式的事。

---

公众号:蓝海资料掘金营，微信deep3321