0%

图像坐标转换为世界坐标

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


1. 坐标系定义

  1. 世界坐标系(World Coordinate System):三维空间中的绝对坐标系,记为 (Xw,Yw,Zw)(X_w, Y_w, Z_w)
  2. 相机坐标系(Camera Coordinate System):以相机光心为原点,记为 (Xc,Yc,Zc)(X_c, Y_c, Z_c)
  3. 图像坐标系(Image Coordinate System):二维像素坐标系,记为 (u,v)(u, v)

2. 转换流程

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

  1. 从图像坐标系到相机坐标系(逆投影):
    • 利用相机内参矩阵和深度信息,将像素坐标转换为相机坐标系下的三维坐标。
  2. 从相机坐标系到世界坐标系
    • 利用相机的外参矩阵(旋转矩阵 RR 和平移向量 TT),将相机坐标系下的坐标转换为世界坐标系。

3. 公式推导

(1)相机模型与投影公式

相机的投影公式为:

Zc[uv1]=K[RT][XwYwZw1]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}

其中:

  • KK 为内参矩阵:

    K=[fx0cx0fycy001]K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}

  • RRTT 为外参矩阵(旋转和平移)。

(2)逆投影:图像坐标 → 相机坐标系

假设已知深度 ZcZ_c,图像坐标 (u,v)(u, v) 对应的相机坐标系坐标为:

{Xc=(ucx)ZcfxYc=(vcy)ZcfyZc=已知深度值\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}

矩阵形式

[XcYcZc]=K1Zc[uv1]\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = K^{-1} \cdot Z_c \cdot \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}

其中 K1K^{-1} 为内参矩阵的逆:

K1=[1fx0cxfx01fycyfy001]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)相机坐标系 → 世界坐标系

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

[XwYwZw1]=[RTRTT01][XcYcZc1]\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}

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


4. 整合公式

若已知深度 ZcZ_c,完整的转换公式为:

[XwYwZw]=RT(K1Zc[uv1]T)\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. 平面场景假设
    • 若物体位于已知平面 Zw=0Z_w = 0,可通过单应性矩阵(Homography)直接转换。

6. 示例代码(Python)

假设已知深度 Zc=1Z_c = 1,使用 OpenCV 进行坐标转换:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
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. 总结

  • 核心公式

    [XwYwZw]=RT(K1Zc[uv1]T)\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)

  • 依赖参数
    • 相机内参 KK、外参 R,TR, T、深度 ZcZ_c
  • 适用场景
    • 深度已知(如 RGB-D 相机)或多视图几何(双目视觉)。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
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;
}