二维像素坐标变换到真实世界三维坐标，首先需要进行相机标定。

1 相机标定

1.1 四个重要坐标系

相机标定涉及到世界坐标系、相机坐标系、图像物理坐标系和像素坐标系四个坐标系之间的关系。

世界坐标系可以表示任何物体在真实世界中的三维坐标，单位米。

相机坐标系以相机光心为原点，该坐标系Z轴与光轴重合，即Z轴指向相机前方，与成像平面垂直，单位米。

图像物理坐标系使用物理单位表示像素的位置，坐标原点为摄像机光轴与图像物理坐标系的交点位置，单位毫米（相机内部CCD传感器很小，为毫米级）。

像素坐标系坐标原点在图片左上角，以像素为单位。CCD传感器到图片像素之间的转换关系为pixel/mm。

1.2 坐标系间变换关系

世界坐标系$O_{w} - X_{w}Y_{w}Z_{w}$到相机坐标系$O_{c} - X_{c}Y_{c}Z_{c}$的变换涉及到坐标系的刚体变换，即旋转变换和平移变换。世界坐标系分别绕着$X_{w}Y_{w}Z_{w}$轴分别旋转$\theta,\phi,\omega$可得到三个轴对应的旋转矩阵$R_{1},R_{2},R_{3}$。完整的旋转矩阵$R$可表示为

$$\begin{matrix} R = R_{1} \cdot R_{2} \cdot R_{3} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & - sin\theta & \cos\theta \\ \end{bmatrix}\begin{bmatrix} cos\phi & 0 & - sin\phi \\ 0 & 1 & 0 \\ sin\phi & 0 & cos\phi \\ \end{bmatrix}\begin{bmatrix} cos\omega & - sin\omega & 0 \\ sin\omega & cos\omega & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \\ \end{matrix}$$

坐标系间的平移向量为$T$，则世界坐标系到相机坐标系的变换可表示为

$$\begin{matrix} \begin{bmatrix} X_{c} \\ Y_{c} \\ Z_{c} \\ \end{bmatrix} = R\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ \end{bmatrix} + T \end{matrix}$$

整理后可得

$$\begin{matrix}\begin{bmatrix} X_{c} \\ Y_{c} \\ \begin{matrix} Z_{c} \\ 1 \\ \end{matrix} \\ \end{bmatrix}= \begin{bmatrix} R & T \\ 0 & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} Z_{w} \\ 1 \\ \end{matrix} \\ \end{bmatrix} \end{matrix}$$

其中R为$3 \times 3$的旋转矩阵，T为$3 \times 1$的平移列向量。

{width="5.22610564304462in" height="2.8696894138232723in"}

图1 世界坐标系变换到相机坐标系

相机坐标系$O_{c} - X_{c}Y_{c}Z_{c}$到图像物理坐标系$o - xy$的变换涉及到坐标系的透视投影变换，即从三维坐标转换到二维坐标关系。透视投影关系中通过相似三角形可得$\mathrm{\Delta}{ABO}{c} \backsim \mathrm{\Delta}{oCO}{c}$，$\mathrm{\Delta}{PBO}{c} \backsim \mathrm{\Delta}{pCO}{c}$，可得

$$\begin{matrix} \frac{AB}{oC} = \frac{AO_{c}}{oO_{c}} = \frac{PB}{pC} = \frac{AB}{oC} = \frac{X_{c}}{x} = \frac{Z_{c}}{f} = \frac{Y_{c}}{y} \\ \end{matrix}$$

即$x = f\frac{X_{c}}{Z_{c}},y = f\frac{Y_{c}}{Z_{c}}$，用矩阵形式表示为

$$\begin{matrix} Z_{c}\begin{bmatrix} x \\ y \\ 1 \\ \end{bmatrix} = \begin{bmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix}\begin{bmatrix} X_{c} \\ Y_{c} \\ \begin{matrix} Z_{c} \\ 1 \\ \end{matrix} \\ \end{bmatrix} \\ \end{matrix}$$

{width="3.187140201224847in" height="3.0292858705161856in"}

图2 相机坐标系变换到图像物理坐标系

