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

图(1) 针孔相机模型

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

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

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

世界坐标系与相机坐标系之间的关系为欧式变换关系，将世界坐标系下的点(XwX_wXw, YwY_wYw, ZwZ_wZw)^T转换到相机坐标系，可以通过旋转矩阵RRR和平移向量ttt进行变换得到：
[XcYcZc1]4∗1=[R3∗3t3∗101]4∗4.[XwYwZw1]4∗1=M4∗4.[XwYwZw1]4∗1(1)\left[ \begin{matrix} X_c \\ Y_c \\ Z_c \\ 1 \\ \end{matrix} \right] _{4*1} = \left[ \begin{matrix} R_{3*3} & t_{3*1} \\ 0 & 1 \\ \end{matrix} \right]_{4*4} . \left[ \begin{matrix} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{matrix} \right] _{4*1} = M_{4*4} . \left[ \begin{matrix} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{matrix} \right] _{4*1}\tag{1} ⎣⎢⎢⎡XcYcZc1⎦⎥⎥⎤4∗1=[R3∗30t3∗11]4∗4.⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤4∗1=M4∗4.⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤4∗1(1)
其中M4∗4M_{4*4}M4∗4称为外参矩阵。

2、相机坐标系 —> 图像物理坐标系

图(2)

根据图(2)的相似三角形有：
Zcf=−XcX′=−YcY′(2)\frac{Z_c}{f} = -\frac{X_c}{X'} = -\frac{Y_c}{Y'} \tag{2}fZc=−X′Xc=−Y′Yc(2)
其中负号表示成倒立的像，fff表示相机的焦距。

