计算机视觉之三维重建（深入浅出SfM与SLAM核心算法）—— 2. 摄像机标定

1. 前置知识

1.1. 非齐次线性方程组求解

在相机标定任务中，需要求解线性方程组以确定相机内外参数 $x_1,x_2,...,x_q$。通过采样一系列点可以列出该方程组（如图所示），此时通常有方程个数 $p$ > 参数个数 $q$​，构成超定方程组 $Ax=y$。
当矩阵 $A$ 列满秩时，求解线性方程组 $Ax=y$ （$\mathbf{A}\in {\mathbb{R}}^{m\times n},x\in {\mathbb{R}}^n,y\in {\mathbb{R}}^m$）的方法有如下三种：

1.1.1. 传统求解方法

先证引理：若 $\mathbf{A}$ 是列满秩，则 ${\mathbf{A}}^T\mathbf{A}$ 可逆。
证：假设存在向量 $x\in {\mathbb{R}}^n$ 满足：
$${\mathbf{A}}^T\mathbf{A}x=0$$两边同时左乘 $x^T$：
$$x^T{\mathbf{A}}^T\mathbf{A}x=0\Rightarrow ||\mathbf{A}x|{|}_2^2=0$$由范数的非负性可得：
$$\mathbf{A}x=0$$由于 $\mathbf{A}$ 是列满秩（列向量线性无关），方程 $\mathbf{A}x=0$ 仅有零解 $x=0$，所以 ${\mathbf{A}}^T\mathbf{A}$ 可逆。
根据引理，$\mathbf{A}x=y\Rightarrow {\mathbf{A}}^T\mathbf{A}x={\mathbf{A}}^{T}y\Rightarrow x=({\mathbf{A}}^T\mathbf{A}{)}^{-1}{A}^{T}y$。

1.1.2. 奇异值分解法

设 $\mathbf{A}\in {\mathbb{R}}^{m\times n}$，其奇异值分解(SVD)为：$\mathbf{A}=\mathbf{U\Sigma }{\mathbf{V}}^T$，其中：

• $\mathbf{U}\in {\mathbb{R}}^{m\times m}$ 是左奇异矩阵（正交阵，$\mathbf{U}{\mathbf{U}}^T=\mathbf{I}$）
• $\mathbf{V}\in {\mathbb{R}}^{n\times n}$ 是右奇异矩阵（正交阵，$\mathbf{V}{\mathbf{V}}^T=\mathbf{I}$）
• $\mathbf{\Sigma }\in {\mathbb{R}}^{m\times n}$ 是对角阵，对角线元素 ${\sigma }_1\ge {\sigma }_2\ge \cdot \cdot \cdot \ge {\sigma }_r$ 为奇异值，$r=rank(\mathbf{A})$，其余元素为 $0$。

则有：
$$\mathbf{A}x=\mathbf{U\Sigma }{\mathbf{V}}^{T}x=0$$左乘 ${\mathbf{V}}^T$：
$${\mathbf{U}}^T\mathbf{U\Sigma }{\mathbf{V}}^T=0\Rightarrow \mathbf{\Sigma }{\mathbf{V}}^{T}x=0$$令 $y={\mathbf{V}}^{T}x$，则 $\mathbf{\Sigma }y=0$。不妨设 $\mathbf{\Sigma }=\left(\begin{array}{cc}{\Sigma }_r& 0\\ 0& 0\end{array}\right)$ 且 ${\mathbf{U}}^{T}b=c\in {\mathbb{R}}^m$，则有 ${\mathbf{\Sigma }}_{\mathbf{r}}=diag({\sigma }_1,{\sigma }_2,...,{\sigma }_r)$，同时将 $y$ 分块为 $y=\left(\begin{array}{c}{y}_r\\ y_{n-r}\end{array}\right)$，其中 $y_r\in {\mathbb{R}}^r$，$y_{n-r}\in {\mathbb{R}}^{n-r}$，则有：
$$\left(\begin{array}{cc}{\mathbf{\Sigma }}_{\mathbf{r}}& 0\\ 0& 0\end{array}\right)\left(\begin{array}{c}{y}_r\\ y_{n-r}\end{array}\right)=\left(\begin{array}{c}{c}_r\\ c_{n-r}\end{array}\right)\Rightarrow y=\left(\begin{array}{c}{y}_r\\ y_{n-r}\end{array}\right)=\left(\begin{array}{c}{\mathbf{\Sigma }}_{\mathbf{r}}^{-1}{c}_r\\ 0\end{array}\right)\Rightarrow x=\mathbf{V}\left(\begin{array}{c}{\mathbf{\Sigma }}_{\mathbf{r}}^{-1}{c}_r\\ 0\end{array}\right)$$其中 $c_r\in {\mathbb{R}}^r$，$c_{m-r}\in {\mathbb{R}}^{n-r}$。

