针孔相机(The pinhole camera)

1 小孔成像

  针孔相机是一个描述真实世界中3D坐标点和图像平面上对应2D坐标点关系的纯几何模型。如下图所示：

  图中，我们假设世界坐标系和相机坐标系重合。这里还有几个专有名词：

• 光心(Optical center)
• 像平面(Image plane)
• 焦距(Focal length)
• 光轴(Optical axis)
• 主点(Principal point)
  接下来,我们就要建立描述真实世界3D坐标点$[u,v,w{]}^T$和它在像平面上投影点$[x,y{]}^T$数学关系的模型。建模的原理很简单，只需要用到初中学的相似三角形。

1.1 The normalized camera

在标准相机(The normalized camera)模型里，有三个限制条件：

• 焦距为1；
• 相机坐标系和世界坐标系重合；
• 2D图像坐标系的原点和主点重合；
  直接上公式：
  $$\begin{gathered} x=\frac{u}{w} \\ y=\frac{v}{w}\text{\hspace{1em}(1)} \end{gathered}$$

1.2 Focal length parameters

标准相机模型的限制条件太多，实际意义不大。在真实的相机中，首先，焦距没有必要一定要设为1；其次，2D图像坐标系中是以像素为单位进行测量的，具有离散化的特点。所以引入了$x$和$y$轴方向上的焦长参数${\varphi }_x$和${\varphi }_y$，注意，这两个参数分别考虑了对应坐标轴方向上感光元件的间距和焦距。待入模型得：
$$\begin{gathered} x=\frac{{\varphi }_{x}u}{w} \\ y=\frac{{\varphi }_{y}v}{w}\text{\hspace{1em}(2)} \end{gathered}$$

1.3 Offset and skew parameters

进一步减少限制条件，在真实世界中，图像坐标系的原点一般位于像平面的左上角，因此引入$x$和$y$轴方向的偏置参数${\delta }_x$和${\delta }_y$，代入模型得：
$$\begin{gathered} x=\frac{{\varphi }_{x}u}{w}+{\delta }_x \\ y=\frac{{\varphi }_{y}v}{w}+{\delta }_y\text{\hspace{1em}(3)} \end{gathered}$$
根据实际经验，我们还需要引入一个参数$\gamma$，这个参数没有实际的物理意义，却可以使得模型更加精确。代入模型得：
$$\begin{gathered} x=\frac{{\varphi }_{x}u+\gamma v}{w}+{\delta }_x \\ y=\frac{{\varphi }_{y}v}{w}+{\delta }_y\text{\hspace{1em}(4)} \end{gathered}$$

1.4 Position and orientation of camera