若将成像平面放到三维空间点同一边，那么式（2）可简化：
Zcf=XcX′=YcY′(3)\frac{Z_c}{f} = \frac{X_c}{X'} = \frac{Y_c}{Y'} \tag{3}fZc=X′Xc=Y′Yc(3)
即：
{X′=fXcZcY′=fYcZc(4)\begin{cases} X' = f\frac{X_c}{Z_c} \\ Y' = f\frac{Y_c}{Z_c} \\ \end{cases} \tag{4} {X′=fZcXcY′=fZcYc(4)
写成矩阵形式为：
[X′Y′1]3∗1=1Zc[f000f0001]3∗3.[XcYcZc]3∗1(5)\left[ \begin{matrix} X' \\ Y' \\ 1 \\ \end{matrix} \right] _{3*1} = \frac{1}{Z_c} \left[ \begin{matrix} f & 0 & 0 \\ 0 & f &0 \\ 0 & 0 & 1 \\ \end{matrix} \right]_{3*3} . \left[ \begin{matrix} X_c \\ Y_c \\ Z_c \\ \end{matrix} \right] _{3*1} \tag{5} ⎣⎡X′Y′1⎦⎤3∗1=Zc1⎣⎡f000f0001⎦⎤3∗3.⎣⎡XcYcZc⎦⎤3∗1(5)

3、图像物理坐标系 —> 像素坐标系

根据图（1），我们假设图像物理坐标系的原点O’在像素坐标系的坐标为(u0u_0u0,v0v_0v0)，
令dx、dy分别表示单个像素在x’和y’方向上的实际物理尺寸大小，则：

{u=X′dx+u0v=Y′dy+v0(6)\begin{cases} u = \frac{X'}{dx} + {u_0} \\ v = \frac{Y'}{dy} + {v_0} \\ \end{cases} \tag{6} {u=dxX′+u0v=dyY′+v0(6)
将式（6）写成齐次矩阵形式：

[uv1]3∗1=[1dx0u001dyv0001]3∗3.[X′Y′1]3∗1(7)\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right] _{3*1} = \left[ \begin{matrix} \frac{1}{dx} & 0 & {u_0} \\ 0 & \frac{1}{dy} & {v_0} \\ 0 & 0 & 1 \\ \end{matrix} \right]_{3*3} . \left[ \begin{matrix} X' \\ Y' \\ 1 \\ \end{matrix} \right] _{3*1} \tag{7} ⎣⎡uv1⎦⎤3∗1=⎣⎡dx1000dy10u0v01⎦⎤3∗3.⎣⎡X′Y′1⎦⎤3∗1(7)
联立式（5）（7），将X′X'X′、Y′Y'Y′用XcX_cXc、YcY_cYc表示得到：
[uv1]3∗1=1Zc[1dx0u001dyv0001]3∗3.[f000f0001]3∗3.[XcYcZc]3∗1(8)\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right] _{3*1} = \frac{1}{Z_c} \left[ \begin{matrix} \frac{1}{dx} & 0 & {u_0} \\ 0 & \frac{1}{dy} & {v_0} \\ 0 & 0 & 1 \\ \end{matrix} \right]_{3*3} . \left[ \begin{matrix} f & 0 & 0 \\ 0 & f &0 \\ 0 & 0 & 1 \\ \end{matrix} \right]_{3*3} . \left[ \begin{matrix} X_c \\ Y_c \\ Z_c \\ \end{matrix} \right] _{3*1} \tag{8} ⎣⎡uv1⎦⎤3∗1=Zc1⎣⎡dx1000dy10u0v01⎦⎤3∗3.⎣⎡f000f0001⎦⎤3∗3.⎣⎡XcYcZc⎦⎤3∗1(8)
再将式（1）与式（8）联立，将XcX_cXc、YcY_cYc用XwX_wXw、YwY_wYw表示：
[uv1]3∗1=1Zc[1dx0u001dyv0001]3∗3.[f0000f000010]3∗4.[R3∗3t3∗101]4∗4.[XwYwZw1]4∗1(9)\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right] _{3*1} = \frac{1}{Z_c} \left[ \begin{matrix} \frac{1}{dx} & 0 & {u_0} \\ 0 & \frac{1}{dy} & {v_0} \\ 0 & 0 & 1 \\ \end{matrix} \right]_{3*3} . \left[ \begin{matrix} f & 0 & 0 & 0\\ 0 & f & 0 & 0\\ 0 & 0 & 1 & 0\\ \end{matrix} \right]_{3*4} . \left[ \begin{matrix} R_{3*3} & t_{3*1}\\ 0 & 1\\ \end{matrix} \right]_{4*4}. \left[ \begin{matrix} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{matrix} \right] _{4*1} \tag{9} ⎣⎡uv1⎦⎤3∗1=Zc1⎣⎡dx1000dy10u0v01⎦⎤3∗3.⎣⎡f000f0001000⎦⎤3∗4.[R3∗30t3∗11]4∗4.⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤4∗1(9)
令fxf_xfx = f/dxf/dxf/dx，fyf_yfy=f/dyf/dyf/dy，则式（9）可简化为：
[uv1]3∗1=1Zc[fx0u000fyv000010]3∗4.[R3∗3t3∗101]4∗4.[XwYwZw1]4∗1(10)\left[ \begin{matrix} u \\ v \\ 1 \\ \end{matrix} \right] _{3*1} = \frac{1}{Z_c} \left[ \begin{matrix} f_x & 0 & {u_0} & 0 \\ 0 & f_y & {v_0} & 0 \\ 0 & 0 & 1 & 0 \\ \end{matrix} \right]_{3*4}. \left[ \begin{matrix} R_{3*3} & t_{3*1}\\ 0 & 1\\ \end{matrix} \right]_{4*4}. \left[ \begin{matrix} X_w \\ Y_w \\ Z_w \\ 1 \\ \end{matrix} \right] _{4*1} \tag{10} ⎣⎡uv1⎦⎤3∗1=Zc1⎣⎡fx000fy0u0v01000⎦⎤3∗4.[R3∗30t3∗11]4∗4.⎣⎢⎢⎡XwYwZw1⎦⎥⎥⎤4∗1(10)
式（10）中fxf_xfx、fyf_yfy、u0u_0u0、v0v_0v0称为相机的内参数，当相机给定时是固定的；RRR和ttt为相机的外参数，反映的是世界坐标系与相机坐标系之间的关系。

参考文献：
1、高翔，视觉slam十四讲[M]