像素坐标转世界坐标的计算

原理

下图表示了小孔成像模型（图片及公式参考 OpenCV官方资料）

这个图里涉及4个坐标系：

1. 世界坐标系：其坐标原点可视情况而定，可以表示空间的物体，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix} X_W \\ Y_W \\Z_W \end{bmatrix}$表示；
2. 相机坐标系：以摄像机光心为原点（在针孔模型中也就是针孔为中心），z轴与光轴重合，也就是z轴指向相机的前方（与成像平面垂直），x轴与y轴的正方向与世界坐标系平行，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix}X_c \\ Y_c \\ Z_c\end{bmatrix}$表示；
3. 图像物理坐标系（也叫成像平面坐标系）：用物理长度单位表示像素的位置，坐标原点为摄像机光轴与图像物理坐标系的交点位置，单位为长度单位，比如MM（毫米），用矩阵$\begin{bmatrix}x \\ y \end{bmatrix}$表示。
4. 像素坐标系：坐标原点在左上角，以像素为单位，有明显的范围限制，即用于表示全画面的像素长和像素长宽，矩阵$\begin{bmatrix}u \\ v \end{bmatrix}$表示。

以下公式描述了$\begin{bmatrix}u & v \end{bmatrix}^T$、$\begin{bmatrix}x & y \end{bmatrix}^T$、$\begin{bmatrix}X_c & Y_c & Z_c\end{bmatrix}^T$和$\begin{bmatrix}X_W & Y_W & Z_W \end{bmatrix}^T$之间的转换关系。

$z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix}= \begin{bmatrix}1/d_x&0&c_x\\0&1/d_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}f&0&0\\ 0&f&0\\ 0&0&1 \end{bmatrix} \begin{bmatrix}r11&r12&r13&t1\\ r21&r22&r23&t2\\ r31&r32&r33&t3 \end{bmatrix} \begin{bmatrix}X_W \\ Y_W \\Z_W \\ 1\end{bmatrix}$

以上公式中，$d_x$和$d_y$表示1个像素有多少长度，即用传感器的尺寸除以像素数量，比如2928.384um * 2205.216um的传感的分辨率为2592 * 1944，每个像素的大小即约1.12um。

由于相机与物体的视角来看，都是三维坐标，因此两者之间的变换只需要进行矩阵的旋转、平移即可达到坐标系转换的目的（不同坐标系中，物体的绝对大小并不会随着坐标系的变化而变化，因此不涉及缩放处理）。对于变换矩阵  $\begin{bmatrix}r11&r12&r13&t1\\ r21&r22&r23&t2\\ r31&r32&r33&t3 \end{bmatrix}$ 需要理解，矩阵是由 3*3 的旋转矩阵 r (rotation) 和 3*1的平移向量 t (translation)组成。

$f$表示焦距，在上图中，根据相似三角形，P点和p点具有以下关系：

$\frac{X_c}{x} = \frac{Y_c}{y} = \frac{Z_c}{f}$ 即$x=X_c/(\frac{Z_c}{f})$ $y=Y_c/(\frac{Z_c}{f})$，可见：$f$越大，$x$和$y$越大，$Z_c$越大，$x$和$y$越小。

$c_x$和$c_y$表示中心点在像素坐标系中的位置。

要求像素坐标系中某像素点对应在世界坐标系中的位置，需要知道相机的内参、外参，相机的内参可以通过标定获得，外参可以人为设定。

第一步，将像素坐标变换到相机坐标系：

$z\begin{bmatrix}u \\ v\\ 1 \end{bmatrix} = \begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix}x \\ y\\ 1 \end{bmatrix} = K\begin{bmatrix}x \\ y\\ 1 \end{bmatrix}$

两边乘以K的逆后推导出：

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

第二步，从相机坐标系变换到世界坐标系：

$\begin{bmatrix}X_c \\ Y_c\\ Z_c \end{bmatrix} = R \begin{bmatrix}X \\ Y\\ Z \end{bmatrix} + t$

将方程乘以$R^{-1}$，可以推导出：

$\begin{bmatrix}X \\ Y\\ Z \end{bmatrix} = \begin{bmatrix}X_c \\ Y_c \\ Z_c \end{bmatrix}R^{-1} - t R^{-1}= z\begin{bmatrix}x\\ y\\ 1 \end{bmatrix}R^{-1} - t R^{-1}$

代码

通过输入相机的内参，旋转向量，平移向量和像素坐标，可以通过以下函数求出对应的世界坐标点。
以下代码中需求注意要对平移向量取转置，将1x3矩阵变为3x1矩阵后，才能实现3x3矩阵和3x1矩阵的乘法运算。

验证

先使用projectPoints生成像素点：

使用以下欧拉角：

对应的平移向量，表示空间坐标原点相对在相平面原点偏移x=134mm，y=132mm，z=200mm。

生成空间坐标点：

经projectPoints计算后对应的像素空间点是：

经函数求出的空间坐标点是：

可以对比按11*8格和30mm/格所生成空间坐标点结果，基本一致。

参考链接

---

• 像素坐标转世界坐标的计算
• Camera Calibration and 3D Reconstruction
•  计算机视觉基础3——内部参数描述