2. 坐标系与变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换关系，齐次坐标与刚体变换
做外参标定，说白了就是在玩坐标系之间的“翻译”游戏。
你想想看，相机看到的是一堆像素点，而机器人或者自动驾驶车需要知道这些点在世界里的真实位置。这中间差了四层坐标系要打通。我刚开始接触这块时，总觉得公式太多、绕得头晕。后来发现，只要把每个坐标系当成一个“视角”，问题就简单多了。
2.1 四个坐标系，一个故事
我们先捋清楚这四个坐标系分别长什么样：
- 世界坐标系 (World Coordinate System)：你选的一个固定参考系。比如房间的墙角、路边的电线杆。单位是米。
- 相机坐标系 (Camera Coordinate System)：以相机光心为原点，Z轴指向镜头前方。单位也是米。
- 图像坐标系 (Image Coordinate System)：在成像平面上，原点在光轴与平面的交点（主点）。单位是毫米。
- 像素坐标系 (Pixel Coordinate System)：就是图像数组的行列索引。原点在左上角，单位是像素。
嗯，这里要注意：世界坐标系到相机坐标系，是刚体变换（旋转+平移）。相机坐标系到图像坐标系，是透视投影（针孔模型）。图像坐标系到像素坐标系，是离散化缩放加平移。
核心一句话：外参标定要解的，就是世界坐标系到相机坐标系的那个旋转矩阵R和平移向量t。
2.2 齐次坐标：为什么我们需要它？
你可能会问：为什么非要用齐次坐标？直接用三维坐标不行吗？
我举个例子。假设你要把世界点P_w先旋转再平移，得到相机坐标P_c。用普通三维坐标写出来是：
P_c = R * P_w + t这看起来还行。但如果连续做多次变换，比如从世界到相机、相机到图像、图像到像素，每次都要写一个加法一个乘法，公式会变得又臭又长。
齐次坐标的妙处在于：把平移也塞进矩阵里。这样所有变换都统一成矩阵乘法。
三维点的齐次坐标是 (x, y, z, 1)。平移向量t变成矩阵的第四列。于是上面的变换变成：
| P_c_x | | R11 R12 R13 t1 | | P_w_x |
| P_c_y | = | R21 R22 R23 t2 | * | P_w_y |
| P_c_z | | R31 R32 R33 t3 | | P_w_z |
| 1 | | 0 0 0 1 | | 1 |你看，一个矩阵搞定旋转和平移。我个人习惯把所有变换都写成4x4矩阵，这样代码里就是一连串的矩阵乘法，清晰又好调试。
小技巧：齐次坐标的最后一维是1表示点，是0表示方向向量。方向向量不会被平移影响——这个细节在计算光心方向时特别有用。
2.3 刚体变换：旋转矩阵与平移向量
刚体变换，说白了就是“拿着一个物体，不捏变形，只转一转、挪一挪”。
旋转矩阵R有三个自由度（绕X、Y、Z轴旋转），但9个元素之间必须满足正交约束（R^T * R = I，行列式为+1）。我刚开始写代码时，直接优化9个元素，结果出来的矩阵根本不是旋转矩阵。后来改用李代数（so3）表示旋转，才彻底解决了这个问题。
平移向量t就是三个方向的位移，简单直接。
完整的刚体变换矩阵T（4x4）长这样：
T = | R t |
| 0 1 |逆变换也很常用：
T_inv = | R^T -R^T * t |
| 0 1 |避坑指南：我曾经在项目中直接用R的转置当逆矩阵用，结果算出来的坐标全反了。记住：旋转矩阵的逆才是转置，但平移部分必须跟着转回去。别偷懒，老老实实算T_inv。
2.4 从世界到像素：完整的投影管线
把上面所有步骤串起来，一个世界点P_w变成像素坐标(u, v)的完整流程是：
- 世界 → 相机：P_c = T_wc * P_w（刚体变换）
- 相机 → 图像：x = f * P_c_x / P_c_z, y = f * P_c_y / P_c_z（针孔投影）
- 图像 → 像素：u = x / dx + u0, v = y / dy + v0（离散化）
写成矩阵形式，就是内参矩阵K和外参矩阵[R|t]的乘积：
| u | | fx 0 u0 0 | | R11 R12 R13 t1 | | P_w_x |
| v | = | 0 fy v0 0 | * | R21 R22 R23 t2 | * | P_w_y |
| 1 | | 0 0 1 0 | | R31 R32 R33 t3 | | P_w_z |
| 0 0 0 1 | | 1 |这个公式，就是整个视觉SLAM和3D重建的基石。你想想看，所有标定、跟踪、建图，最终都在解这个方程里的未知数。
2.5 本章知识体系
下面这张图，是我自己画的一个结构图，帮你把四个坐标系和变换关系串起来：
2.6 避坑与经验
最后分享几个我踩过的坑：
- 坐标系方向别搞反：相机坐标系Z轴朝前，Y轴朝下（图像行方向）。我见过有人把Y轴朝上，结果投影出来的点全反了。
- 齐次坐标归一化：做完矩阵乘法后，记得把最后一维归一化成1。否则你拿到的坐标是缩放过的。
- 内参和外参别混用：内参是相机自己的属性（焦距、主点），外参是相机在世界里的位姿。标定时要分开优化。
我的习惯：在代码里，我会把世界坐标系原点选在标定板的第一个角点上。这样外参的物理意义特别清晰——相机相对于标定板的位姿。调试时一眼就能看出旋转和平移是否合理。
好了，坐标系和变换这部分就聊到这儿。记住：四个坐标系、一个齐次坐标、一个刚体变换矩阵。把这些吃透了，后面的标定和跟踪就是水到渠成的事。