【视觉SLAM十四讲】4个坐标系变换总结

本文为视觉 SLAM 学习总结，总结 4 个坐标系变换的关系。

在视觉 SLAM 中，我们引入了 4 个坐标系，通过不同坐标系观测到同一个点会得到不同的坐标位置，并且存在一定的关系。

• 世界坐标系 $P_W(m)$： $(x_w,y_w,z_w)$。一般取相机的初始位置或特殊位置。
• 相机坐标系 $P_C(m)$： $(x_c,y_c,z_c)$。以相机光心为原点，见下图
• 图像坐标系 $P(mm)$： $(x,y)$。在物理成像平面上，以成像平面中心为原点
• 像素坐标系 $p$ (pixel)： $(u,v)$。在成像平面上，以成像平面左上角为原点

我们有 4 个不同的坐标系，则坐标系之间就需要 3 个变换，我们接下来一一进行讲解：

世界坐标系到相机坐标系（ $P_W\to P_C$）

世界坐标系与相机坐标系之间仅相差一个旋转和一个平移，我们将图像中的坐标系旋转到与世界坐标系相同的方向，再将两个原点通过平移重合到一起即可完成变换。详细过程见【三维空间刚体运动】。

相机坐标系到图像坐标系（ $P_C\to P$）

这部分的原理为小孔成像。

在相机坐标系中是三维坐标，投影到成像平面变为二维坐标，丢失的一个维度为焦距。其变换关系为：
$\frac{z_c}{f}=\frac{x_c}{x}=\frac{y_c}{y}$

图像坐标系到像素坐标系（ $P\to p$）

像素坐标系与图像坐标系的原点相差一个平移，但是两个坐标系的单位不同，还需要一个比例缩放。变换关系为：
$u=\alpha x+c_x$

$v=\beta y+c_y$

然后我们就可以将这三个变换联立

相机坐标系到像素坐标系

首先从图像坐标系和相机坐标系间的变换关系中解出 $x,y$，然后代入图像坐标系到像素坐标系的变换：
$u=\alpha f\frac{x_c}{z_c}+c_x$

$v=\beta f\frac{y_c}{z_c}+c_y$

写成矩阵形式：
$\left( \begin{matrix} u \\ v\\ 1 \end{matrix} \right)=\frac{1}{z_c}\left( \begin{matrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0& 1 \end{matrix} \right)\left( \begin{matrix} x_c \\ y_c\\ z_c \end{matrix} \right)=\frac{1}{z_c}KP_C=p$
当然齐次坐标中的 $z_c$ 可以省略，省略后可以表示为 $p=KP_C$。

世界坐标系到像素坐标系

这一步更加简单，只需要将 $P_C$ 替换即可：
$p=\left( \begin{matrix} u \\ v\\ 1 \end{matrix} \right)=\frac{1}{z_c}\left( \begin{matrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0& 1 \end{matrix} \right)\left( \begin{matrix} x_c \\ y_c\\ z_c \end{matrix} \right)=\frac{1}{z_c}KTP_W$