2. 坐标系与刚体变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系；旋转矩阵、平移向量、齐次坐标
好，咱们正式开始聊视觉定位里最基础、也最绕不开的一环——坐标系与刚体变换。
说实话，我刚开始做SLAM那会儿，最头疼的就是这几个坐标系来回倒腾。明明一个三维点，愣是在四个坐标系里转来转去，稍不留神就搞反了方向。后来我养成了一个习惯：拿到任何视觉数据，第一件事就是画坐标系转换链路图。这招帮我省了太多debug的时间。
2.1 四个坐标系，一个都不能少
视觉定位里，我们打交道的一共四个坐标系。它们之间的关系，说白了就是一条流水线：世界→相机→图像→像素。
核心链路：
世界坐标系 (3D) → 相机坐标系 (3D) → 图像坐标系 (2D) → 像素坐标系 (2D)
我习惯用下面这张图来理解整个流程，你仔细看看：
嗯，这张图我建议你保存下来。每次做定位误差分析时，对着它捋一遍，思路会清晰很多。
2.2 世界坐标系与相机坐标系——刚体变换
世界坐标系是我们定义场景的"绝对"参考系。相机坐标系呢？它以相机光心为原点，Z轴指向相机前方。
这两个坐标系之间是什么关系？刚体变换。说白了，就是旋转加平移，不改变物体的形状和大小。
刚体变换公式：
Pc = R * Pw + t其中：Pw 是世界坐标点，Pc 是相机坐标点，R 是3x3旋转矩阵，t 是3x1平移向量。
我记得有一次做视觉里程计，发现轨迹一直在漂。查了半天，原来是旋转矩阵的符号搞反了——我把从世界到相机的R写成了从相机到世界的R。你想想看，这一反，整个轨迹都镜像了。
旋转矩阵 R
旋转矩阵是个3x3的正交矩阵，满足 R^T * R = I，行列式为+1。它有3个自由度，对应绕X、Y、Z轴的三个旋转角。
绕Z轴旋转θ角的矩阵长这样：
Rz(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]绕X轴和Y轴类似，只是把sin和cos放到对应位置。实际中我们常用的是欧拉角或四元数来表示旋转，但最终都要转成矩阵形式参与运算。
我的小技巧： 调试时如果发现旋转结果不对，先检查旋转矩阵的行列式是不是1。如果不是，说明矩阵不是正交的，大概率是代码里数值精度出了问题。
平移向量 t
平移向量 t 表示世界坐标系原点在相机坐标系下的位置。注意方向——是从世界到相机的平移。
我曾经犯过一个低级错误：把平移向量直接当成了相机在世界坐标系下的位置。这两个是互为相反数的关系，千万别搞混了。
2.3 齐次坐标——让变换变得优雅
你发现没有？刚体变换公式 Pc = R * Pw + t 里，旋转是乘法，平移是加法。这种"混合运算"在连续变换时特别麻烦。
齐次坐标就是来解决这个问题的。它给三维点加一个维度，变成四维：
Pw_homogeneous = [Xw, Yw, Zw, 1]^T
Pc_homogeneous = [Xc, Yc, Zc, 1]^T然后刚体变换就变成了一个4x4矩阵：
[Xc] [R11 R12 R13 tx] [Xw]
[Yc] = [R21 R22 R23 ty] [Yw]
[Zc] [R31 R32 R33 tz] [Zw]
[1 ] [0 0 0 1 ] [1 ]写成简洁形式：Pc = T * Pw，其中T是4x4的变换矩阵。
为什么用齐次坐标？
- 把旋转和平移统一成矩阵乘法，方便连续变换
- 可以表示无穷远点（最后一个分量为0）
- 在SLAM后端优化中，便于求导和线性化
2.4 相机坐标系到图像坐标系——透视投影
这一步是把三维的相机坐标点投影到二维图像平面上。用的是小孔成像模型。
x = f * Xc / Zc
y = f * Yc / Zc其中 f 是焦距，(x, y) 是图像坐标系下的坐标，单位是毫米。
这里有个关键点：深度 Zc 在分母上。这意味着距离越远，同样的三维位移在图像上产生的位移越小。这就是透视效应的本质。
注意： 当 Zc = 0 时，投影无定义。实际中如果遇到 Zc 接近0的点，要么是点在相机后面，要么是标定板放得太近了。我在做近距离三维重建时就踩过这个坑，后来加了深度阈值过滤才解决。
2.5 图像坐标系到像素坐标系——离散化
图像坐标系是连续的，单位是毫米。像素坐标系是离散的，单位是像素。转换关系很简单：
u = x / dx + u0
v = y / dy + v0其中 dx、dy 是每个像素的物理尺寸，u0、v0 是主点（光轴与图像平面的交点）的像素坐标。
写成矩阵形式就是相机内参矩阵 K：
[u] [fx 0 u0] [x]
[v] = [0 fy v0] [y]
[1] [0 0 1] [1]其中 fx = f/dx，fy = f/dy，是焦距的像素表示。
避坑指南： 我曾经在标定时发现 fx 和 fy 差了好几个像素，一开始以为是标定板不平。后来才发现是相机传感器像素不是正方形。所以拿到相机先看传感器规格，别默认 dx = dy。
2.6 完整的投影方程
把上面所有步骤串起来，就是一个三维世界点到像素坐标的完整投影：
s * [u] [fx 0 u0 0] [R11 R12 R13 tx] [Xw]
[v] = [0 fy v0 0] [R21 R22 R23 ty] [Yw]
[1] [0 0 1 0] [R31 R32 R33 tz] [Zw]
[0 0 0 1 ] [1 ]简化写法：s * p = K * [R|t] * Pw
其中 s 是尺度因子（其实就是 Zc），K 是内参矩阵，[R|t] 是外参矩阵。
误差分析的关键：
- 外参 [R|t] 的误差 → 世界到相机的刚体变换不准
- 内参 K 的误差 → 焦距、主点标定不准
- 像素坐标 (u,v) 的误差 → 特征点提取精度有限
这三类误差叠加在一起，就是视觉定位误差的根源。
2.7 我的一点经验总结
做视觉定位这些年，我最大的体会是：坐标系转换看似简单，但90%的bug都出在这里。
给你几个实用建议：
- 画图：每次做新项目，先把坐标系转换链路图画出来，贴在工位上
- 验证：用已知点反算，检查投影结果对不对。我习惯在标定板上选几个角点，手动算一遍投影坐标，跟代码输出对比
- 统一：整个项目组用同一套坐标系约定。左手系还是右手系？Y轴向上还是Z轴向上？这些必须提前定好
- 记录：每次标定完，把内参、外参、重投影误差都记下来。方便后面排查问题
嗯，坐标系与刚体变换就聊到这儿。这部分内容虽然基础，但真的是视觉定位的基石。你把这些搞透了，后面学误差分析、补偿方法就会轻松很多。
公众号:蓝海资料掘金营，微信deep3321