坐标系基础：世界坐标系、相机坐标系、图像坐标系、像素坐标系、刚体变换与齐次坐标
做多传感器标定，说白了就是搞清楚「每个传感器在哪儿、朝哪儿看」。
我刚开始接触这行时，觉得坐标系就是个数学概念，没什么大不了的。直到第一次做激光雷达和相机的联合标定，数据怎么都对不齐，折腾了两天才发现——哦，原来是我把图像坐标系和像素坐标系搞混了。
嗯，从那以后，我再也不敢小看坐标系了。
1. 四个坐标系，一个都不能少
多传感器标定里，我们打交道最多的就是这四个坐标系。它们之间层层递进，像一条流水线。
核心逻辑：世界坐标系 → 相机坐标系 → 图像坐标系 → 像素坐标系
每一步都是一个变换，每一步都有物理意义。
1.1 世界坐标系 (World Coordinate System)
这是「绝对」的参考系。你可以把它理解为整个场景的「大地基准」。我习惯把世界坐标系的原点放在标定板的第一个角点上，或者放在机器人底盘的几何中心。
单位：米（m）
坐标表示：(Xw, Yw, Zw)
1.2 相机坐标系 (Camera Coordinate System)
以相机光心为原点，Z轴指向相机前方（也就是拍摄方向），X轴向右，Y轴向下。注意，这个Y轴方向和图像坐标系是反的，很多新手在这里栽跟头。
单位：米（m）
坐标表示：(Xc, Yc, Zc)
我的小技巧： 调试时，把相机坐标系下的点云投影到图像上看看。如果投影结果上下颠倒，十有八九是Y轴方向搞反了。
1.3 图像坐标系 (Image Coordinate System)
这是一个二维坐标系，原点在图像平面的中心（也就是主点位置），X轴向右，Y轴向上。
单位：毫米（mm）
坐标表示：(x, y)
1.4 像素坐标系 (Pixel Coordinate System)
这就是我们最终看到的图像。原点在图像的左上角，u轴向右，v轴向下。
单位：像素（pixel）
坐标表示：(u, v)
你想想看，从物理世界到数字图像，经历了三次变换。每一次变换都有对应的数学表达。
2. 刚体变换：旋转和平移
刚体变换，说白了就是「不改变物体形状的移动」。你拿一本书，可以平移，可以旋转，但书不会变形。
在标定中，我们经常需要把一个坐标系下的点变换到另一个坐标系下。比如，把激光雷达检测到的障碍物位置，变换到相机坐标系下，再投影到图像上。
刚体变换的数学形式很简单：
P_c = R * P_w + t其中：
- Pc 是相机坐标系下的点
- Pw 是世界坐标系下的点
- R 是旋转矩阵（3x3）
- t 是平移向量（3x1）
旋转矩阵R有9个元素，但自由度只有3。为什么？因为旋转矩阵是正交矩阵，行列式为1。这个约束条件让R的9个元素之间相互关联。
我曾经踩过的坑： 旋转矩阵的乘法顺序很重要！R1 * R2 和 R2 * R1 结果完全不同。我习惯先旋转再平移，也就是 P_c = R * P_w + t。如果你先平移再旋转，公式就变成了 P_c = R * (P_w + t)，结果完全不一样。
3. 齐次坐标：让变换变得优雅
刚体变换的公式 P_c = R * P_w + t 看起来简单，但用起来很麻烦。为什么？因为旋转是乘法，平移是加法，混在一起不好处理。
齐次坐标就是来解决这个问题的。
做法很简单：给三维点加一个维度，变成四维向量。
P_w (齐次) = [X_w, Y_w, Z_w, 1]^T
P_c (齐次) = [X_c, Y_c, Z_c, 1]^T然后，刚体变换就变成了一个矩阵乘法：
P_c = T * P_w
其中 T = [R t]
[0 1]T 是一个 4x4 的变换矩阵。你看，旋转和平移被统一到了一个矩阵里。这就是齐次坐标的魅力。
为什么用齐次坐标？
- 统一了旋转和平移，一个矩阵搞定
- 可以方便地串联多个变换：T总 = T1 * T2 * T3
- 在SLAM后端优化中，齐次变换矩阵的求逆也很方便
4. 从世界到像素：完整的投影过程
现在我们把所有东西串起来。一个世界坐标系下的点，是怎么变成图像上的像素的？
整个过程分三步：
- 世界 → 相机： Pc = Tcw * Pw （刚体变换）
- 相机 → 图像： 归一化平面投影，x = Xc / Zc, y = Yc / Zc
- 图像 → 像素： u = fx * x + cx, v = fy * y + cy
其中 fx, fy 是焦距（单位：像素），cx, cy 是主点坐标。
写成矩阵形式就是：
s * [u, v, 1]^T = K * [R | t] * [X_w, Y_w, Z_w, 1]^TK 是相机内参矩阵，[R | t] 是外参矩阵。s 是尺度因子。
我的经验： 在实际项目中，我很少手动推导这些公式。但我会在代码里写一个函数，专门做「世界坐标到像素坐标」的转换。调试时，把标定板的角点投影到图像上，看看投影点和检测点是否重合。如果偏差超过2个像素，说明标定参数有问题。
5. 知识体系总览
下面这张图，是我自己总结的坐标系变换全流程。你看一遍，应该就能记住整个脉络。
6. 实际项目中的坐标系管理
在真正的多传感器标定项目中，我一般会这样做：
- 统一世界坐标系： 所有传感器都变换到同一个世界坐标系下。我习惯用第一个相机或者IMU作为基准。
- 维护变换树： 用ROS的tf树或者自己写一个变换管理器。每个传感器都有一个相对于世界坐标系的变换矩阵。
- 验证投影： 标定完成后，一定要做投影验证。把3D点投影到2D图像上，看看误差有多大。
注意： 不同传感器的时间戳要对齐。如果时间不同步，坐标系变换再准也没用。我见过一个项目，标定参数明明是对的，但融合结果就是不对，最后发现是时间戳差了50毫秒。
好了，坐标系基础就讲到这里。这些概念虽然基础，但贯穿了整个多传感器标定流程。你想想看，如果没有齐次坐标，我们要怎么处理串联的变换？如果没有刚体变换，我们怎么把激光雷达的点云投影到图像上？
嗯，下一节我们会深入相机内参标定，到时候这些坐标系知识会反复用到。
公众号:蓝海资料掘金营，微信deep3321