3. 坐标系统与变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系，刚体变换与齐次坐标
做多传感器融合，说白了就是要把不同传感器看到的「世界」对齐到同一个坐标系下。激光雷达看到的是三维点云，相机看到的是二维图像，这两者怎么对应起来？嗯，核心就是坐标变换。
我记得刚入行那会儿，第一次做激光雷达和相机的联合标定，被四个坐标系绕得晕头转向。后来我总结了一个经验：你只要搞清楚「点」是怎么从真实世界一步步走到像素格子里的，这事就通了。
3.1 四个坐标系，一个链条
我们先捋一遍这四个坐标系的关系。它们其实是一条流水线：
- 世界坐标系：你定的一个全局参考系，单位是米。比如以车后轴中心为原点，X轴朝前，Y轴朝左，Z轴朝上。
- 相机坐标系：以相机光心为原点，Z轴指向镜头前方，单位也是米。
- 图像坐标系：在成像平面上，以光轴与平面的交点为原点，单位是毫米。
- 像素坐标系：在图像上，以左上角为原点，单位是像素。
链条是这样的：
世界坐标 → 相机坐标 → 图像坐标 → 像素坐标
每一步都有对应的变换矩阵。你想想看，一个三维空间里的点，经过四次变换，最终落在你屏幕上的某个像素位置。是不是挺神奇的？
3.2 刚体变换与齐次坐标
从世界坐标系到相机坐标系，这一步是刚体变换。说白了就是旋转加平移，物体本身不会变形，只是换个角度看。
刚体变换的数学表达是：
P_cam = R * P_world + t其中R是3x3的旋转矩阵，t是3x1的平移向量。
但这里有个麻烦事——旋转和平移是分开写的，连续变换时很不方便。这时候齐次坐标就派上用场了。
核心思想：把三维点(x, y, z)变成四维向量(x, y, z, 1)，这样旋转和平移就能合并成一个4x4的变换矩阵。
| x_cam | | R11 R12 R13 t1 | | x_world |
| y_cam | = | R21 R22 R23 t2 | | y_world |
| z_cam | | R31 R32 R33 t3 | | z_world |
| 1 | | 0 0 0 1 | | 1 |我个人习惯把齐次变换矩阵记作T，这样写起来特别清爽：
P_cam = T * P_world连续变换就是矩阵乘法：
P_pixel = T_intrinsic * T_cam2world * P_world嗯，这里要注意：矩阵乘法不满足交换律，顺序搞反了结果就完全不对。我曾经在项目里因为把旋转和平移的顺序写反了，调试了整整两天才找到问题。
3.3 从相机坐标到图像坐标
这一步是透视投影。相机坐标系下的点(X, Y, Z)，投影到成像平面上：
x = f * X / Z
y = f * Y / Z其中f是焦距。你看，这里出现了除法，所以从三维到二维，深度信息Z就丢失了。这也是为什么单目相机无法直接测距的原因。
避坑指南：我曾经在融合项目中，直接用图像上的像素坐标反算三维点，结果发现算出来的距离完全不对。后来才意识到，没有深度信息，一个像素点对应的是三维空间中的一条射线，而不是一个点。
3.4 从图像坐标到像素坐标
这一步是离散化。图像坐标是连续的毫米值，像素坐标是离散的整数值。变换关系是：
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 |这个矩阵就是内参矩阵的一部分。每个相机都有自己独特的内参，需要通过标定得到。
3.5 完整的投影公式
把上面所有步骤串起来，就是完整的投影公式：
| u | | fx 0 cx | | R11 R12 R13 t1 | | X_w |
| v | = | 0 fy cy | * | R21 R22 R23 t2 | | Y_w |
| 1 | | 0 0 1 | | R31 R32 R33 t3 | | Z_w |
| 0 0 0 1 | | 1 |其中fx = f/dx，fy = f/dy，cx = u0，cy = v0。
关键点：这个公式就是激光雷达点云投影到图像上的数学基础。你有一个激光点(X_w, Y_w, Z_w)，乘以外参矩阵得到相机坐标，再乘以内参矩阵得到像素坐标(u, v)。如果这个像素在图像范围内，就说明这个激光点被相机「看到」了。
3.6 我在项目中的实践
做激光雷达和相机融合时，我一般会做这几步：
- 标定：用棋盘格标定板，同时获取激光点云和图像，解算出外参矩阵R和t。
- 投影验证：把激光点云投影到图像上，看是否对齐。如果偏差超过几个像素，说明标定有问题。
- 深度赋值：对于图像中的每个目标，找到对应的激光点，获取深度信息。
我记得有一次做路测，发现投影结果总是偏左几个像素。排查了半天，发现是标定板的角点检测出了问题——棋盘格在激光雷达的反射强度图上不够清晰。后来换了高反射率的标定板，问题就解决了。
注意事项：
- 齐次坐标的最后一维必须是1，否则需要归一化。
- 旋转矩阵必须是正交矩阵，行列式为1。如果标定出来的R不满足这个条件，说明标定有误差。
- 内参矩阵和外参矩阵的乘法顺序不能错：先外参，后内参。
3.7 小结
坐标变换是传感器融合的基石。你只要记住：世界→相机（刚体变换）→图像（透视投影）→像素（离散化），这条链路就清晰了。齐次坐标让变换变得简洁优雅，而内参和外参则是连接不同传感器的桥梁。
下一章我们会深入讲激光雷达和相机的联合标定方法，到时候这些坐标变换的知识会反复用到。嗯，先把基础打牢，后面就顺了。