2. 坐标系与变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系及其转换关系

做视觉标定，说白了就是搞清楚「东西在哪」和「相机看到的东西在哪」之间的数学关系。我刚开始接触这个领域时，被这四个坐标系绕得晕头转向。后来在产线上调一个机械臂抓取项目，才真正体会到——坐标系搞不清楚，标定就是空中楼阁。

今天我们就把这四个坐标系彻底捋清楚。你想想看，一个物体从真实世界到相机照片，中间经历了什么？

2.1 四个坐标系，一个故事

我们先给每个坐标系一个直观的定义：

嗯，这里要注意：世界坐标系是我们自己定义的。你可以把原点放在工作台角落，也可以放在机械臂基座上。我个人习惯把世界坐标系原点放在标定板的第一个角点上，这样后续计算会省很多事。

2.2 从世界到相机：刚体变换

物体从世界坐标系到相机坐标系，只经历了一件事——刚体变换。说白了就是旋转加平移，物体本身不会变形。

数学表达很简单：

| Xc |   | R11 R12 R13 | | Xw |   | Tx |
| Yc | = | R21 R22 R23 | | Yw | + | Ty |
| Zc |   | R31 R32 R33 | | Zw |   | Tz |

写成齐次坐标形式更简洁：

| Xc |   | R11 R12 R13 Tx | | Xw |
| Yc | = | R21 R22 R23 Ty | | Yw |
| Zc |   | R31 R32 R33 Tz | | Zw |
| 1  |   | 0   0   0   1  | | 1  |

这里的 R 是 3×3 旋转矩阵，T 是 3×1 平移向量。它们合起来就是相机的外参。

2.3 从相机到图像：透视投影

这一步是核心。相机坐标系下的3D点，通过小孔成像模型投影到2D图像平面上。

公式很简单：

x = f * Xc / Zc
y = f * Yc / Zc

其中 f 是焦距，单位是毫米。这里有个关键点——Zc 不能为0，否则点就在相机光心平面上，投影无意义。

写成矩阵形式：

| x |   | f  0  0  0 | | Xc |
| y | = | 0  f  0  0 | | Yc |
| 1 |   | 0  0  1  0 | | Zc |
                      | 1  |

2.4 从图像到像素：离散化

图像坐标系是连续的毫米单位，但像素坐标系是离散的整数。这一步就是把物理尺寸转换成像素坐标。

公式：

u = x / dx + u0
v = y / dy + v0

其中：

• dx, dy：每个像素在x和y方向的物理尺寸（mm/pixel）
• u0, v0：图像中心（主点）的像素坐标

齐次形式：

| u |   | 1/dx  0    u0 | | x |
| v | = | 0     1/dy v0 | | y |
| 1 |   | 0     0    1  | | 1 |

2.5 合在一起：完整的投影方程

把上面三步串起来，就是完整的相机模型：

| 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 * [u, v, 1]^T = K * [R|T] * [Xw, Yw, Zw, 1]^T

其中：

• K：内参矩阵（fx, fy, u0, v0）
• [R|T]：外参矩阵
• s：尺度因子（其实就是 Zc）

2.6 知识体系总览

下面这张图把整个变换链路画清楚了：

// 世界坐标转像素坐标
cv::Point2f worldToPixel(cv::Point3f worldPt, 
                         cv::Mat cameraMatrix, 
                         cv::Mat distCoeffs, 
                         cv::Mat rvec, 
                         cv::Mat tvec) 
{
    std::vector<cv::Point3f> objectPoints;
    objectPoints.push_back(worldPt);
    
    std::vector<cv::Point2f> imagePoints;
    cv::projectPoints(objectPoints, rvec, tvec, 
                      cameraMatrix, distCoeffs, imagePoints);
    
    return imagePoints[0];
}

// 像素坐标转相机坐标（已知深度Zc）
cv::Point3f pixelToCamera(cv::Point2f pixelPt, float depth,
                          cv::Mat cameraMatrix) 
{
    float fx = cameraMatrix.at<double>(0,0);
    float fy = cameraMatrix.at<double>(1,1);
    float cx = cameraMatrix.at<double>(0,2);
    float cy = cameraMatrix.at<double>(1,2);
    
    float x = (pixelPt.x - cx) * depth / fx;
    float y = (pixelPt.y - cy) * depth / fy;
    
    return cv::Point3f(x, y, depth);
}