最后，为了定义任意的世界坐标系，需要引入场景中3D坐标点在世界坐标系和相机坐标系的转换：
$$\left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\\ w^{\prime }\end{array}\right]=\left[\begin{array}{ccc}{\omega }_{11}& {\omega }_{12}& {\omega }_{13}\\ {\omega }_{21}& {\omega }_{22}& {\omega }_{23}\\ {\omega }_{31}& {\omega }_{32}& {\omega }_{33}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]+\left[\begin{array}{c}{\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]\text{\hspace{1em}(5)}$$
简写为：
$$w^{\prime }=\Omega w+\tau \text{\hspace{1em}(6)}$$

1.5 Full pinhole camera model

结合式(4)和(5)可得完整的3D世界坐标点$[u,v,w{]}^T$到2D图像坐标点$[x,y{]}^T$的数学关系为：
$$\begin{gathered} x=\frac{{\varphi }_x({\omega }_{11}u+{\omega }_{12}v+{\omega }_{13}w+{\tau }_x)+\gamma ({\omega }_{21}u+{\omega }_{22}v+{\omega }_{23}w+{\tau }_y)}{{\omega }_{31}u+{\omega }_{32}v+{\omega }_{33}w+{\tau }_z}+{\delta }_x \\ y=\frac{{\varphi }_y({\omega }_{21}u+{\omega }_{22}v+{\omega }_{23}w+{\tau }_y)}{{\omega }_{31}u+{\omega }_{32}v+{\omega }_{33}w+{\tau }_z}+{\delta }_y\text{\hspace{1em}(7)} \end{gathered}$$
内部参数(intrinsic parameters)： { ϕ x , ϕ y , γ , δ x , δ y } \{\phi_x,\phi_y,\gamma,\delta_x,\delta_y\} {ϕx​,ϕy​,γ,δx​,δy​}，也用矩阵形式表达为：
$$\Lambda =\left[\begin{array}{ccc}{\varphi }_x& \gamma & {\delta }_x\\ 0& {\varphi }_y& {\delta }_y\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(8)}$$
外部参数(extrinsic parameters)： { Ω , τ } \{\Omega,\tau\} {Ω,τ}
最终的相机模型可简化为：
$$x=pinhole[w,\Lambda ,\Omega ,\tau ]\text{\hspace{1em}(9)}$$
概率形式为：
$$Pr[x|w,\Lambda ,\Omega ,\tau ]=Nor{m}_x[pinhole[w,\Lambda ,\Omega ,\tau ],{\delta }^{2}I]\text{\hspace{1em}(10)}$$

2 齐次坐标系

引入齐次坐标系的目的是将式(7)的非线性关系式转化为线性关系式。
2D坐标点齐次坐标：
$$\tilde{x}=\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]\text{\hspace{1em}(11)}$$
2D坐标点的笛卡尔坐标到齐次坐标的转换只需要在向量后增加一个元素1，齐次坐标到笛卡尔坐标的转换只需要前两个元素除以最后一个元素。
3D坐标点齐次坐标：
$$\tilde{w}=\lambda \left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]\text{\hspace{1em}(12)}$$
3D坐标点的笛卡尔坐标到齐次坐标的转换只需要在向量后增加一个元素1，齐次坐标到笛卡尔坐标的转换只需要前两个元素除以最后一个元素。

2.1 齐次坐标下的相机模型

$$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\varphi }_x& \gamma & {\delta }_x\\ 0& {\varphi }_y& {\delta }_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\omega }_{11}& {\omega }_{12}& {\omega }_{13}& {\tau }_x\\ {\omega }_{21}& {\omega }_{22}& {\omega }_{23}& {\tau }_y\\ {\omega }_{31}& {\omega }_{32}& {\omega }_{33}& {\tau }_z\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]\text{\hspace{1em}(13)}$$
或者简记为：
$$\lambda \tilde{x}=\Lambda [\Omega \tau ]\tilde{w}\text{\hspace{1em}(14)}$$

3 学习外部参数

Perspective-n-point(PnP)

3.1 问题描述

  已知场景中$I$个不同的三D坐标点 { w i } i = 1 I \{w_i\}_{i=1}^I {wi​}i=1I​，与之对应的图像平面2D投影点 { x i } i = 1 I \{x_i\}_{i=1}^I {xi​}i=1I​和内部参数$\Lambda$，求解外部参数 { Ω , τ } \{\Omega,\tau\} {Ω,τ}：
$$\hat{\Omega },\hat{\tau }=\underset{\Omega ,\tau }{\mathrm{argmax}}[\sum _{i=1}^I\log [Pr(x_i|w_i,\Lambda ,\Omega ,\tau )]]\text{\hspace{1em}(15)}$$

3.2 求解过程

  该优化问题是非凸问题，我们不能得到目标函数的封闭解，所以只能用非线性优化求解。而非线性优化求解需要一个初始值，而这个初始值的挑选非常关键，它必须保证迭代过程最后收敛到全局最优解。所以我们用齐次坐标重新表示目标函数以求得一个满足要求的封闭形式的初始解。
  第$I$个齐次3D坐标点和对应的2D图像投影点的关系为：
$${\lambda }_i\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\varphi }_x& \gamma & {\delta }_x\\ 0& {\varphi }_y& {\delta }_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\omega }_{11}& {\omega }_{12}& {\omega }_{13}& {\tau }_x\\ {\omega }_{21}& {\omega }_{22}& {\omega }_{23}& {\tau }_y\\ {\omega }_{31}& {\omega }_{32}& {\omega }_{33}& {\tau }_z\end{array}\right]\left[\begin{array}{c}{u}_i\\ v_i\\ w_i\\ 1\end{array}\right]\text{\hspace{1em}(16)}$$
  式(16)等号两边同时左乘${\Lambda }^{-1}$得：