图像物理坐标系$o - xy$到像素坐标系$o_{uv} - uv$之间坐标原点位置不一致，坐标尺度不一致，故两者之间的变换只涉及平移变换和伸缩变换，不涉及旋转变换，即

$$\begin{matrix} \left{ \begin{matrix} u = \frac{x}{dx} + u_{0} \\ v = \frac{y}{dy} + v_{0} \\ \end{matrix} \right.\ \\ \end{matrix}$$

其中$dx$和$dy$分别表示每个像素对应的长度，单位pixel/mm。用矩阵形式表示为

$$\begin{matrix} \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} \frac{1}{dx} & 0 & u_{0} \\ 0 & \frac{1}{dy} & v_{0} \\ 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} x \\ y \\ 1 \\ \end{bmatrix} \\ \end{matrix}$$

{width="3.455333552055993in" height="3.294027777777778in"}

图3 图像物理坐标系变换到像素坐标系

综上完整的真实世界三维坐标系到二维像素坐标系的变换关系为

$$Z_{c}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} \frac{1}{dx} & 0 & u_{0} \\ 0 & \frac{1}{dy} & v_{0} \\ 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix}\begin{bmatrix} R & T \\ 0 & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} Z_{w} \\ 1 \\ \end{matrix} \\ \end{bmatrix} = \begin{bmatrix} f_{x} & 0 & u_{0} & 0 \\ 0 & f_{y} & v_{0} & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix}\begin{bmatrix} R & T \\ 0^{T} & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} Z_{w} \\ 1 \\ \end{matrix} \\ \end{bmatrix}$$

其中$f_{x} = \frac{f}{dx},f_{y} = \frac{f}{dy}$。

令$K = \begin{bmatrix} f_{x} & 0 & u_{0} & 0 \ 0 & f_{y} & v_{0} & 0 \ 0 & 0 & 1 & 0 \ \end{bmatrix}$，则上式可简化为

$$\begin{matrix} Z_{c}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = K\begin{bmatrix} R & T \\ 0^{T} & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} Z_{w} \\ 1 \\ \end{matrix} \\ \end{bmatrix} \\ \end{matrix}$$

其中，$3 \times 4$矩阵$K$为相机的内参数矩阵，$4 \times 4$矩阵$\begin{bmatrix} R & T \ 0^{T} & 1 \ \end{bmatrix}$为相机的外参数矩阵。

1.3 直接线性标定法

实际问题中，相机的内外参数往往未知，为解决后续的坐标变换问题，需要先标定部分真实世界点的坐标及对应像素坐标，借助方程组求解内外参数中的未知数。

令$L = K\begin{bmatrix} R & T \ 0^{T} & 1 \ \end{bmatrix}$，则$L$为$3 \times 4$的未知参数矩阵，包含12个未知参数

$$\begin{matrix} L = \begin{bmatrix} \begin{matrix} l_{00} & l_{01} \\ \end{matrix} & \begin{matrix} l_{02} & l_{03} \\ \end{matrix} \\ \begin{matrix} l_{10} & l_{11} \\ \end{matrix} & \begin{matrix} l_{12} & l_{13} \\ \end{matrix} \\ \begin{matrix} l_{20} & l_{21} \\ \end{matrix} & \begin{matrix} l_{22} & l_{23} \\ \end{matrix} \\ \end{bmatrix} \\ \end{matrix}$$

对于第$i$个标定点的真实坐标和像素坐标集合${ X_{w}^{i},Y_{w}^{i},Z_{w}^{i},u^{i},v^{i}}$，将矩阵转变为方程组形式，并消去$Z_{c}$后，可得一组$u^{i},v^{i}$与$X_{w}^{i},Y_{w}^{i},Z_{w}^{i}$间的关系为

