SLAM学习入门（1）针孔相机模型公式推导

图(1) 针孔相机模型

在谈到针孔相机模型时，首先要搞清楚以下几个坐标系：
世界坐标系：世界坐标系也称绝对坐标系，是一个基准坐标系，可以根据根据实际情况进行指定。世界坐标系中的点我们用($X_w$, $Y_w$, $Z_w$)^T表示。
相机坐标系：图中的O-x-y-z为相机坐标系，以光心O（也是相机的针孔）为原点，相机坐标系会随着相机的运动不断发生变化。相机坐标系中的点我们用($X_c$, $Y_c$, $Z_c$)^T表示。
图像物理坐标系：图中的O’-x’-y’为图像物理坐标系，位于CCD/CMOS的成像平面上。图像物理坐标系中的点我们用$(X^{\prime },Y^{\prime })$表示。
像素坐标系：图中的O’‘-u-v为图像像素坐标系，像素坐标系中的点用$(u,v)$表示，它表征的是像素所在的行和列。

针孔相机模型实际是将世界坐标系中的空间点映射到像素坐标系中，那么如果已知$P$点在世界坐标系中的坐标为($X_w$, $Y_w$, $Z_w$)^T，怎么求$P$点在像素坐标系下投影的坐标$(u,v)$呢？

1、世界坐标系 —> 相机坐标系

世界坐标系与相机坐标系之间的关系为欧式变换关系，将世界坐标系下的点($X_w$, $Y_w$, $Z_w$)^T转换到相机坐标系，可以通过旋转矩阵$R$和平移向量$t$进行变换得到：
$${\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]}_{4\ast 1}={\left[\begin{array}{cc}{R}_{3\ast 3}& t_{3\ast 1}\\ 0& 1\end{array}\right]}_{4\ast 4}.{\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]}_{4\ast 1}=M_{4\ast 4}.{\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]}_{4\ast 1}\text{\hspace{1em}(1)}$$
其中 M 4 ∗ 4 M_{4*4}