$${\lambda }_i\left[\begin{array}{c}{x}_i^{\prime }\\ y_i^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\omega }_{11}& {\omega }_{12}& {\omega }_{13}& {\tau }_x\\ {\omega }_{21}& {\omega }_{22}& {\omega }_{23}& {\tau }_y\\ {\omega }_{31}& {\omega }_{32}& {\omega }_{33}& {\tau }_z\end{array}\right]\left[\begin{array}{c}{u}_i\\ v_i\\ w_i\\ 1\end{array}\right]\text{\hspace{1em}(17)}$$
  消去${\lambda }_i$得：
$$\left[\begin{array}{c}({\omega }_{31}{u}_i+{\omega }_{32}{v}_i+{\omega }_{33}{w}_i+{\tau }_z)x_i^{\prime }\\ ({\omega }_{31}{u}_i+{\omega }_{32}{v}_i+{\omega }_{33}{w}_i+{\tau }_z)y_i^{\prime }\end{array}\right]=\left[\begin{array}{cccc}{\omega }_{11}& {\omega }_{12}& {\omega }_{13}& {\tau }_x\\ {\omega }_{21}& {\omega }_{22}& {\omega }_{23}& {\tau }_y\end{array}\right]\left[\begin{array}{c}{u}_i\\ v_i\\ w_i\\ 1\end{array}\right]\text{\hspace{1em}(18)}$$
  将$I$个点对应的等式全部整合为下式：
$$\left[\begin{array}{cccccccccccc}{u}_1& v_1& w_1& 1& 0& 0& 0& 0& -u_1{x}_1^{\prime }& -v_1{x}_1^{\prime }& -w_1{x}_1^{\prime }& -x_1^{\prime }\\ 0& 0& 0& 0& u_1& v_1& w_1& 1& -u_1{y}_1^{\prime }& -v_1{y}_1^{\prime }& -w_1{y}_1^{\prime }& -y_1^{\prime }\\ u_2& v_2& w_2& 1& 0& 0& 0& 0& -u_2{x}_2^{\prime }& -v_2{x}_2^{\prime }& -w_2{x}_2^{\prime }& -x_2^{\prime }\\ 0& 0& 0& 0& u_2& v_2& w_2& 1& -u_2{y}_2^{\prime }& -v_2{y}_2^{\prime }& -w_2{y}_2^{\prime }& -y_2^{\prime }\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ u_I& v_I& w_I& 1& 0& 0& 0& 0& -u_I{x}_I^{\prime }& -v_I{x}_I^{\prime }& -w_I{x}_I^{\prime }& -x_I^{\prime }\\ 0& 0& 0& 0& u_I& v_I& w_I& 1& -u_I{y}_I^{\prime }& -v_I{y}_I^{\prime }& -w_I{y}_I^{\prime }& -y_I^{\prime }\end{array}\right]\left[\begin{array}{c}{\omega }_{11}\\ {\omega }_{12}\\ {\omega }_{13}\\ {\tau }_x\\ {\omega }_{21}\\ {\omega }_{22}\\ {\omega }_{23}\\ {\tau }_y\\ {\omega }_{31}\\ {\omega }_{32}\\ {\omega }_{33}\\ {\tau }_z\end{array}\right]=0\text{\hspace{1em}(19)}$$
  此时，问题就变为形如$Ab=0$的minimum direction problem (请阅读参考资料)，即求解奇异值分解$A=UL{V}^T$并且设$\hat{b}$为$V$的最后一列。但是从$\hat{b}$提取得到的旋转矩阵$\Omega$和平移向量$\tau$与真实值有个未知标量的差别，所以还需要进一步求解：
  首先求出最接近的旋转矩阵，问题形如$\hat{\Omega }=\underset{{\Omega }^{\prime }}{\mathrm{argmin}}[{\Omega }^{\prime }-\Omega ]$,此时就可以参照Orthogonal Procrustes problem求解(请阅读参考资料)，具体为求奇异值分解$\Omega =UL{V}^T$并且设$\hat{\Omega }$为$U{V}^T$。
  最后再按照下式求解$\hat{\tau }$:
$$\hat{\tau }=\sum _{m=1}^3\sum _{n=1}^3\frac{{\hat{\Omega }}_{mn}}{{\Omega }_{mn}}\tau \text{\hspace{1em}(20)}$$