$$\begin{matrix} \left{ \begin{matrix} u^{i} = \frac{l_{00}X_{w}^{i} + l_{01}Y_{w}^{i} + l_{02}Z_{w}^{i} + l_{03}}{l_{20}X_{w}^{i} + l_{21}Y_{w}^{i} + l_{22}Z_{w}^{i} + l_{23}} \\ v^{i} = \frac{l_{10}X_{w}^{i} + l_{11}Y_{w}^{i} + l_{12}Z_{w}^{i} + l_{13}}{l_{20}X_{w}^{i} + l_{21}Y_{w}^{i} + l_{22}Z_{w}^{i} + l_{23}} \\ \end{matrix} \right.\ \\ \end{matrix}$$

移项后整理可得

$$\begin{matrix} \left{ \begin{matrix} l_{00}X_{w}^{i} + l_{01}Y_{w}^{i} + l_{02}Z_{w}^{i} + l_{03} - l_{20}{uX}_{w}^{i} - l_{21}{uY}_{w}^{i} - l_{22}uZ_{w}^{i} = l_{23}u^{i} \\ l_{10}X_{w}^{i} + l_{11}Y_{w}^{i} + l_{12}Z_{w}^{i} + l_{13} - l_{20}{vX}_{w}^{i} - l_{21}{vY}_{w}^{i} - l_{22}vZ_{w}^{i} = l_{23}v^{i} \\ \end{matrix} \right.\ \\ \end{matrix}$$

用矩阵形式表示为

$$\begin{matrix} \begin{bmatrix} X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{i} & - {uY}_{w}^{i} & - uZ_{w}^{i} \\ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}_{w}^{i} & - {vY}_{w}^{i} & - vZ_{w}^{i} \\ \end{bmatrix}\begin{bmatrix} l_{00} \\ l_{01} \\ l_{02} \\ l_{03} \\ l_{10} \\ l_{11} \\ l_{12} \\ l_{13} \\ l_{20} \\ l_{21} \\ l_{22} \\ \end{bmatrix} = l_{23}\begin{bmatrix} u^{i} \\ v^{i} \\ \end{bmatrix} \\ \end{matrix}$$

上式即为单个标定点对应的方程组的矩阵形式。若有共$n$个标定点，则这$2n$个方程对应的矩阵为

$$\begin{matrix} \begin{bmatrix} X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{1} & - {uY}_{w}^{1} & - uZ_{w}^{1} \\ 0 & 0 & 0 & 0 & X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & - {vX}_{w}^{1} & - {vY}_{w}^{1} & - vZ_{w}^{1} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{i} & - {uY}_{w}^{i} & - uZ_{w}^{i} \\ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}_{w}^{i} & - {vY}_{w}^{i} & - vZ_{w}^{i} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{i} & - {uY}_{w}^{i} & - uZ_{w}^{i} \\ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}_{w}^{i} & - {vY}_{w}^{i} & - vZ_{w}^{i} \\ \end{bmatrix}\begin{bmatrix} l_{00} \\ l_{01} \\ l_{02} \\ l_{03} \\ l_{10} \\ l_{11} \\ l_{12} \\ l_{13} \\ l_{20} \\ l_{21} \\ l_{22} \\ \end{bmatrix} = l_{23}\begin{bmatrix} u^{1} \\ v^{1} \\ \vdots \\ u^{i} \\ v^{i} \\ \vdots \\ u^{n} \\ v^{n} \\ \end{bmatrix} \\ \end{matrix}$$

令$11 \times 1$列向量

$$m = \begin{bmatrix} m_{1} & m_{2} & m_{3} & m_{4} & m_{5} & m_{6} & m_{7} & m_{8} & m_{9} & m_{10} & m_{11} \\ \end{bmatrix}^{T} = {1\text{/}l}_{23}{\cdot \begin{bmatrix} l_{00} & l_{01} & l_{02} & l_{03} & l_{10} & l_{11} & l_{12} & l_{13} & l_{20} & l_{21} & l_{22} \\ \end{bmatrix}}^{T}$$

则上式和矩阵$L$可表示为