1.1.3. 牛顿法或者梯度下降法

令 $f(\mathbf{x})=\Vert \mathbf{A}x-\mathbf{b}{\Vert }^2$，即可利用牛顿法或者梯度下降法求解 $x$。

1.2. 齐次线性方程组的最小二乘解

值得注意的是：求解齐次线性方程组的时候，令 $\Vert x\Vert =1$，实际上 $\lambda x$ 都是齐次方程组的解。

1.3. 非线性方程组的最小二乘解

2. 相机标定

相机的内、外参数矩阵描述了三维世界到二维像素的映射关系，相机标定的任务就是求解摄像机内、外参数矩阵。
如下图所示，相机标定问题抽象为：已知世界坐标系中 $P_1,P_2,...,P_n$ 的位置，及这些点在像素坐标系上投影的位置 $p_1,p_2,...,p_n$，计算出相机的内、外参数矩阵。这里用小写 $p$ 表示像素坐标系，大写 $P$ 表示世界坐标系。

值得注意的是：选取的点 $P_i$ 不能位于同一平面上。

假设世界坐标系中的点 $P_i$ 投影到像素坐标系的点 $p_i$，则有：
$$p_i=\left(\begin{array}{c}{u}_i\\ v_i\end{array}\right)=\left(\begin{array}{c}\frac{m_1{P}_i}{m_3{P}_i}\\ \frac{m_2{P}_i}{m_3{P}_i}\end{array}\right)$$即有：
$$\{\begin{array}{l}{u}_i=\frac{m_1{P}_i}{m_3{P}_i}\\ v_i=\frac{m_2{P}_i}{m_3{P}_i}\end{array}\Rightarrow \{\begin{array}{l}{u}_i(m_3{P}_i)=m_1{P}_i\\ v_i(m_3{P}_i)=m_2{P}_i\end{array}\Rightarrow \{\begin{array}{l}{m}_1{P}_i-u_i(m_3{P}_i)=0\\ m_2{P}_i-v_i(m_3{P}_i)=0\end{array}$$由于 $\mathbf{M}$ 中有 11 个未知量，所以至少需要 6 对点才能求解出 $\mathbf{M}$ 中的所有未知量。在实验中，使用多于 6 对点以获得更加鲁棒的结果。
假设有 $n$ 对点 $(p_i,P_i)$，则有方程组：
$$\{\begin{array}{l}-u_1(m_3{P}_1)+m_1{P}_1=0\\ -v_1(m_3{P}_1)+m_2{P}_1=0\\ ⋮\\ -u_n(m_3{P}_n)+m_1{P}_n=0\\ -v_n(m_3{P}_n)+m_2{P}_n=0\end{array}\Rightarrow \mathbf{P}\overrightarrow{\mathbf{m}}=0$$其中，$\mathbf{P}\stackrel{\text{def}}{=}{\left(\begin{array}{ccc}{P}_1^T& 0^T& -u_1{P}_1^T\\ 0^T& P_1^T& -v_1{P}_1^T\\ & ⋮& \\ P_n^T& 0^T& -u_n{P}_n^T\\ 0^T& P_n^T& -v_n{P}_n^T\end{array}\right)}_{2n\times 12}$ 且 $\overrightarrow{\mathbf{m}}\stackrel{\text{def}}{=}{\left(\begin{array}{c}{\mathrm{m}}_1^T\\ {\mathrm{m}}_2^T\\ {\mathrm{m}}_3^T\end{array}\right)}_{12\times 1}$。
使用 SVD 求解上述超定齐次线性方程组 $\mathbf{P}\overrightarrow{\mathbf{m}}=0$，如下图所示：

值得注意的是：实际的投影矩阵为 $\rho \mathbf{M}$，如下图所示：

假设旋转矩阵 $\mathbf{R}=\left(\begin{array}{c}{r}_1^T\\ r_2^T\\ r_3^T\end{array}\right)$，平移向量 $\mathbf{T}=\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)$，则有：
$$\begin{array}{rl}\rho \mathbf{M}=\mathbf{K}[\mathbf{R},T]& =\left(\begin{array}{ccc}\alpha & -\alpha \cot \theta & u_0\\ 0& \frac{\beta }{\sin \theta }& v_0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{cc}{r}_1^T& t_x\\ r_2^T& t_y\\ r_3^T& t_z\end{array}\right)\\ & =\left(\begin{array}{cc}\alpha r_1^T-\alpha \cot \theta r_2^T+u_0{r}_3^T& \alpha t_x-\alpha \cot \theta t_y+u_0{t}_z\\ \frac{\beta }{\sin \theta }{r}_2^T+v_0{r}_3^T& \frac{\beta }{\sin \theta }{t}_y+v_0{t}_z\\ r_3^T& t_z\end{array}\right)\end{array}$$

2.1. 相机内参数求解

2.1.1. 求解 $u_0$ 和 $v_0$

不妨设 $\mathbf{M}=[\mathbf{A},b]$，则根据 $\rho [\mathbf{A},\mathbf{b}]=[\rho \mathbf{A},\rho \mathbf{b}]=\mathbf{K}[\mathbf{R},\mathbf{T}]=[\mathbf{KR},\mathbf{KT}]$ 有 $\rho \mathbf{A}=\mathbf{KR}$，即有：
$$\rho A=\rho \left(\begin{array}{c}{a}_1^T\\ a_2^T\\ a_3^T\end{array}\right)=\left(\begin{array}{c}\alpha r_1^T-\alpha \cot \theta r_2^T+u_0{r}_3^T\\ \frac{\beta }{\sin \theta }{r}_2^T+v_0{r}_3^T\\ r_3^T\end{array}\right)=KR$$注意：这里的 $a_1^T,a_2^T,a_3^T$ 都是已知的。
由于旋转矩阵 $\mathbf{R}$ 是正交阵，所以有 $r_i^T\cdot r_j^T=0$ ($i\ne j$)（这里的 $\cdot$ 表示点积） 且 $\Vert r_i\Vert =1$。
一方面，因为 $\rho a_3^T=r_3^T\Rightarrow \Vert \rho a_3^T\Vert =\Vert r_3^T\Vert =1$，所以：
$$\rho =\pm \frac{1}{\Vert a_3\Vert }\text{\hspace{1em}(1)}$$这里还无法确定正负号。
另一方面，$$\begin{array}{rl}\rho a_1^T\cdot \rho a_3^T={\rho }^2{a}_1^T\cdot a_3^T& =(\alpha r_1^T-\alpha \cot \theta r_2^T+u_0{r}_3^T)\cdot r_3^T\\ & =\alpha r_1^T\cdot r_3^T-\alpha \cot \theta r_2^T\cdot r_3^T+u_0{r}_3^T\cdot r_3^T\\ & =u_0\end{array}$$$$\begin{array}{rl}\rho a_2^T\cdot \rho a_3^T={\rho }^2{a}_2^T\cdot a_3^T& =\left(\frac{\beta }{\sin \theta }{r}_2^T+v_0{r}_3^T\right)\cdot r_3^T\\ & =\frac{\beta }{\sin \theta }{r}_2^T\cdot r_3^T+v_0{r}_3^T\cdot r_3^T\\ & =v_0\end{array}$$所以有：
$$\{\begin{array}{l}{u}_0={\rho }^2{a}_1\cdot a_3\\ v_0={\rho }^2{a}_2\cdot a_3\end{array}\text{\hspace{1em}(2)}$$

2.1.2. 求解 $\theta$

由于旋转矩阵 $\mathbf{R}$ 是正交阵，所以有 $r_1^T\times r_2^T=r_3^T,r_1^T\times r_3^T=r_2^T,r_3^T\times r_1^T=r_2^T$（这里的 $\times$ 表示叉积）。
则有：
$$\begin{array}{rl}\rho a_1^T\times \rho a_3^T={\rho }^2(a_1\times a_3)& =\alpha (r_1^T\times r_3^T)-\alpha cot\theta (r_2^T\times r_3^T)+u_0(r_3^T\times r_3^T)\\ & =-\alpha r_2-\alpha cot\theta r_1\end{array}$$两边同时取模则有：
$${\rho }^2\Vert a_1\times a_3\Vert =\Vert \alpha r_2+\alpha cot\theta r_1\Vert$$又因为
$$\begin{array}{rl}\Vert \alpha r_2+\alpha cot\theta r_1{\Vert }^2& =(\alpha r_2+\alpha cot\theta r_1{)}^T\cdot (\alpha r_2+\alpha cot\theta r_1)={\alpha }^2+{\alpha }^{2}co{t}^2\theta \\ & ={\alpha }^2(1+\frac{co{s}^2\theta }{si{n}^2\theta })=\frac{{\alpha }^2}{si{n}^2\theta }\end{array}$$即有：
$${\rho }^2\Vert a_1\times a_3\Vert =\frac{\alpha }{sin\theta }\text{\hspace{1em}(3)}$$注意这里的 $\alpha =\frac{f_x}{dx}>0$ 且 $0<\theta \le \frac{\pi }{2}$。
又因为 $$\begin{array}{rl}\rho a_2^T\times \rho a_3^T={\rho }^2(a_2\times a_3)& =\frac{\beta }{sin\theta }{r}_2^T\times r_3^T+v_0{r}_3^T\times r_3^T\\ & =\frac{\beta }{sin\theta }{r}_1\end{array}$$所以：
$${\rho }^2\Vert a_2\times a_3\Vert =\frac{\beta }{sin\theta }\text{\hspace{1em}(4)}$$从而：
$$\{\begin{array}{l}{\rho }^2(a_1\times a_3)\cdot {\rho }^2(a_2\times a_3)=(\alpha r_2-\alpha cot\theta r_1)\cdot \frac{\beta }{sin\theta }=-\frac{\alpha \beta cot\theta }{sin\theta }=-\frac{\alpha \beta cos\theta }{si{n}^2\theta }\\ {\rho }^2\Vert a_1\times a_3\Vert \cdot {\rho }^2\Vert a_2\times a_3\Vert =\frac{\alpha \beta }{si{n}^2\theta }\end{array}$$两式相除有：
$$cos\theta =-\frac{(a_1\times a_3)\cdot (a_2\times a_3)}{\Vert a_1\times a_3\Vert \cdot \Vert a_2\times a_3\Vert }\text{\hspace{1em}(5)}$$

2.1.3. 求解 $\alpha$ 和 $\beta$

根据公式 $(3)$ 和 $(4)$，可得：
$$\{\begin{array}{l}\alpha ={\rho }^2\Vert a_1\times a_3\Vert sin\theta \\ \beta ={\rho }^2\Vert a_2\times a_3\Vert sin\theta \end{array}\text{\hspace{1em}(6)}$$
至此 5 个相机内参数已经全部求出，分别对应公式 $(1),(2),(5),(6)$。

2.2. 相机外参数求解

2.2.1. 求解旋转矩阵 $\mathbf{R}$

因为 $\rho a_3^T=r_3^T$ 且 $\rho =\pm \frac{1}{\Vert a_3\Vert }$，所以：
$$r_3=\pm \frac{a_3}{\Vert a_3\Vert }$$又因为 $${\rho }^2(a_2\times a_3)=\frac{\beta }{sin\theta }{r}_1$$即 $a_2\times a_3$ 与 $r_1$ 同方向，所以：
$$r_1=\frac{a_2\times a_3}{\Vert a_2\times a_3\Vert }$$最后有：
$$r_2=r_3\times r_1$$综上有：
$$\{\begin{array}{l}{r}_3=\pm \frac{a_3}{\Vert a_3\Vert }\\ r_1=\frac{a_2\times a_3}{\Vert a_2\times a_3\Vert }\\ r_2=r_3\times r_1\end{array}\text{\hspace{1em}(7)}$$

2.2.2. 求解平移向量 $\mathbf{T}$

因为 $\rho b=\mathbf{KT}$，所以：
$$\mathbf{T}=\rho {\mathbf{K}}^{-1}b$$由于 $\mathbf{K}=\left(\begin{array}{ccc}\alpha & -\alpha \cot \theta & u_0\\ 0& \frac{\beta }{\sin \theta }& v_0\\ 0& 0& 1\end{array}\right)$，显然 $\mathbf{K}$ 是满秩矩阵，所以 $\mathbf{K}$ 是可逆的。
至此相机的内外参数都求解出来了，如下图所示：

2.3. 径向畸变情况下的相机标定

前面讨论的内容没有考虑径向畸变的情况，实际上 $\mathbf{M}{P}_i$ 计算的是理想情况下（无径向畸变）的像素坐标 $\left(\begin{array}{c}{u}^{\prime }\\ v^{\prime }\end{array}\right)$，畸变前的像素坐标与畸变后的像素坐标 $\left(\begin{array}{c}u\\ v\end{array}\right)$ 满足如下关系：
$$\{\begin{array}{l}u=u^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\\ v=v^{\prime }(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)\end{array}$$其中，$k_1,k_2,k_3$ 是畸变系数，$r^2=(u^{\prime }{)}^2+(v^{\prime }{)}^2$。
不妨令 $\lambda =(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)$，则有：
$$\left(\begin{array}{ccc}\lambda & 0& 0\\ 0& \lambda & 0\\ 0& 0& 1\end{array}\right)\mathbf{M}{P}_i=\left(\begin{array}{c}{u}_i\\ v_i\end{array}\right)=p_i$$记 $\mathbf{Q}=\left(\begin{array}{ccc}\lambda & 0& 0\\ 0& \lambda & 0\\ 0& 0& 1\end{array}\right)\mathbf{M}=\left(\begin{array}{c}{q}_1\\ q_2\\ q_3\end{array}\right)$，则有：
$$p_i=\left(\begin{array}{c}{u}_i\\ v_i\end{array}\right)=\left(\begin{array}{c}\frac{q_1{P}_i}{q_3{P}_i}\\ \frac{q_2{P}_i}{q_3{P}_i}\end{array}\right)\Rightarrow \{\begin{array}{l}{u}_i{q}_3{P}_i=q_1{P}_i\\ v_i{q}_3{P}_i=q_2{P}_i\end{array}\Rightarrow \{\begin{array}{l}{u}_i{q}_3{P}_i-q_1{P}_i=0\\ v_i{q}_3{P}_i-q_2{P}_i=0\end{array}$$我们无法用之前的方法求解出 $k_1,k_2,k_3,m_1,m_2,m_3$，因为方程组是非线性的。因此采用非线性求解方法进行求解，如下图所示：

从初始解开始迭代，若初始解与实际相距较远，可能会很慢。为了加快算法求解的速度，可以先求解出 $m_1$ 和 $m_2$，这样后续需要求解的变量就从 $k_1,k_2,k_3,m_1,m_2,m_3$ 减少到 $k_1,k_2,k_3,m_3$，求解 $m_1$ 和 $m_2$ 的方法如下图所示：

3. Faugeras 定理

第一点证明如下：
先证必要性：如果 $\mathbf{M}$ 是透视投影矩阵，则相机内参矩阵 $\mathbf{K}$ 满足 $det(\mathbf{K})\ne 0$，旋转矩阵 $\mathbf{R}$ 满足 $det(\mathbf{R})\ne 0$，所以 $det(\mathbf{A})=det(\mathbf{KR})=det(\mathbf{K})det(\mathbf{R})\ne 0$。
再证充分性：由于 $det(\mathbf{A})\ne 0$，所以 $\mathbf{A}$ 可逆，将 $\mathbf{A}$ 进行 QR 分解有 $\mathbf{A}=\mathbf{KR}$，其中 $\mathbf{K}$ 是可逆的上三角矩阵（满足内参矩阵结构），$\mathbf{R}$ 是正交矩阵（满足旋转矩阵性质）。进而可以还原完整的相机投影矩阵：
$$\mathbf{M}=[\mathbf{A},b]=\mathbf{K}[\mathbf{R},{\mathbf{K}}^{-1}b]=\mathbf{K}[\mathbf{R},T]$$证毕。

第二点证明：
由公式 $(5)$ 可知：$cos\theta =-\frac{(a_1\times a_3)\cdot (a_2\times a_3)}{\Vert a_1\times a_3\Vert \cdot \Vert a_2\times a_3\Vert }$。因此 $\mathbf{M}$ 为零倾斜透视投影矩阵 $\iff$ $\theta =\frac{\pi }{2}$ $\iff$ $(a_1\times a_3)\cdot (a_2\times a_3)=0$。
证毕。

第三点证明：
根据公式 $(6)$ 可知 $\{\begin{array}{l}\alpha ={\rho }^2\Vert a_1\times a_3\Vert sin\theta \\ \beta ={\rho }^2\Vert a_2\times a_3\Vert sin\theta \end{array}$，则宽高比为 1 $\iff$ $\Vert a_1\times a_3\Vert =\Vert a_2\times a_3\Vert$ $\iff$ $(a_1\times a_3)\cdot (a_1\times a_3)=(a_2\times a_3)\cdot (a_2\times a_3)$。
结合第二点的证明，则可以完成第三点的证明。
证毕。