4、点云坐标系与变换：坐标系定义（世界坐标系、传感器坐标系、车身坐标系）、刚体变换（旋转矩阵、平移向量、齐次坐标）、欧拉角与四元数、坐标变换实战（Open3D实现）

点云处理里，坐标系变换是绕不开的基础。说白了，你拿到的每一帧点云，都是传感器在某个瞬间“看到”的世界。但传感器装在车上、车上又有自己的坐标系，最终还得映射到真实世界里去。这一节，我就带你把这些坐标系和变换彻底捋清楚。

4.1 坐标系定义：三个核心坐标系

做点云融合、SLAM或者目标检测，你至少得跟三个坐标系打交道。我个人习惯把它们分成三层：传感器层、载体层、全局层。

4.1.1 世界坐标系（World Coordinate System）

世界坐标系是绝对参考系。它固定在大地上，不随车辆移动而改变。通常我们用东北天（ENU）或者北东地（NED）来定义。比如，你用一个RTK-GPS定位，输出的经纬高最终会投影到某个局部平面坐标系下，那就是世界坐标系。

关键点：世界坐标系是所有数据的最终归宿。多帧点云拼接、地图构建，最终都要统一到世界坐标系下。

4.1.2 传感器坐标系（Sensor Coordinate System）

每个传感器都有自己的“眼睛”。激光雷达的坐标系原点在激光头中心，通常定义X轴向前、Y轴向左、Z轴向上（不同厂家有差异，比如Velodyne是Z轴向上，但有些是Z轴向下）。

相机坐标系则是光心为原点，Z轴指向物体方向。嗯，这里要注意：激光雷达和相机的外参标定，本质上就是求传感器坐标系到车身坐标系的变换矩阵。

⚠️ 避坑指南： 我曾经在项目里吃过亏——不同厂家的激光雷达坐标系定义不一样。比如RoboSense和Hesai的坐标系方向就不同。如果你直接拿过来用，不做坐标系对齐，点云会完全错位。所以拿到新雷达，第一件事就是看手册里的坐标系定义。

4.1.3 车身坐标系（Vehicle/Body Coordinate System）

车身坐标系以车辆中心为原点，通常X轴指向车头、Y轴指向左侧、Z轴指向上方。所有传感器都安装在车身上，所以传感器坐标系到车身坐标系的变换是固定的（外参）。

你想想看，自动驾驶里做障碍物检测，最终输出的目标位置是在车身坐标系下的。然后结合车辆自身的位姿（来自IMU+GPS），才能转换到世界坐标系。

4.2 刚体变换：旋转矩阵、平移向量、齐次坐标

坐标系之间的变换，本质上是刚体变换——只旋转和平移，不改变形状。我刚开始学的时候，总觉得旋转矩阵很抽象。后来发现，你把它当成一个“旋转操作符”就好。

4.2.1 旋转矩阵（Rotation Matrix）

旋转矩阵是一个3x3的正交矩阵，行列式为1。它描述了从一个坐标系到另一个坐标系的旋转关系。比如，点P在坐标系A下的坐标是P_A，在坐标系B下的坐标是P_B，那么：

P_B = R * P_A

其中R就是旋转矩阵。R的每一列，其实是坐标系A的基向量在坐标系B下的表示。

个人经验： 我建议你记住一个性质——旋转矩阵的逆等于它的转置。这在代码里非常有用，因为求逆比求转置慢得多。

4.2.2 平移向量（Translation Vector）

平移向量t是一个3x1的向量，表示坐标系A的原点在坐标系B下的位置。完整的刚体变换是：

P_B = R * P_A + t

4.2.3 齐次坐标（Homogeneous Coordinates）

为什么要有齐次坐标？因为上面的公式里，旋转和平移是分开写的。如果你连续做多次变换，公式会变得很啰嗦。齐次坐标把旋转和平移合并到一个4x4的变换矩阵里：

| P_B |   =   | R   t |   *   | P_A |
|  1  |       | 0   1 |       |  1  |

这样，多次变换就可以直接矩阵相乘了。我在做多传感器融合时，经常用齐次矩阵把激光雷达、相机、IMU的变换串起来，代码非常简洁。

4.3 欧拉角与四元数

旋转矩阵有9个参数，但自由度只有3。所以实际应用中，我们更喜欢用欧拉角或四元数来表示旋转。

