三维空间坐标转换关系

以下内容包含了2D坐标与3D坐标系之间的转换以及关于相机场的基础知识，理解这部分内容可以更快入门SLAM相关、多视角合成、三维重构等内容。

1. 相机标定

1.1 动机

• 照相机制造过程中的一些涉及到透镜精密以及组装工艺等原因需要对图像进行相应的矫正。如下所示：

• 需要建立世界坐标系到图像坐标系之间的映射关系从而保证后续的任务的进行。

1.2 张氏标定法

坐标系转换原理

其中的四个坐标系分别是世界坐标系/相机坐标系/图像坐标系/像素坐标系，各种坐标系之间可以通过转换公式联系起来，其中，相机坐标系就是观察空间，将世界坐标系的真实信息变换到相机坐标系的观察空间下就是取景变换,相机它自己位于观察空间的坐标系原点，沿z轴正向观察，x轴指向相机右侧，y轴指向相机正上方。取景变换（view transform），对应的矩阵称作观察矩阵（view matrix）。

总的来说就是四个坐标系及其之间的联系：

世界坐标系通过平移和旋转得到观察空间坐标系也就是相机坐标系。相机坐标系通过成像模型中的相似三角形原理得到图像坐标系。图像坐标系通过平移和缩放得到像素坐标系。

1.3 世界坐标系到观察空间即相机坐标系/取景变换（view transform）

（本质上就是欧拉角）外参构建平移关系 通过平移和旋转（即刚体变换）对坐标系进行相应的变换，刚体变换不改变形状变换：

通过上图可以得到下述的坐标系之间的转换公式：

