2、坐标系与空间变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系；刚体变换与齐次坐标

好，咱们进入第二章。这一章说实话，是很多做视觉追踪的朋友容易栽跟头的地方。坐标系搞混了，后面所有算法都是白搭。我当年刚入行时，就因为在像素坐标系和图像坐标系之间少乘了一个内参，结果追踪的轨迹全飘到屏幕外面去了……嗯，从那以后，我对坐标系变换就格外上心。

2.1 四个坐标系，一个都不能少

在动态障碍物追踪里，我们其实是在四个不同的坐标系之间来回穿梭。你想想看，障碍物在真实世界里有个位置，相机拍到了它，图像上有个点，最后显示在屏幕上又是一个像素坐标。这中间每一步，都是一次坐标变换。

我个人习惯把这四个坐标系按「从大到小」的顺序来记：

  1. 世界坐标系：真实世界的三维空间，单位是米。这是所有物体的「绝对位置」。
  2. 相机坐标系：以相机光心为原点，Z轴指向镜头前方。说白了，就是把世界坐标「搬到」相机跟前来看。
  3. 图像坐标系：在成像平面上，单位是毫米。这是物理世界在相机内部的投影。
  4. 像素坐标系：最终屏幕上看到的，单位是像素。图像坐标系经过缩放和偏移，就变成了像素坐标。

核心要点：世界→相机是刚体变换（旋转+平移），相机→图像是透视投影，图像→像素是仿射变换（缩放+平移）。每一步都有对应的矩阵运算。

2.2 刚体变换与齐次坐标

刚体变换，说白了就是「物体本身不变，只是换个位置和姿势」。在三维空间里，一个刚体有6个自由度：3个旋转（绕X、Y、Z轴）和3个平移（沿X、Y、Z方向）。

为什么需要齐次坐标？因为平移和旋转在普通坐标系里没法统一成一个矩阵。旋转是乘法，平移是加法，混在一起写很麻烦。齐次坐标的妙处在于——给三维坐标加一个维度（变成4维），这样旋转和平移就能合并成一个4x4的矩阵了。

我记得有一次调试一个多相机融合的项目，就是因为忘了把平移向量塞进齐次矩阵里，结果两个相机的数据怎么都对不齐。后来花了整整一个下午才找到这个bug……

2.3 坐标变换的数学表达

咱们直接上公式。假设世界坐标系中有一个点 Pw = (Xw, Yw, Zw)T，要变换到相机坐标系 Pc = (Xc, Yc, Zc)T：

P_c = R * P_w + t

其中 R 是3x3旋转矩阵，t 是3x1平移向量。用齐次坐标写就是：

[P_c]   = [R  t] * [P_w]
[1  ]     [0  1]   [1  ]

这个4x4矩阵就是刚体变换矩阵，通常记作 T。你想想看，有了这个矩阵，连续变换就变成了矩阵乘法，多清爽。

2.4 从相机坐标到像素坐标

这一步是透视投影加仿射变换。相机坐标系下的点 (Xc, Yc, Zc) 投影到图像平面：

x = f * X_c / Z_c
y = f * Y_c / Z_c

然后从图像坐标 (x, y) 到像素坐标 (u, v)：

u = x / dx + u0
v = y / dy + v0

其中 dx, dy 是每个像素的物理尺寸，u0, v0 是图像中心在像素坐标系中的位置。把这两步合并，就是相机内参矩阵 K：

[u]   [f/dx  0    u0] [X_c]
[v] = [0    f/dy  v0] [Y_c]
[1]   [0     0    1 ] [Z_c]

我的小技巧：实际写代码时，我习惯把内参矩阵 K 和刚体变换矩阵 T 分开定义。这样调试时，可以单独验证投影是否正确，再验证外参是否准确。一步到位反而容易出问题。

2.5 避坑指南：我踩过的几个坑

  • 坐标系方向搞反：我曾经在定义相机坐标系时，把Z轴方向搞反了（指向了相机后方），结果所有障碍物的深度都是负数。嗯，这个问题排查了整整两天。
  • 旋转矩阵不满足正交性：手动构造旋转矩阵时，一定要检查 R*RT = I 和 det(R) = 1。否则变换出来的坐标会变形。
  • 齐次坐标忘记归一化：从齐次坐标转回普通坐标时，要除以最后一个分量。我见过不少新手直接取前三个分量，结果坐标全错了。

重要提醒：在动态障碍物追踪中，相机本身也在运动。所以每一帧的刚体变换矩阵 T 都可能不同。千万别用上一帧的 T 去变换当前帧的障碍物坐标，否则追踪轨迹会严重漂移。

2.6 知识体系结构图

下面这张图展示了四个坐标系之间的变换关系，以及每一步对应的数学操作。我建议你把它打印出来贴在工位上，写代码时对照着看，能省不少事。

世界坐标系 相机坐标系 图像坐标系 像素坐标系 刚体变换 R, t → 4x4矩阵 透视投影 f, Z_c 仿射变换 dx, dy, u0, v0 内参矩阵 K (3x3) 外参矩阵 T (4x4) 图：四个坐标系之间的变换关系

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

光说不练假把式。下面是我写的一个小工具函数，用来做世界坐标到像素坐标的完整变换。代码不长，但每一步都有注释，你直接拿去用就行。

import numpy as np

def world_to_pixel(P_w, R, t, K):
    """
    世界坐标 → 像素坐标
    P_w: 世界坐标系下的点 (3,)
    R:   旋转矩阵 (3,3)
    t:   平移向量 (3,)
    K:   内参矩阵 (3,3)
    """
    # 1. 世界 → 相机（刚体变换）
    P_c = R @ P_w + t
    
    # 2. 相机 → 像素（投影 + 仿射）
    # 注意：这里用齐次坐标一步到位
    P_c_h = np.append(P_c, 1)  # 转齐次
    pixel_h = K @ P_c_h[:3]    # 内参投影
    u = pixel_h[0] / pixel_h[2]
    v = pixel_h[1] / pixel_h[2]
    
    return np.array([u, v])

# 举个栗子
R = np.eye(3)  # 假设相机没有旋转
t = np.array([0, 0, 5])  # 相机在世界坐标系中沿Z轴偏移5米
K = np.array([[500, 0, 320],
              [0, 500, 240],
              [0, 0, 1]])

P_w = np.array([1, 2, 3])  # 世界坐标系中的障碍物位置
pixel = world_to_pixel(P_w, R, t, K)
print(f"像素坐标: ({pixel[0]:.2f}, {pixel[1]:.2f})")

个人经验：实际项目中，R 和 t 通常来自相机标定或者SLAM系统。我建议你每次拿到新的R和t，先用一个已知坐标的点验证一下，确保变换正确再往下走。这一步花不了1分钟，但能省下几小时的调试时间。

2.8 小结

这一章我们聊了四个坐标系的关系，刚体变换和齐次坐标的数学本质，以及如何用代码实现坐标变换。说白了，坐标系变换就是「把障碍物在真实世界的位置，一步步映射到屏幕上」。每一步都有明确的数学公式，只要按部就班，就不会出错。

嗯，我记得刚学这部分时，总觉得矩阵运算很抽象。后来做项目多了才发现，其实每个矩阵都有物理意义——旋转矩阵就是「相机怎么转」，平移向量就是「相机在哪」，内参矩阵就是「镜头怎么成像」。理解了这些，公式就不再是死记硬背了。

可可精品资料库

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