通过小孔成像模型，来结算几个坐标系之间的关系。
分别为机身及摄像机坐标系，图像坐标系和世界坐标系。
相机坐标系与机身坐标系
摄像机坐标系（光心坐标系） $C=[x_C, y_C, z_C]^T$以摄像机光心$O_C$为原点，其$z_C$轴与光轴重合，水平方向为$x_C$轴，$y_C$轴垂直于$X_C, O_c, Z_C$平面，焦距$f$为光心到像平面的距离。
机身坐标系$B=[x_B, y_B, z_B]^T$，即无人机最中心的坐标。
摄像机坐标系和机身坐标系之间存在一个安装误差，用$E=\begin{bmatrix}
\alpha & 0 & 0 \\\
0 & \beta & 0 \\\
0 & 0 & \gamma
\end{bmatrix}
\begin{bmatrix}
x_e & 0 & 0 \\\
0 & y_e & 0 \\\
0 & 0 & z_e
\end{bmatrix}$
表示，其中$[\alpha, \beta, \gamma]^T$为安装的误差角，$[x_e, y_e, z_e]^T$为摄像机到机身坐标原点的空间距离。
摄像机坐标系与机身坐标系之间的关系为：
相机坐标系与世界坐标系
世界坐标系：是客观三维世界的绝对坐标系，也称客观坐标系。对于空间中的一点$P_E=(x_E, y_E, z_E)$，其对应的相机坐标系的坐标$(x_C, y_C, z_C)$与相机的姿态角和GPS（所在的位置）有关。在无人机飞行的过程中，可以通过IMU实时获取其姿态角信息$(\phi, \theta, \varphi)$，以便对摄像头姿态校正。
相机坐标系与世界坐标系**之间的关系为：
其中，$R(\phi, \theta, \varphi)$为机身坐标与地面坐标的转换矩阵，$t $为相机坐标与地面坐标间的平移向量，其中$M=
\begin{bmatrix}
R(\phi, \theta, \varphi) & t \\
0 & 1
\end{bmatrix} $，为相机外部参数矩阵。
像素坐标系与物理坐标系
图像像素坐标系$[u, v]$，该坐标系没有物理单位。像素坐标系中的坐标点$(u_1, v_1)$代表图像在存储数组中的行和列位置，该位置存储的是图像的颜色或灰度信息。
图像物理坐标系$I=[x_I, y_I]$，其中$x_I$，$y_I$轴分别和$x_C$，$y_C$轴平行。物理坐标系中的坐标点单位为毫米，如果一个像素点对应物理坐标系中$x_I$轴方向物理尺寸${\rm d}x$和$y_I$轴方向物理尺寸${\rm d}y$，${\rm d}x$和${\rm d}y$与摄像机焦距$f$有关。
物理坐标系上的一点$I_1=(x_1, y_1)$与其像素坐标系中的点$(u_1, v_1)$对应关系为：
其中，$(u_0, v_0)$为图像像素坐标系中的中心点，也即图像物理坐标系的原点所对应的像素点，$K= \begin{bmatrix}
\frac{1}{dx} & 0 & u_0 \\
0 & \frac{1}{dy} & v_0 \\
0 & 0 & 1
\end{bmatrix}$，为相机的内参矩阵，包含是个与相机内部结构有关的参数。
相机坐标系与物理坐标系
假设摄像机坐标系中一点$P=(x_C, y_C, z_C,)$，连接光心在$O_C$物理坐标系中的投影点为$P_{I1}=(x_I, y_I)$，则这两点之间的坐标转换关系为：
转化为矩阵形式为：
像素坐标系与世界坐标系
结合摄像机与世界坐标系 (2)，摄像机与物理坐标系 (5)，以及像素与物理坐标系 (3)，可得出像素坐标系与世界坐标系之间关系为：
由上式可知，从空间中（世界坐标系）一点转换到像素坐标系中一点，只有一个位置变量（比例变量）$z_C$，即 (7) 式：
当得知摄像机高度 $h$，姿态角$(\phi, \theta, \varphi)$，以及空间（世界坐标系）中一点在物理坐标系中的投影$(x_I, y_I)$，即可通过上式来计算出空间该点的世界坐标系坐标$(x_E, y_E, z_E)$。
具体步骤：
- 由$(u_1, v_1)$通过式 (3) 求$x_I$；
- 由$x_I$通过式 (7) 求比例系数$z_C$；
- 由$(u_1, v_1)$通过式 (6) 求$(x_E, y_E, z_E)$
参考：