4.3.1 欧拉角（Euler Angles）

欧拉角用三个角度表示旋转：绕X轴旋转（roll，翻滚角）、绕Y轴旋转（pitch，俯仰角）、绕Z轴旋转（yaw，偏航角）。

但是，欧拉角有个大坑——万向锁（Gimbal Lock）。 当pitch接近±90°时，roll和yaw会变得无法区分。我记得有一次做无人机点云建图，飞机俯仰过大，欧拉角直接失效，点云全飞了。从那以后，我只要涉及连续旋转，一律用四元数。

4.3.2 四元数（Quaternions）

四元数用四个参数表示旋转：q = w + xi + yj + zk，其中w是实部，x、y、z是虚部。它没有万向锁问题，而且插值平滑（比如SLAM里的姿态插值）。

你可能会问：四元数怎么用？其实在代码里，你不需要手算四元数乘法，直接用库函数就行。但你要理解它的物理意义：四元数本质上是一个单位向量（旋转轴）加上一个旋转角度。

💡 实用技巧： 我个人习惯用四元数做旋转，用欧拉角做可视化。因为欧拉角直观，调试时一眼就能看出车头朝向。但内部计算一定用四元数，避免万向锁。

4.4 坐标变换实战：Open3D实现

理论说完了，咱们直接上代码。Open3D里处理点云变换非常方便。

4.4.1 加载点云并可视化

import open3d as o3d
import numpy as np

# 加载点云
pcd = o3d.io.read_point_cloud("example.pcd")
print(f"点云点数: {len(pcd.points)}")

# 可视化原始点云
o3d.visualization.draw_geometries([pcd], window_name="原始点云")

4.4.2 构建变换矩阵并应用

# 定义旋转矩阵（绕Z轴旋转45度）
theta = np.radians(45)
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta),  np.cos(theta), 0],
    [0,              0,             1]
])

# 定义平移向量（沿X轴移动1米）
t = np.array([1.0, 0.0, 0.0])

# 构建齐次变换矩阵
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = t

# 应用变换
pcd_transformed = pcd.transform(T)

# 可视化变换后的点云
o3d.visualization.draw_geometries([pcd_transformed], window_name="变换后点云")

4.4.3 使用四元数进行旋转

from scipy.spatial.transform import Rotation as R

# 定义四元数 [x, y, z, w]
quat = [0.0, 0.0, np.sin(theta/2), np.cos(theta/2)]
rot = R.from_quat(quat)
R_matrix = rot.as_matrix()

# 构建变换矩阵
T_quat = np.eye(4)
T_quat[:3, :3] = R_matrix
T_quat[:3, 3] = t

# 应用
pcd_quat = pcd.transform(T_quat)
🔑 核心要点：
  • Open3D的transform()函数直接接受4x4齐次矩阵
  • 旋转矩阵和四元数可以互相转换，用scipy的Rotation类最方便
  • 变换是原地操作（in-place），如果想保留原始点云，记得先copy()

4.4.4 多坐标系变换示例

假设你有一个激光雷达点云，需要先变换到车身坐标系，再变换到世界坐标系：

# 传感器到车身的外参
T_sensor_to_vehicle = np.array([[...]])  # 4x4矩阵

# 车身到世界的外参（来自定位模块）
T_vehicle_to_world = np.array([[...]])   # 4x4矩阵

# 链式变换
T_sensor_to_world = T_vehicle_to_world @ T_sensor_to_vehicle

# 应用
pcd_world = pcd.transform(T_sensor_to_world)

你看，用齐次矩阵做链式变换，代码非常干净。我实际项目中，会把所有传感器的外参存成一个字典，然后统一乘上当前帧的位姿，一步到位转到世界坐标系。

⚠️ 注意： 矩阵乘法的顺序很重要！T_sensor_to_world = T_vehicle_to_world @ T_sensor_to_vehicle，这个顺序表示“先做传感器到车身的变换，再做车身到世界的变换”。顺序反了，点云会飞到天上去。

好了，这一节的内容就到这里。坐标系变换是点云处理的基石，你花时间把它吃透，后面做多传感器融合、SLAM建图都会轻松很多。下一节，我们聊聊点云滤波——怎么把噪点去掉，保留有用的信息。

← 上一章 📖 返回目录 下一章 →