$$\begin{matrix} \begin{bmatrix} X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{1} & - {uY}_{w}^{1} & - uZ_{w}^{1} \\ 0 & 0 & 0 & 0 & X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & - {vX}_{w}^{1} & - {vY}_{w}^{1} & - vZ_{w}^{1} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{i} & - {uY}_{w}^{i} & - uZ_{w}^{i} \\ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}_{w}^{i} & - {vY}_{w}^{i} & - vZ_{w}^{i} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}_{w}^{i} & - {uY}_{w}^{i} & - uZ_{w}^{i} \\ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}_{w}^{i} & - {vY}_{w}^{i} & - vZ_{w}^{i} \\ \end{bmatrix}\begin{bmatrix} m_{1} \\ m_{2} \\ m_{3} \\ m_{4} \\ m_{5} \\ m_{6} \\ m_{7} \\ m_{8} \\ m_{9} \\ m_{10} \\ m_{11} \\ \end{bmatrix} = \begin{bmatrix} u^{1} \\ v^{1} \\ \vdots \\ u^{i} \\ v^{i} \\ \vdots \\ u^{n} \\ v^{n} \\ \end{bmatrix} \\ \end{matrix}$$

$$\begin{matrix} L = \begin{bmatrix} \begin{matrix} l_{00} & l_{01} \\ \end{matrix} & \begin{matrix} l_{02} & l_{03} \\ \end{matrix} \\ \begin{matrix} l_{10} & l_{11} \\ \end{matrix} & \begin{matrix} l_{12} & l_{13} \\ \end{matrix} \\ \begin{matrix} l_{20} & l_{21} \\ \end{matrix} & \begin{matrix} l_{22} & l_{23} \\ \end{matrix} \\ \end{bmatrix} = l_{23} \cdot \begin{bmatrix} \begin{matrix} m_{1} & m_{2} \\ \end{matrix} & \begin{matrix} m_{3} & m_{4} \\ \end{matrix} \\ \begin{matrix} m_{5} & m_{6} \\ \end{matrix} & \begin{matrix} m_{7} & m_{8} \\ \end{matrix} \\ \begin{matrix} m_{9} & m_{10} \\ \end{matrix} & \begin{matrix} m_{11} & 1 \\ \end{matrix} \\ \end{bmatrix} \\ \end{matrix}$$

令$A = \begin{bmatrix} X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & 0 & 0 & 0 & 0 & - {uX}{w}^{1} & - {uY}{w}^{1} & - uZ_{w}^{1} \ 0 & 0 & 0 & 0 & X_{w}^{1} & Y_{w}^{1} & Z_{w}^{1} & 1 & - {vX}{w}^{1} & - {vY}{w}^{1} & - vZ_{w}^{1} \ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}{w}^{i} & - {uY}{w}^{i} & - uZ_{w}^{i} \ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}{w}^{i} & - {vY}{w}^{i} & - vZ_{w}^{i} \ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \ X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & 0 & 0 & 0 & 0 & - {uX}{w}^{i} & - {uY}{w}^{i} & - uZ_{w}^{i} \ 0 & 0 & 0 & 0 & X_{w}^{i} & Y_{w}^{i} & Z_{w}^{i} & 1 & - {vX}{w}^{i} & - {vY}{w}^{i} & - vZ_{w}^{i} \ \end{bmatrix},U = \begin{bmatrix} u^{1} \ v^{1} \ \vdots \ u^{i} \ v^{i} \ \vdots \ u^{n} \ v^{n} \ \end{bmatrix}$，则$2n$个方程组可简化表示为

$$\begin{matrix} Am = U\ \\ \end{matrix}$$

每个标定点可提供2组方程求解共计12个未知数，因此至少需要6个标定点进行未知参数矩阵$L$的求解。当然使用越多的标定点进行标定，方程个数将超过未知数个数，此时借助最小二乘法，求解方程问题就变为了拟合问题，L的求解就越为准确。

当利用奇异值分解等方法求解出$m$后，并不意味着未知参数矩阵$L$被完全解出了，因为最后一个未知元素$l_{23}$的值并未被解出。

基于使用$m$向量表示$L$矩阵的方式及$L$矩阵与$K$和$R、T$之间的等式关系：

