📚 课程目录第2章坐标系统基础

坐标系统基础：世界坐标系、相机坐标系、图像坐标系、像素坐标系的定义与转换关系

做传感器融合，第一个绕不开的坎就是坐标系统。我记得刚入行那会儿，总觉得坐标转换不就是几个矩阵乘法嘛，结果第一次做激光雷达和相机的联合标定，数据怎么都对不齐。后来才发现，是我对这四个坐标系的定义理解得不够透彻。

说白了，传感器融合的本质，就是把不同传感器看到的「世界」统一到同一个坐标系下。你想想看，激光雷达告诉你「前方2米有个障碍物」，相机告诉你「图像第320列有个边缘」，这两句话用的语言完全不同。怎么让它们对上话？就是靠坐标转换。

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

我们先从最直观的开始，一个一个捋清楚。

坐标系	维度	单位	物理含义
世界坐标系	3D (Xw, Yw, Zw)	米/mm	真实世界的位置，所有传感器的参考基准
相机坐标系	3D (Xc, Yc, Zc)	米/mm	以相机光心为原点，Z轴指向镜头前方
图像坐标系	2D (x, y)	毫米	物理成像平面上的位置，光心投影为原点
像素坐标系	2D (u, v)	像素	图像数组中的行列索引，左上角为原点

嗯，这里要注意：世界坐标系是我们人为定义的。你可以把原点放在机器人底座中心，也可以放在房间的某个角落。我习惯把世界坐标系的原点放在机器人启动位置的地面上，这样Z轴朝上，方便做地面相关的计算。

世界坐标系 → 相机坐标系：刚体变换

这一步其实就是一个旋转加平移。相机在世界里有个位置和姿态，对吧？

公式很简单：

Pc = R * Pw + t

其中：

Pw：世界坐标系下的3D点
R：3×3旋转矩阵，描述相机的朝向
t：3×1平移向量，描述相机的位置
Pc：转换到相机坐标系下的3D点

我建议在实际工程中，用齐次坐标来写，这样旋转和平移可以合并成一个4×4矩阵：

[ Xc ]   [ R11 R12 R13 tx ] [ Xw ]
[ Yc ] = [ R21 R22 R23 ty ] [ Yw ]
[ Zc ]   [ R31 R32 R33 tz ] [ Zw ]
[ 1  ]   [   0   0   0  1 ] [ 1  ]

我的经验：旋转矩阵有9个参数，但自由度只有3。实际项目中我更喜欢用四元数或欧拉角来存储姿态，最后再转成旋转矩阵做计算。四元数做插值比欧拉角稳定得多，不会遇到万向锁。

相机坐标系 → 图像坐标系：透视投影

这一步是3D到2D的降维打击。相机坐标系下的点，通过小孔成像模型，投影到物理成像平面上。

公式：

x = f * Xc / Zc
y = f * Yc / Zc

其中 f 是相机的物理焦距（单位：毫米）。

为什么会除以Zc？因为近大远小啊。你想想看，一个物体离相机越远，它在成像平面上的投影就越小。这个除法就是透视投影的核心。

我曾经踩过的坑：Zc不能为0！如果物体正好在相机光心所在的平面上，Zc=0，除法就炸了。实际场景中这种情况很少见，但做仿真或者处理合成数据时一定要加保护判断。

图像坐标系 → 像素坐标系：离散化

图像坐标系是连续的物理尺寸（毫米），但像素坐标系是离散的整数索引。这一步就是把连续量变成离散量。

公式：

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

其中：

dx, dy：每个像素在x和y方向上的物理尺寸（毫米/像素）
(u0, v0)：图像坐标系原点在像素坐标系中的位置，通常是图像中心

说白了，就是问你：物理平面上的一个点，落在哪个像素格子里？

完整的转换链：从世界到像素

把上面三步串起来，就是完整的投影过程：

Zc * [ u ]   [ fx  0  u0  0 ] [ R t ] [ Xw ]
     [ v ] = [ 0  fy v0  0 ] [ 0 1 ] [ Yw ]
     [ 1 ]   [ 0   0   1  0 ]         [ Zw ]
                                       [ 1  ]

这个3×4矩阵，就是大名鼎鼎的相机投影矩阵P。它把世界坐标系下的3D点，直接映射到像素坐标。

其中：

fx = f / dx，fy = f / dy：以像素为单位的焦距
(u0, v0)：主点坐标
内参矩阵 K = [[fx, 0, u0], [0, fy, v0], [0, 0, 1]]
外参矩阵 [R | t]

  核心理解：相机标定就是求内参K和外参[R|t]。内参是相机的固有属性，出厂后基本不变；外参是相机在世界坐标系下的位姿，每次移动相机都要重新标定。

知识结构图：坐标转换全流程

逆变换：从像素到世界

实际项目中，我们经常需要做反向操作。比如，图像上检测到一个目标，想知道它在世界坐标系下的位置。

但这里有个问题：从像素到世界是欠约束的。一个像素点对应的是世界坐标系下的一条射线，而不是一个确定的点。

怎么解决？加约束条件。最常见的是假设目标在地面上（Zw=0），这样就能解出唯一的Xw和Yw。

公式推导：

1. 像素 → 归一化相机坐标：
   x_norm = (u - u0) / fx
   y_norm = (v - v0) / fy

2. 归一化坐标 → 相机坐标（假设Zc = s）：
   Xc = s * x_norm
   Yc = s * y_norm
   Zc = s

3. 相机坐标 → 世界坐标（利用地面约束Zw=0）：
   代入刚体变换的逆变换，解出s

实用技巧：如果你有深度传感器（比如双目相机或激光雷达），可以直接拿到Zc的值，就不需要地面假设了。我在做自动驾驶项目时，就是用激光雷达的点云给相机提供深度信息，这样每个像素都能对应到唯一的世界坐标点。

常见问题与避坑指南

坐标系方向搞反：相机坐标系Z轴朝前，Y轴朝下（图像坐标系Y轴朝下），这个和世界坐标系不一样。我见过有人把Y轴方向搞反，结果投影出来的点全在图像上方。
单位不统一：世界坐标系用米，相机内参用毫米，一乘就错。我建议所有数据统一用毫米，或者统一用米，别混着用。
畸变没校正：直接用原始像素坐标做投影，边缘区域误差很大。先做畸变校正，再做投影。
外参标定精度不够：旋转矩阵差1度，在10米外就是17厘米的误差。做传感器融合时，外参标定一定要做精细。

我曾经踩过的坑：有一次做多相机拼接，怎么都对不齐。查了两天，最后发现是其中一个相机的内参标定文件里，fx和fy写反了。从那以后，我每次拿到标定结果，都会先做一个简单的验证：把一个已知世界坐标的点投影到图像上，看看对不对得上。

坐标系统是传感器融合的基石。这四个坐标系之间的转换关系，说白了就是一套数学语言，让不同传感器能说同一种话。理解透了，后面的融合算法才能做得扎实。