1、坐标变换基础：为什么感知需要坐标变换？从欧几里得空间到齐次坐标

大家好，我是老张。今天咱们聊聊坐标变换。

说实话，我刚入行那会儿，觉得坐标变换就是个数学工具，没什么大不了的。直到有一次，我在做多传感器融合项目时，激光雷达和摄像头的数据怎么都对不上——明明前方有个行人，激光点云显示在左边，图像却显示在右边。折腾了两天，最后发现是外参标定矩阵写反了。嗯，从那以后，我再也不敢小看坐标变换了。

1.1 为什么感知离不开坐标变换？

你想想看，一辆自动驾驶汽车上，传感器少说也有七八个：

• 前视摄像头看到的是2D图像，像素坐标
• 激光雷达返回的是3D点云，传感器坐标系
• 毫米波雷达给出的是极坐标下的目标信息
• GPS/IMU提供的是大地坐标系下的位置

这些数据格式不同、原点不同、朝向不同。如果不做坐标变换，它们就是一堆各自为政的「方言」，根本没法交流。

我个人习惯把坐标变换分成三个层次：

1. 传感器内部变换：比如相机从像素坐标到归一化平面
2. 传感器间变换：把激光雷达的点云投影到图像上
3. 车体到世界变换：知道车在地图上的绝对位置

这三个层次，缺一个都不行。

1.2 从欧几里得空间说起

欧几里得空间，说白了就是我们熟悉的「三维世界」。一个点用 (x, y, z) 表示，旋转用矩阵，平移用向量。

举个例子，把点 P 从激光雷达坐标系转到车体坐标系：

P_car = R * P_lidar + t

其中 R 是3x3旋转矩阵，t 是3x1平移向量。看起来很简单对吧？

但问题来了——如果你要做连续变换呢？比如从激光雷达到车体，再到世界坐标系：

P_world = R2 * (R1 * P_lidar + t1) + t2

这式子越写越长，而且旋转和平移混在一起，代码写起来特别容易出错。我在项目中就踩过这个坑——括号位置写错，结果点云全飞到天上去了。

1.3 齐次坐标：一个优雅的解决方案

为什么会这样？因为旋转是线性变换，平移不是。它们俩「不兼容」。

齐次坐标的妙处在于：它把三维点 (x, y, z) 扩展成四维 (x, y, z, 1)。这样一来，平移也能写成矩阵乘法了。

看这个对比：

变换类型	欧几里得空间	齐次坐标
旋转	P' = R * P	P' = [R 0; 0 1] * P
平移	P' = P + t	P' = [I t; 0 1] * P
旋转+平移	P' = R*P + t	P' = [R t; 0 1] * P

你看，齐次坐标下，所有变换都统一成了4x4矩阵乘法。连续变换就是矩阵连乘：

P_world = T2 * T1 * P_lidar

简洁、优雅、不容易出错。我个人非常推荐在代码里统一用齐次坐标，哪怕你只做一次变换。

1.4 实战：用Python实现坐标变换

下面是我在实际项目中常用的代码模板。咱们以激光雷达到车体为例：

import numpy as np

def build_transform(R, t):
    """
    构建4x4齐次变换矩阵
    R: 3x3旋转矩阵
    t: 3x1平移向量
    """
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t.flatten()
    return T

def transform_points(points, T):
    """
    对点云做齐次坐标变换
    points: Nx3 的点云数组
    T: 4x4 变换矩阵
    """
    # 转成齐次坐标 (N, 4)
    ones = np.ones((points.shape[0], 1))
    homo_points = np.hstack([points, ones])
    
    # 变换
    transformed = (T @ homo_points.T).T
    
    # 转回三维坐标
    return transformed[:, :3]

# 示例：激光雷达到车体
R_lidar_to_car = np.eye(3)  # 假设无旋转
t_lidar_to_car = np.array([[0.5], [0.0], [0.3]])  # 车体后方0.5米，上方0.3米

T = build_transform(R_lidar_to_car, t_lidar_to_car)
points_lidar = np.array([[1.0, 0.0, 0.0], [2.0, 1.0, 0.5]])
points_car = transform_points(points_lidar, T)

print(points_car)
# 输出: [[1.5 0.  0.3]
#        [2.5 1.  0.8]]

这段代码我用了好几年，基本没出过问题。唯一要注意的是：矩阵乘法顺序。齐次坐标下，变换矩阵左乘点云，顺序不能搞反。

1.5 避坑指南

我曾经在项目里犯过一个低级错误：把旋转矩阵和平移向量的顺序搞反了。结果点云全部偏移了半米，排查了一整天。

这里给大家几个建议：

• 统一用齐次坐标，别混用欧几里得和齐次
• 写单元测试：用单位矩阵和零平移验证，看输出是否等于输入
• 可视化验证：把变换前后的点云画出来，一眼就能看出问题
• 注意坐标系定义：x向前、y向左、z向上？还是其他约定？一定要统一

1.6 本章知识体系

下面这张图总结了坐标变换的核心逻辑：

嗯，以上就是坐标变换的基础内容。说白了，它就是感知系统的「翻译官」——让不同传感器的数据能互相理解。齐次坐标让这个翻译过程变得简洁而优雅。

可可精品资料库