坐标系与空间变换：从世界到像素的奇幻漂流
做视觉定位这些年，我最大的感触就是——坐标系这东西，看着简单，但真上手就全是坑。你想想看，一个三维点从真实世界跑到相机照片上，中间要经历多少次“变形记”？今天我们就来捋一捋这条路径上的四个关键坐标系，以及它们之间的变换关系。
四个坐标系，一个故事
先给个全景图。一个空间点要变成像素，得走这么一条路：
说白了，这就是一个从三维到二维的降维打击过程。每走一步，信息就丢一点。嗯，这里要注意——我们做SLAM的核心工作之一，就是想办法从二维图像里反推三维信息。
世界坐标系与相机坐标系：刚体变换
世界坐标系是我们定义场景的“绝对参考系”。你随便找个墙角当原点，X轴朝东，Y轴朝北，Z轴朝天——好了，这就是你的世界坐标系。
相机坐标系呢？它以相机光心为原点，Z轴指向相机前方，X轴向右，Y轴向下（这个跟图像坐标系对齐）。
同一个点，在两个坐标系下的坐标不一样。怎么转换？用刚体变换。
刚体变换 = 旋转 + 平移
一个物体在空间中移动，如果不发生形变，那就是刚体变换。它有6个自由度：3个旋转角度 + 3个平移量。
数学表达很简单：
Pc = R * Pw + t其中Pw是世界坐标，Pc是相机坐标，R是3x3旋转矩阵，t是3x1平移向量。
我个人习惯用齐次坐标来写，这样能把旋转和平并到一个矩阵里：
| Pc | = | R t | * | Pw |
| 1 | | 0 1 | | 1 |这个4x4矩阵就是传说中的外参矩阵。我在项目里调试标定结果时，经常盯着这个矩阵看——如果旋转矩阵的行列式不是1，那肯定标错了。
小技巧：旋转矩阵R是正交矩阵，满足R^T * R = I，且det(R)=1。如果你算出来的R不满足这个性质，说明数值计算有问题，或者标定板没放平。
齐次坐标：为什么需要它？
你可能会问：为什么非要用齐次坐标？直接用Pc = R*Pw + t不也挺好吗？
嗯，这里有个实际问题。假设你要连续做两次变换：先转成相机坐标，再投影到图像平面。用普通坐标，你得写：
Pc = R1 * Pw + t1
p = K * Pc用齐次坐标，一次矩阵乘法搞定：
p = K * [R1 | t1] * Pw_homogeneous说白了，齐次坐标就是把平移“伪装”成线性变换。这样多个变换就能串联成矩阵链，计算效率高得多。我在写SLAM前端时，所有位姿都用齐次矩阵表示，代码清爽不少。
注意：齐次坐标的最后一个分量是1。如果你算出来不是1，记得归一化。我曾经在优化过程中忘了归一化，结果整个地图飘到天上去——排查了整整一个下午。
相机坐标系到图像坐标系：投影变换
这一步是真正的“降维打击”。三维点(Xc, Yc, Zc)投影到成像平面上，变成二维点(x, y)。
根据小孔成像模型：
x = f * Xc / Zc
y = f * Yc / Zc其中f是焦距。注意这里有个除法，所以投影变换是非线性的。这也是为什么SLAM里处理投影时要特别小心——深度信息就在这个除法里丢掉了。
用齐次坐标写：
| x | = | f 0 0 0 | * | Xc |
| y | | 0 f 0 0 | | Yc |
| 1 | | 0 0 1 0 | | Zc |
| 1 |这个3x4矩阵就是投影矩阵的核心部分。实际相机还要考虑主点偏移和像素缩放，所以完整的投影矩阵会更复杂一些。
图像坐标系到像素坐标系：离散化
图像坐标系是连续的，单位是毫米。像素坐标系是离散的，单位是像素。怎么转？
u = x / dx + u0
v = y / dy + v0其中dx、dy是每个像素的物理尺寸，u0、v0是主点（光轴与成像平面交点）的像素坐标。
写成矩阵形式：
| u | = | 1/dx 0 u0 | * | x |
| v | | 0 1/dy v0 | | y |
| 1 | | 0 0 1 | | 1 |把前面两步合并，就得到了完整的内参矩阵K：
K = | fx 0 u0 |
| 0 fy v0 |
| 0 0 1 |其中fx = f/dx，fy = f/dy。这就是相机标定要算的核心参数。
完整投影公式（从世界坐标到像素）：
| u | | Xw |
| v | = K * [R | t] * | Yw |
| 1 | | Zw |
| 1 |K是内参，[R|t]是外参。内外参一乘，世界点就变成了像素点。
避坑指南：我踩过的那些坑
- 坐标系方向搞反：我曾经把相机坐标系的Y轴方向搞反了，结果重建出来的点云全是镜像的。检查了三天才发现是标定时坐标系定义不一致。
- 齐次坐标忘记归一化：优化过程中齐次坐标的最后一个分量会漂移，必须定期归一化。不然地图会慢慢变形。
- 内参矩阵的fx和fy搞混：OpenCV和MATLAB的标定结果里，fx和fy的顺序可能不一样。我建议拿到标定结果后，先投影几个点验证一下。
- 旋转矩阵的数值稳定性：长时间优化后，旋转矩阵可能不再满足正交性。我习惯每迭代几步就做一次SVD分解，强制正交化。
总结一下
四个坐标系，三次变换，一条流水线。世界坐标→相机坐标（刚体变换）→图像坐标（投影）→像素坐标（离散化）。
做SLAM说白了就是反复在这几个坐标系之间来回跳转。前端特征匹配要投影，后端优化要反投影，回环检测要算相似变换——每一步都离不开这些基础变换。
我个人建议：刚开始学的时候，别急着上复杂的SLAM框架。先拿几组标定数据，手写一遍从世界坐标到像素坐标的完整流程。把每个矩阵的维度、每个除法的含义都搞清楚。这个基本功打扎实了，后面学任何SLAM算法都会轻松很多。