python 像素坐标系转相机坐标系,代码实现

假设我们已经知道相机内参矩阵 $K$ 和相机外参矩阵 $[R|t]$，其中 $R$ 是旋转矩阵，$t$ 是平移向量。假设我们要将像素坐标 $(u,v)$ 转换为相机坐标系下的坐标 $(X,Y,Z)$。

首先，我们需要将像素坐标 $(u,v)$ 转换为归一化平面坐标 $(x,y)$，公式为：

$$
\begin{bmatrix}x \\ y \\ 1 \end{bmatrix} = K^{-1} \begin{bmatrix}u \\ v \\ 1\end{bmatrix}
$$

其中，$K^{-1}$ 是相机内参矩阵的逆矩阵。

接着，我们需要将归一化平面坐标 $(x,y)$ 转换为相机坐标系下的坐标 $(X,Y,Z)$，公式为：

$$
\begin{bmatrix}X \\ Y \\ Z \end{bmatrix} = [R|t]^{-1} \begin{bmatrix}x \\ y \\ 1\end{bmatrix}
$$

其中，$[R|t]^{-1}$ 是相机外参矩阵的逆矩阵。

下面是 Python 代码实现：

import numpy as np

# 相机内参矩阵
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]])

# 相机外参矩阵
R = np.array([[r11, r12, r13],
              [r21, r22, r23],
              [r31, r32, r33]])
t = np.array([tx, ty, tz]).reshape(3, 1)
ext_mat = np.hstack((R, t))

# 像素坐标
u, v = 100, 200

# 像素坐标转归一化平面坐标
uv1 = np.array([u, v, 1]).reshape(3, 1)
xy1 = np.linalg.inv(K) @ uv1
x, y = xy1[0], xy1[1]

# 归一化平面坐标转相机坐标系下的坐标
xy1 = np.array([x, y, 1]).reshape(3, 1)
XYZ_hom = np.linalg.inv(ext_mat) @ xy1
X, Y, Z = XYZ_hom[0], XYZ_hom[1], XYZ_hom[2]

其中，$fx$、$fy$、$cx$、$cy$、$r11$、$r12$、$r13$、$r21$、$r22$、$r23$、$r31$、$r32$、$r33$、$tx$、$ty$、$tz$ 是相机内参和外参的参数。