$\left{\begin{array}{l}X_c=X_w \ Y_c=Y_w \times \cos \theta+Z_w \times \sin \theta \ Z_c=-Y_w \times \sin \theta+Z_w \times \cos \theta\end{array}\right.$

进一步将其转换为以x轴的矩阵的形式:

$\left[\begin{array}{c}X_c \ Y_c \ Z_c\end{array}\right]=\left[\begin{array}{ccc}1 & 0 & 0 \ 0 & \cos \theta & \sin \theta \ 0 & -\sin \theta & \cos \theta\end{array}\right]\left[\begin{array}{c}X_w \ Y_w \ Z_w\end{array}\right]=R_x\left[\begin{array}{c}X_w \ Y_w \ Z_w\end{array}\right]$

同理可得以y轴和z轴的矩阵的形式：

$\begin{aligned} & {\left[\begin{array}{l}X_c \ Y_c \ Z_c\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & 0 & -\sin \theta \ 0 & 1 & 0 \ \sin \theta & 0 & \cos \theta\end{array}\right]\left[\begin{array}{l}X_w \ Y_w \ Z_w\end{array}\right]=R_y\left[\begin{array}{l}X_w \ Y_w \ Z_w\end{array}\right]} \ & {\left[\begin{array}{l}X_c \ Y_c \ Z_c\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0 \ -\sin \theta & \cos \theta & 0 \ 0 & 0 & 1\end{array}\right]\left[\begin{array}{l}X_w \ Y_w \ Z_w\end{array}\right]=R_z\left[\begin{array}{l}X_w \ Y_w \ Z_w\end{array}\right]}\end{aligned}$

这几个绕轴其实就对应了欧拉角的几种情形：俯仰角（绕X轴）、翻滚角（绕Z轴）和偏航角（绕Y轴），以上三个部分可以由陀螺仪等传感器得到，所以旋转矩阵即$R={R_x R_y R_z}$,所以有：

因此有：

$\begin{gathered}{\left[\begin{array}{c}X_c \ Y_c \ Z_c\end{array}\right]=R\left[\begin{array}{c}X_w \ Y_w \ Z_w\end{array}\right]+T \longrightarrow\left[\begin{array}{c}X_c \ Y_c \ Z_c \ 1\end{array}\right]=\left[\begin{array}{cc}R & T \ 0^T & 1\end{array}\right]\left[\begin{array}{c}X_w \ Y_w \ Z_w \ 1\end{array}\right]} \ R: 3 \times 3 \quad T: 3 \times 1\end{gathered}$

上面这个过程就是世界坐标系到观察空间/相机坐标系的刚体变换，也叫做取景变换（view transform）。而这个相机外参矩阵也就是包含R和T的转移变换矩阵，假设计做W，也就是： 相机外参矩阵即是：

$W=\left[\begin{array}{cc}R & T \ 0^T & 1\end{array}\right]$

其中R是旋转矩阵，T是平移量。 观察空间/相机坐标系=R×世界坐标系+t=外参矩阵×世界坐标系（此公式可以进行互换） 即世界坐标系=外参x观察空间

综合来看就是完成世界坐标系到相机坐标系之间的转换需要四个参数，分别是欧拉角的三个参数和t转移矢量的一个参数即可完成转换。

1.4 相机坐标系到图像坐标系（投影变换）内参构建

通过$\frac{x_c}{x}=\frac{y_c}{y}=\frac{z_c}{f}$有如下的关系式子：

上面这个关系式子中可以看到二维坐标系（图像）和三维坐标系（相机坐标系）之间的转换是通过f也就是相机焦距$f$，相机距离图像的距离进行对应量化的。

上面的公式可以直接写为下面的形式：

$\left[\begin{array}{l}x \ y \ 1\end{array}\right]=\left[\begin{array}{llll}f & 0 & 0 & 0 \ 0 & f & 0 & 0 \ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{c}X_c \ Y_c \ Z_c \ 1\end{array}\right] / Z_c$

注意：二维坐标系转换到三维坐标系需要扩充一个维度，x和y好说，而z则是非线性的，因此需要增广形式把它扩充一个维度，

$\left{\begin{array}{l}\frac{x}{f}=\frac{X_c}{Z_c} \ \frac{y}{f}=\frac{Y_c}{Z_c}\end{array}\right.$

根据上面的式子我们也可以得到：

$\left{\begin{array}{l}x=f \frac{X_c}{Z_c} \ y=f \frac{Y_c}{Z_c}\end{array}\right.$

进一步可以得到增广的形式：

$Z_c\left[\begin{array}{l}x \ y \ 1\end{array}\right]=\left[\begin{array}{llll}f & 0 & 0 & 0 \ 0 & f & 0 & 0 \ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{c}X_c \ Y_c \ Z_c \ 1\end{array}\right]=[K \mid 0]\left[\begin{array}{c}X_c \ Y_c \ Z_c \ 1\end{array}\right]$

说明： 由于实际应用不知道主点的确切位置，因此以图像的中心位置或者图像的左上角为坐标原点建立图像的坐标系，当主点不是坐标原点的时候，相机的内参矩阵就变为了：

$K=\left[\begin{array}{ccc}\boldsymbol{f} & \mathbf{0} & \boldsymbol{x}{\mathbf{0}} \ \mathbf{0} & \boldsymbol{f} & \boldsymbol{y}{\mathbf{0}} \ \mathbf{0} & \mathbf{0} & \mathbf{1}\end{array}\right]$

K相机内参矩阵中的f和相机的焦距大小和成像大小相关，而x_0和y_0则与相机的成像平移距离有关,因此在SLAM中这些是假定已知的，而三维重建中这些参数则是未知的。这个就是光心或者主点，因此内参矩阵也写为这样的形式，这个如果提供IMU等数据的话可以观察一下是否主点是已知的：

$K=\left[\begin{array}{ccc}\boldsymbol{f} & \boldsymbol{s} & \boldsymbol{x}_0 \ \mathbf{0} & \boldsymbol{f} & \boldsymbol{y}_0 \ \mathbf{0} & \mathbf{0} & \mathbf{1}\end{array}\right]$

相机坐标系Z×图像坐标系=K×相机坐标系 或 图像坐标系=(K×相机坐标系)/深度Z

1.5 图像坐标系到像素坐标系

像素坐标系u-v的原点是$o_{uv}$,而图像坐标系x-y的坐标原点是$o(u_0,v_0)$,也就是像素坐标系的中心位置，因此每一个图像坐标系上的点在像素坐标系上的宽高分别是dx和dy,因此有下面的式子：

这个变换就是通过一个矩阵将二维图像平面转换到二维像素平面，而这个转换矩阵我们计为$submatrix$（上三角矩阵）：

$submatrix =\left[\begin{array}{ccc}1 / d x & 0 & u_0 \ 0 & 1 / d y & v_0 \ 0 & 0 & 1\end{array}\right]$

其中的1/dx和1/dy是用焦距充当了缩放因子，而u0和v0则是主点，也就是光心，而这里的s就是调整实际像平面和理想像平面。而submatrix就是我们经常说到的像平面-像素平面转换矩阵，通过这个矩阵以及相机内参矩阵完成光学射线到像素射线的转换任务。

像素坐标系=一定关系×图像坐标系

由上述的等式我们得到四个坐标系的总的转换关系式子：

即下面的公式就是总的转换关系式子：

$z_c\left[\begin{array}{c}u \ v \ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d x} & 0 & u_0 \ 0 & \frac{1}{d y} & v_0 \ 0 & 0 & 1\end{array}\right]\left[\begin{array}{cccc}f & 0 & 0 & 0 \ 0 & f & 0 & 0 \ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{cc}R & t \ 0^T & 1\end{array}\right]\left[\begin{array}{c}x_w \ y_w \ z_w \ 1\end{array}\right]$

由于相机本身的制造工艺等原因：

$\begin{gathered}u=u_0+\frac{x_d}{d x}-\frac{y_d \cot \theta}{d x} \ v=v_0+\frac{y_d}{d y \sin \theta}\end{gathered}$

因此有如下的公式：

$z_c\left[\begin{array}{c}u \ v \ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d x} & -\frac{\cot \theta}{d x} & u_0 \ 0 & \frac{1}{d y \sin \theta} & v_0 \ 0 & 0 & 1\end{array}\right]\left[\begin{array}{cccc}f & 0 & 0 & 0 \ 0 & f & 0 & 0 \ 0 & 0 & 1 & 0\end{array}\right]\left[\begin{array}{cc}R & t \ 0^T & 1\end{array}\right]\left[\begin{array}{c}x_w \ y_w \ z_w \ 1\end{array}\right]$

即：

相机标定即求解这两个内外参数矩阵 其中当把世界坐标系放在标定板处时候，Z方向即为0，此时公式就可以转换为：

$\left[\begin{array}{l}u \ v \ 1\end{array}\right]=s\left[\begin{array}{ccc}f_x & \gamma & u_0 \ 0 & f_y & v_0 \ 0 & 0 & 1\end{array}\right]\left[\begin{array}{lll}r_1 & r_2 & t\end{array}\right]\left[\begin{array}{c}x_W \ y_W \ 1\end{array}\right]$

因此H矩阵（由内参和外参组成）可以这么表示：

$H=s\left[\begin{array}{ccc}f_x & \gamma & u_0 \ 0 & f_y & v_0 \ 0 & 0 & 1\end{array}\right]\left[\begin{array}{lll}r_1 & r_2 & t\end{array}\right]=s M\left[\begin{array}{lll}r_1 & r_2 & t\end{array}\right]$

以上内容有疑问或者讲解疏漏的地方欢迎大家补充完善！！！再次感谢！！！