4 学习内部参数

Camera Calibration

4.1 问题描述

  已知场景中$I$个不同的三D坐标点 { w i } i = 1 I \{w_i\}_{i=1}^I {wi​}i=1I​和与之对应的图像平面2D投影点 { x i } i = 1 I \{x_i\}_{i=1}^I {xi​}i=1I​，利用最大似然方法求解内部参数 { Ω , τ } \{\Omega,\tau\} {Ω,τ}：
$$\hat{\Lambda }=\underset{\Lambda }{\mathrm{argmax}}\left[\underset{\Omega ,\tau }{\max }\right[\sum _{i=1}^I\log [Pr[x_i|w_i,\Lambda ,\Omega ,\tau ]\left]\right]\text{\hspace{1em}(21)}$$

4.2 求解过程

  由于内部参数和外部参数都为未知，所以一个简单(但效率很差)的方法是coordinate ascent method。该方法交替迭代下述两个过程：

• 更新内部参数(已知)，求解外部参数：
  $$\hat{\Omega },\hat{\tau }=\underset{\Omega ,\tau }{\mathrm{argmax}}[\sum _{i=1}^I\log [Pr(x_i|w_i,\Lambda ,\Omega ,\tau )]]\text{\hspace{1em}(22)}$$
• 更新外部参数(已知)，求解内部参数：
  $$\hat{\Lambda }=\underset{\Lambda }{\mathrm{argmax}}[\sum _{i=1}^I\log [Pr(x_i|w_i,\Lambda ,\Omega ,\tau )]]\text{\hspace{1em}(23)}$$
    由于第一步在第三节已经介绍过，所以接下来介绍第二步的解法。幸运的是，第二个问题有封闭解(不需要齐次表示)。重新描述问题为：
    已知场景中$I$个不同的三D坐标点 { w i } i = 1 I \{w_i\}_{i=1}^I {wi​}i=1I​，与之对应的图像平面2D投影点 { x i } i = 1 I \{x_i\}_{i=1}^I {xi​}i=1I​和外部参数 { Ω , τ } \{\Omega,\tau\} {Ω,τ}，求解内部参数$\Lambda$：
  $$\begin{gathered} \hat{\Lambda }=\underset{\Lambda }{\mathrm{argmax}}\left[\sum _{i=1}^I\log \right[Nor{m}_{x_i}[pinhole[w_i,\Lambda ,\Omega ,\tau ],{\sigma }^{2}I]\left]\right] \\ =\underset{\Lambda }{\mathrm{argmin}}[\sum _{i=1}^I(x_i-pinhole[w_i,\Lambda ,\Omega ,\tau ]{)}^T(x_i-pinhole[w_i,\Lambda ,\Omega ,\tau ])]\text{\hspace{1em}(24)} \end{gathered}$$
    很明显，此时问题转化为一个Least squares problem(请阅读参考资料):
  $$\begin{gathered} A_i=\left[\begin{array}{ccccc}\frac{{\omega }_{11}{u}_i+{\omega }_{12}{v}_i+{\omega }_{13}{w}_i+{\tau }_z}{{\omega }_{31}{u}_i+{\omega }_{32}{v}_i+{\omega }_{33}{w}_i+{\tau }_z}& \frac{{\omega }_{21}{u}_i+{\omega }_{22}{v}_i+{\omega }_{23}{w}_i+{\tau }_y}{{\omega }_{31}{u}_i+{\omega }_{32}{v}_i+{\omega }_{33}{w}_i+{\tau }_z}& 1& 0& 0\\ 0& 0& 0& \frac{{\omega }_{21}{u}_i+{\omega }_{22}{v}_i+{\omega }_{23}{w}_i+{\tau }_y}{{\omega }_{31}{u}_i+{\omega }_{32}{v}_i+{\omega }_{33}{w}_i+{\tau }_z}& 1\end{array}\right] \\ h=[{\varphi }_x,\gamma ,{\delta }_x,{\varphi }_y,{\delta }_y{]}^T \end{gathered}$$
    最后需要注意的是，这个求解方法实际中应用很少，因为它收敛速度非常慢。实际中一般采用该方法迭代几次，然后直接采用非线性优化方法同时优化内部参数和外部参数。

5 推理三维坐标点

Calibrated stereo reconstruction (Two calibrated cameras)
Multi-view reconstruction (Three or more calibrated cameras)

5.1 问题描述

  已知$J$个标定好并且已知位姿的相机(也即已知${\Lambda }_j,{\Omega }_j,{\tau }_j$)和场景中某个未知的三维坐标点$w$在每个相机上的2D投影点坐标 { x j } j = 1 J \{x_j\}_{j=1}^J {xj​}j=1J​，求该3D坐标点的位置：
$$\hat{w}=\underset{w}{\mathrm{argmax}}[\sum _{j=1}^J\log [Pr(x_j|w_j,{\Lambda }_j,{\Omega }_j,{\tau }_j)]]\text{\hspace{1em}(26)}$$

5.2 求解过程

  本问题同样没有封闭解，所以我们需要借助齐次坐标重新表示以求得一个合适的初始解。
  3D齐次坐标点和第$j$个相机中的2D齐次坐标的关系为：
$${\lambda }_j\left[\begin{array}{c}{x}_j\\ y_j\\ 1\end{array}\right]=\left[\begin{array}{ccc}{\varphi }_{xj}& {\gamma }_j& {\delta }_{xj}\\ 0& {\varphi }_{yj}& {\delta }_{yj}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\omega }_{11j}& {\omega }_{12j}& {\omega }_{13j}& {\tau }_{xj}\\ {\omega }_{21j}& {\omega }_{22j}& {\omega }_{23j}& {\tau }_{yj}\\ {\omega }_{31j}& {\omega }_{32j}& {\omega }_{33j}& {\tau }_{zj}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]\text{\hspace{1em}(27)}$$
  式(27)等号两边同时左乘${\Lambda }_j^{-1}$得：