$$L = l_{23} \cdot \begin{bmatrix} \begin{matrix} m_{1} & m_{2} \\ \end{matrix} & \begin{matrix} m_{3} & m_{4} \\ \end{matrix} \\ \begin{matrix} m_{5} & m_{6} \\ \end{matrix} & \begin{matrix} m_{7} & m_{8} \\ \end{matrix} \\ \begin{matrix} m_{9} & m_{10} \\ \end{matrix} & \begin{matrix} m_{11} & 1 \\ \end{matrix} \\ \end{bmatrix} = l_{23} \cdot \begin{bmatrix} \begin{matrix} M_{1}^{T} \\ \end{matrix} & \begin{matrix} m_{4} \\ \end{matrix} \\ M_{2}^{T} & \begin{matrix} m_{8} \\ \end{matrix} \\ M_{3}^{T} & \begin{matrix} 1 \\ \end{matrix} \\ \end{bmatrix} = K\begin{bmatrix} R & T \\ 0^{T} & 1 \\ \end{bmatrix} = \begin{bmatrix} f_{x} & 0 & u_{0} & 0 \\ 0 & f_{y} & v_{0} & 0 \\ 0 & 0 & 1 & 0 \\ \end{bmatrix}\begin{bmatrix} r_{1}^{T} & t_{x} \\ r_{2}^{T} & t_{y} \\ r_{3}^{T} & t_{z} \\ 0^{T} & 1 \\ \end{bmatrix}$$

式中$M_{1}^{T} = \begin{bmatrix} m_{1} & m_{2} & m_{3} \ \end{bmatrix}^{T},M_{2}^{T} = \begin{bmatrix} m_{5} & m_{6} & m_{7} \ \end{bmatrix}^{T},M_{3}^{T} = \begin{bmatrix} m_{9} & m_{10} & m_{11} \ \end{bmatrix}^{T}$，$r_{1}^{T},r_{2}^{T},r_{3}^{T}$为旋转矩阵$R$的行向量，$t_{x},t_{y},t_{z}$为平移向量$T$的元素。将上式进一步运算后可得

$$\begin{matrix} l_{23} \cdot \begin{bmatrix} \begin{matrix} M_{1}^{T} \\ \end{matrix} & \begin{matrix} m_{4} \\ \end{matrix} \\ M_{2}^{T} & \begin{matrix} m_{8} \\ \end{matrix} \\ M_{3}^{T} & \begin{matrix} 1 \\ \end{matrix} \\ \end{bmatrix} = \begin{bmatrix} {f_{x}r}_{1}^{T} + u_{0}r_{3}^{T} & {f_{x}t}_{x} + u_{0}t_{z} \\ f_{y}r_{2}^{T} + v_{0}r_{3}^{T} & f_{y}t_{y} + {v_{0}t}_{z} \\ r_{3}^{T} & t_{z} \\ \end{bmatrix} \\ \end{matrix}$$

考虑到上述等式中矩阵的一一对应相等，可得以下方程组

