相机坐标系、像素坐标系转换

相机内参矩阵是相机的重要参数之一，它描述了相机光学系统的内部性质，例如焦距、光学中心和图像畸变等信息。在计算机视觉和图形学中，相机内参矩阵通常用于将图像坐标系中的像素坐标转换为相机坐标系中的三维坐标，或者将相机坐标系中的三维坐标投影到图像坐标系中的像素坐标。

相机内参矩阵通常表示为一个 3x3 的矩阵，具有以下形式：

fx  0   cx
0   fy  cy
0   0   1

其中 (cx, cy) 是图像的光学中心，表示图像平面中心对应于相机坐标系的投影坐标。fx 和 fy 是相机在 x 和 y 方向上的焦距，表示相机坐标系中的一个单位长度在图像坐标系中对应的像素数。如果图像的 x 和 y 方向的焦距相等，则可以使用一个标量 f 来表示它们的平均值。

在计算机视觉和图形学中，像素坐标系和相机坐标系之间的转换是非常重要的，因为它允许我们将从相机中捕获的图像中的像素位置转换为三维场景中的物体位置。下面是一个简单的 C++ 代码示例，演示如何将像素坐标系中的坐标转换为相机坐标系中的坐标。

假设我们有一个相机，它在世界坐标系中的位置为 (cx, cy, cz)，相机朝向的方向为 (fx, fy, fz)。假设我们有一个图像，它的分辨率为 (width, height)，我们想要将像素坐标 (u, v) 转换为相机坐标系中的坐标 (x, y, z)。

首先，我们需要计算像素坐标系中的坐标 (u, v) 对应的图像平面上的点 (px, py)。对于具有焦距 f 的相机，可以使用以下公式将像素坐标 (u, v) 转换为图像平面上的点 (px, py)：

px = (u - width/2) / f
py = (v - height/2) / f

其中 width 和 height 是图像的分辨率，f 是相机的焦距。

接下来，我们可以使用相机的内参矩阵将 (px, py) 转换为相机坐标系中的坐标 (x, y, z)：

[x]   [fx  0   cx  0] [px]
[y] = [0   fy  cy  0] [py]
[z]   [0   0   1   0] [1 ]

其中 [fx fy cx cy] 是相机的内参矩阵，它包含相机的焦距和光学中心的位置。

下面是一个 C++ 函数，将像素坐标 (u, v) 转换为相机坐标系中的坐标 (x, y, z)：

cv::Mat pixel2cam(const cv::Point2d& p, const cv::Mat& K) {
    cv::Mat pt_cam = cv::Mat::zeros(3, 1, CV_64F);

    pt_cam.at<double>(0, 0) = (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0);
    pt_cam.at<double>(1, 0) = (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1);
    pt_cam.at<double>(2, 0) = 1.0;

    return pt_cam;
}

其中 p 是像素坐标，K 是相机内参矩阵。这个函数返回一个 cv::Mat，它包含相机坐标系中的坐标 (x, y, z）。