图像坐标转换为世界坐标

理解轮眉测试中如何通过识别图像点计算实际轮眉高度, 转换函数为:CameraLib.CameraAgorithm下的public static PointF3D Pixel2World(PointF Pixel, CameraBase _camera, double Distance)
图像坐标转换为空间坐标的过程涉及相机的内参矩阵、外参矩阵（旋转矩阵和平移向量）以及可能的深度信息.

1. 坐标系定义

世界坐标系（World Coordinate System）：三维空间中的绝对坐标系，记为 $(X_w, Y_w, Z_w)$ 。
相机坐标系（Camera Coordinate System）：以相机光心为原点，记为 $(X_c, Y_c, Z_c)$ 。
图像坐标系（Image Coordinate System）：二维像素坐标系，记为 $(u, v)$ 。

2. 转换流程

图像坐标到空间坐标的转换需要以下步骤：

从图像坐标系到相机坐标系（逆投影）：
- 利用相机内参矩阵和深度信息，将像素坐标转换为相机坐标系下的三维坐标。
从相机坐标系到世界坐标系：
- 利用相机的外参矩阵（旋转矩阵 $R$ 和平移向量 $T$ ），将相机坐标系下的坐标转换为世界坐标系。

3. 公式推导

（1）相机模型与投影公式

相机的投影公式为：

Z_c \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & T \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix}

其中：

$K$ 为内参矩阵： $K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$
$R$ 和 $T$ 为外参矩阵（旋转和平移）。

（2）逆投影：图像坐标 → 相机坐标系

假设已知深度 $Z_c$ ，图像坐标 $(u, v)$ 对应的相机坐标系坐标为：

\begin{cases} X_c = \frac{(u - c_x) \cdot Z_c}{f_x} \\ Y_c = \frac{(v - c_y) \cdot Z_c}{f_y} \\ Z_c = \text{已知深度值} \end{cases}

矩阵形式：

\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = K^{-1} \cdot Z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}

其中 $K^{-1}$ 为内参矩阵的逆：

K^{-1} = \begin{bmatrix} \frac{1}{f_x} & 0 & -\frac{c_x}{f_x} \\ 0 & \frac{1}{f_y} & -\frac{c_y}{f_y} \\ 0 & 0 & 1 \end{bmatrix}

（3）相机坐标系 → 世界坐标系

利用外参矩阵将相机坐标系坐标转换到世界坐标系：

\begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} = \begin{bmatrix} R^T & -R^T T \\ 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix}

其中 $R^T$ 是旋转矩阵的转置， $-R^T T$ 是平移向量的逆变换。

4. 整合公式

若已知深度 $Z_c$ ，完整的转换公式为：

\begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} = R^T \left( K^{-1} \cdot Z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} - T \right)

5. 特殊情况说明

深度未知（单目相机）：
- 单张图像无法唯一确定三维坐标，需通过多视图几何（如立体匹配）或已知约束（如平面假设）求解。
平面场景假设：
- 若物体位于已知平面 $Z_w = 0$ ，可通过单应性矩阵（Homography）直接转换。

6. 示例代码（Python）

假设已知深度 $Z_c = 1$ ，使用 OpenCV 进行坐标转换：

import numpy as np

# 内参矩阵 K
K = np.array([[1000, 0, 320], [0, 1000, 240], [0, 0, 1]])

# 外参矩阵 R 和 T
R = np.eye(3)  # 假设无旋转
T = np.array([0, 0, 0])  # 假设无平移

# 图像坐标 (u, v)
u, v = 400, 300

# 已知深度 Z_c
Z_c = 1.0

# 逆投影到相机坐标系
uv1 = np.array([u, v, 1])
K_inv = np.linalg.inv(K)
Xc_Yc_Zc = Z_c * K_inv.dot(uv1)

# 转换到世界坐标系
Xc_Yc_Zc_homogeneous = np.append(Xc_Yc_Zc, 1)
RT = np.hstack((R.T, -R.T.dot(T).reshape(-1, 1)))
Xw_Yw_Zw = RT.dot(Xc_Yc_Zc_homogeneous)

print("世界坐标系坐标:", Xw_Yw_Zw[:3])

7. 总结

核心公式： $\begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} = R^T \left( K^{-1} \cdot Z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} - T \right)$
依赖参数：
- 相机内参 $K$ 、外参 $R, T$ 、深度 $Z_c$ 。
适用场景：
- 深度已知（如 RGB-D 相机）或多视图几何（双目视觉）。

public static PointF3D Pixel2World(PointF Pixel, CameraBase _camera, double Distance)
{
    Mat Rvecs = new Mat(3, 1, MatType.CV_64FC1, new double[] { _camera.Extrinsics_Rx, _camera.Extrinsics_Ry, _camera.Extrinsics_Rz });
    Mat Tvecs = new Mat(3, 1, MatType.CV_64FC1, new double[] {
        _camera.Extrinsics_Tx,
        _camera.Extrinsics_Ty,
        _camera.Extrinsics_Tz });
    Mat RotationMatrix = new Mat(3, 3, MatType.CV_64FC1);
    Mat RotationMatrix_Invert = new Mat(3, 3, MatType.CV_64FC1);
    Mat CameraMatrix = new Mat(3,3,MatType.CV_64FC1,new double[3,3]{ 
    {_camera.Intrinsics_Fx,0,_camera.Intrinsics_Cx},
    {0,_camera.Intrinsics_Fy,_camera.Intrinsics_Cy},
    {0,0,1}});
    Mat CameraMatrix_Invert = new Mat(3,3,MatType.CV_64FC1);
    Mat ImagePoint = new Mat(3, 1, MatType.CV_64FC1, new double[] { Pixel.X, Pixel.Y, 1 });

    Cv2.Rodrigues(Rvecs, RotationMatrix);
    Cv2.Invert(RotationMatrix, RotationMatrix_Invert, DecompTypes.SVD);
    Cv2.Invert(CameraMatrix, CameraMatrix_Invert, DecompTypes.SVD);

    //计算坐标(相机坐标系)
    Mat wcPoint = RotationMatrix_Invert * (Distance * CameraMatrix_Invert * ImagePoint - Tvecs);

    PointF3D worldPoint = new PointF3D(
        wcPoint.At<double>(0, 0)  + _camera.Point3DX, 
        wcPoint.At<double>(1, 0)  + _camera.Point3DY, 
        wcPoint.At<double>(2, 0) + _camera.Point3DZ);
    return worldPoint;
}