图像坐标转换为世界坐标

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

1. 坐标系定义

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

---

2. 转换流程

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

1. 从图像坐标系到相机坐标系（逆投影）：
   • 利用相机内参矩阵和深度信息，将像素坐标转换为相机坐标系下的三维坐标。
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. 特殊情况说明

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

---

6. 总结

• 核心公式： $$\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;
}