$${\lambda }_j\left[\begin{array}{c}{x}_j^{\prime }\\ y_j^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\omega }_{11j}& {\omega }_{12j}& {\omega }_{13j}& {\tau }_{xj}\\ {\omega }_{21j}& {\omega }_{22j}& {\omega }_{23j}& {\tau }_{yj}\\ {\omega }_{31j}& {\omega }_{32j}& {\omega }_{33j}& {\tau }_{zj}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]\text{\hspace{1em}(28)}$$
  消去${\lambda }_j$得：
$$\left[\begin{array}{c}({\omega }_{31}u+{\omega }_{32}v+{\omega }_{33}w+{\tau }_z)x_j^{\prime }\\ ({\omega }_{31}u+{\omega }_{32}v+{\omega }_{33}w+{\tau }_z)y_j^{\prime }\end{array}\right]=\left[\begin{array}{cccc}{\omega }_{11j}& {\omega }_{12j}& {\omega }_{13j}& {\tau }_{xj}\\ {\omega }_{21j}& {\omega }_{22j}& {\omega }_{23j}& {\tau }_{yj}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]\text{\hspace{1em}(29)}$$
  进一步整合得到：
$$\left[\begin{array}{ccc}{\omega }_{31j}{x}_j^{\prime }-{\omega }_{11j}& {\omega }_{32j}{x}_j^{\prime }-{\omega }_{12j}& {\omega }_{33j}{x}_j^{\prime }-{\omega }_{13j}\\ {\omega }_{31j}{y}_j^{\prime }-{\omega }_{21j}& {\omega }_{32j}{y}_j^{\prime }-{\omega }_{22j}& {\omega }_{33j}{y}_j^{\prime }-{\omega }_{23j}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{c}{\tau }_{xj}-{\tau }_{zj}{x}_j^{\prime }\\ {\tau }_{yj}-{\tau }_{zj}{y}_j^{\prime }\end{array}\right]\text{\hspace{1em}(30)}$$
  将$J$个形如式(30)等式整合成一个大的线性方程组，然后按照Least squares problem求解可得到一个合理的初始解，最后进行非线性迭代求解。

5.3 总结

  该算法是三维重建算法的基础，它有以下特征：

• 该方法要求我们已知与场景中代求3D坐标点对应的$J$个2D投影点坐标；
• 该方法要求我们已知所有$J$个相机的内部参数和外部参数。