$$\begin{matrix} \left{ \begin{matrix} l_{23} \cdot M_{3}^{T} = r_{3}^{T} \\ u_{0} = \left( {f_{x}r}_{1}^{T} + u_{0}r_{3}^{T} \right)r_{3} \\ v_{0} = \left( f_{y}r_{2}^{T} + v_{0}r_{3}^{T} \right)r_{3} \\ f_{x} = \left| \left( {f_{x}r}_{1}^{T} + u_{0}r_{3}^{T} \right) \times r_{3}^{T} \right| \\ f_{y} = \left| \left( f_{y}r_{2}^{T} + v_{0}r_{3}^{T} \right) \times r_{3}^{T} \right| \\ r_{1}^{T} = {(l}_{23} \cdot M_{1}^{T} - u_{0}r_{3}^{T})\text{/}f_{x} \\ r_{2}^{T} = {(l}_{23} \cdot M_{2}^{T} - v_{0}r_{3}^{T})\text{/}f_{y} \\ t_{z} = l_{23} \\ t_{x} = \left( l_{23}m_{4} - u_{0}t_{z} \right)\text{/}f_{x} \\ t_{y} = \left( l_{23}m_{8} - v_{0}t_{z} \right)\text{/}f_{y} \\ \end{matrix} \right.\ \\ \end{matrix}$$

借助旋转矩阵$R$为正交矩阵的特性，化简上式可得到相机内外参数矩阵中的各元素为：

$$\begin{matrix} \left{ \begin{matrix} l_{23} = 1\text{/}\left| M_{3}^{T} \right| \\ u_{0} = {l_{23}}^{2}M_{1}^{T} \cdot M_{3}^{T} \\ v_{0} = {l_{23}}^{2}M_{2}^{T} \cdot M_{3}^{T} \\ f_{x} = {l_{23}}^{2}\left| M_{1}^{T} \times M_{3}^{T} \right| \\ f_{y} = {l_{23}}^{2}\left| M_{2}^{T} \times M_{3}^{T} \right| \\ r_{1}^{T} = l_{23}\left( M_{1}^{T} - u_{0}M_{3}^{T} \right)\text{/}f_{x} \\ r_{2}^{T} = l_{23}\left( M_{2}^{T} - v_{0}M_{3}^{T} \right)\text{/}f_{y} \\ r_{3}^{T} = l_{23} \cdot M_{3}^{T} \\ t_{x} = l_{23}\left( m_{4} - u_{0} \right)\text{/}f_{x} \\ t_{y} = l_{23}\left( m_{8} - v_{0} \right)\text{/}f_{y} \\ t_{z} = l_{23} \\ \end{matrix} \right.\ \\ \end{matrix}$$

2 坐标变换

从像素坐标系的二维坐标$(u,v)$变换到世界坐标系的三维坐标变换$(X_{w},Y_{w},Z_{w})$过程中，存在4个未知数$X_{w},Y_{w},Z_{w},Z_{c}$，但只有3个方程，因此无法完全解出所有参数。考虑到船舶在视频中的较短运河段中航行且速度较慢，故假设运河段处于平面，且该平面为世界坐标系的$Z = 0$平面，这样则将未知数从4个减少到了3个。

令$W = \begin{bmatrix} f_{x} & 0 & u_{0} \ 0 & f_{y} & v_{0} \ 0 & 0 & 1 \ \end{bmatrix},Z_{w} = 0$，则坐标变换 可表示为

$$Z_{c}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = K\begin{bmatrix} R & T \\ 0^{T} & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ 1 \\ \end{matrix} \\ \end{bmatrix} = \left\lbrack W|0 \right\rbrack\begin{bmatrix} R & T \\ 0^{T} & 1 \\ \end{bmatrix}\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ 1 \\ \end{matrix} \\ \end{bmatrix} = W\left( R\begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ \end{matrix} \\ \end{bmatrix} + T \right)$$

等式两边同时乘$R^{- 1}W^{- 1}$可得

$$\begin{matrix} Z_{c} \cdot R^{- 1}W^{- 1}\begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ \end{matrix} \\ \end{bmatrix} + R^{- 1}T \\ \end{matrix}$$

令$R^{- 1}W^{- 1}\begin{bmatrix} u \ v \ 1 \ \end{bmatrix} = \begin{bmatrix} a \ b \ c \ \end{bmatrix},R^{- 1}T = \begin{bmatrix} \tau_{1} \ \tau_{2} \ \tau_{3} \ \end{bmatrix}$，可得

$$\begin{matrix} Z_{c}\begin{bmatrix} a \\ b \\ c \\ \end{bmatrix} = \begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ \end{matrix} \\ \end{bmatrix} + \begin{bmatrix} \tau_{1} \\ \tau_{2} \\ \tau_{3} \\ \end{bmatrix} \\ \end{matrix}$$

$$\begin{matrix} Z_{c} \cdot c = \tau_{3} \\ \end{matrix}$$

故像素坐标系的二维坐标可借助下式转换为真实世界坐标系的三维坐标

$$\begin{matrix} \begin{bmatrix} X_{w} \\ Y_{w} \\ \begin{matrix} 0 \\ \end{matrix} \\ \end{bmatrix} = \frac{\tau_{3}}{c}\begin{bmatrix} a \\ b \\ c \\ \end{bmatrix} - \begin{bmatrix} \tau_{1} \\ \tau_{2} \\ \tau_{3} \\ \end{bmatrix} \\ \end{matrix}$$