四、相机标定求解

1. 单应性矩阵

在学习相机标定求解之前，先了解一个概念：单应性矩阵，其定义为从一个平面到另一个平面的投影映射，因此可以将世界坐标系中的物体平面到成成像平面之间的映射理解为单应性关系。我们知道相机标定一般使用标定板进行标定，因此世界坐标系中的平面就是标定板平面。

假设标定板平面上一点 $Q=[X,Y,Z,1]^{T}$ ，映射到成像平面上的点为 $q=[x,y,1]^{T}$ ,两者之间满足单应性关系： $q=s\cdot H\cdot Q$ , 其中H为单应性矩阵，s为尺度因子，单应性矩阵有8个参数，即8个参数需要求解，因此需要8个方程，通过4个点可以得到8个方程，因此至少需要4个点就可以求解单应性关系。

opencv提供了一个接口：fingHomography用于求解单应性矩阵。

2. 基本方程

2.1 基本变量

定义像素坐标系中的点 $m=[u,v]^{T}$ ,对应世界坐标系中的三维空间点 $M= [X,Y,Z]^{T}$ ,为方便计算，添加一个1得到增广向量 $\widetilde{m}=[u,v,1]^{T}$ ， $\widetilde{M}=[X,Y,Z,1]^{T}$ ,根据之前章节提到的小孔成像原理，三维点到投影点的关系可描述为： $s\widetilde{m}=A[R,t]\widetilde{M}$ ,其中s 为尺度因子，[R,t]为外参旋转平移矩阵。A为内参：

$A = \begin{bmatrix} f_{x} &\gamma &u_{0} \\ 0& f_{y} &v_{0} \\ 0&0 &1 \end{bmatrix}$

$\gamma$ 为两个坐标系的倾斜角，前面没有提到，因为默认两个坐标轴是垂直的，但实际上可能不垂直。

2.2 相机标定

我们可以假设，标定板平面世界坐标系的z坐标为0，R的第i列为 $r_{i}$ ,那么可以得到：

$s\begin{bmatrix} u\\v \\ 1 \end{bmatrix}=A\begin{bmatrix} r_{1} & r_{2} &r_{3} &t \end{bmatrix}\begin{bmatrix} X\\ Y\\ 0\\ 1 \end{bmatrix}=A\begin{bmatrix} r_{1} &r_{2} &t \end{bmatrix}\begin{bmatrix} X\\ Y\\ 1 \end{bmatrix}$

标定平面和成像平面的单应性关系： $s\widetilde{m}=H\widetilde{M}$ , with $H =A\begin{bmatrix} r_{1} & r_{2} &t \end{bmatrix}$ .\

如前所述，我们可以通过两个平面上的多个点求解单应性矩阵H

假设 $H=\begin{bmatrix} h_{1} & h_{2} & h_{3} \end{bmatrix}$ , $\Rightarrow$ $\begin{bmatrix} h_{1} &h_{2} &h_{3} \end{bmatrix} = \lambda A\begin{bmatrix} r_{1}& r_{2} &t \end{bmatrix}$

根据上式可得：

$\left\{\begin{matrix} h_{1}=\lambda \cdot A\cdot r_{1}\\ h_{2}=\lambda \cdot A\cdot r_{2} \\ h_{3}=\lambda \cdot A\cdot t \end{matrix}\right.$

从而得：

$\left\{\begin{matrix} r_{1}=\frac{1}{\lambda }\cdot A^{-1}\cdot h_{1} \\ r_{2}=\frac{1}{\lambda }\cdot A^{-1}\cdot h_{2} \\ t=\frac{1}{\lambda }\cdot A^{-1}\cdot h_{3} \end{matrix}\right.$

相机成像有两个约束条件：

1. 旋转向量 $r_{1},r_{2}$ 分别代表绕x和y轴得旋转，所以这两个向量是正交的，故：

$r_{1}^{T}r_{2}=0$ , 从而得： $h_{1}^{T}A^{-T}A^{-1}h_{2}=0$

2.旋转向量是单位向量，故而有：

$r_{1}^{T}r_{1}=r_{2}^{T}r_{2}$ 及 $\left | \left |r_{1} \right | \right |=\left | \left |r_{2} \right | \right |$

进而得：

$h_{1}^{T}A^{-T}A^{-1}h_{1}=h_{2}^{T}A^{-T}A^{-1}h_{2}$

3. 封闭解

上面讲到相机成像的两个约束条件。

接下来定义中间变量 $B= A^{-T}AA^{-1}=\begin{bmatrix} B_{11} &B_{12} &B_{13} \\ B_{21}& B_{22} &B_{23} \\ B_{31}& B_{32} &B_{33} \end{bmatrix}=\begin{bmatrix} \frac{1}{f_{x}^{2}} & -\frac{\gamma }{f_{x}^{2}f_{y}} & \frac{v_{0}\gamma -u_{0}f_{y}}{f_{x}^{2}f_{y}}\\ -\frac{\gamma }{f_{x}^{2}f_{y}} &\frac{\gamma ^{2}}{f_{x}^{2}f_{y}^2} +\frac{1}{f_{y}^2} & -\frac{\gamma (v_{0}\gamma -u_{0}f_{y}))}{f_{x}^{2}f_{y}^2}-\frac{v_{0}}{f_{y}^2}\\ \frac{v_{0}\gamma -u_{0}f_{y}}{f_{x}^{2}f_{y}}& -\frac{\gamma (v_{0}\gamma -u_{0}f_{y}))}{f_{x}^{2}f_{y}^2}-\frac{v_{0}}{f_{y}^2} & -\frac{(v_{0}\gamma -u_{0}f_{y}))}{f_{x}^{2}f_{y}^2}+\frac{v_{0}^2}{f_{y}^2}+1 \end{bmatrix}$

B是对称矩阵，故只有6个参数需要求解，定义 $b=\begin{bmatrix} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} &B_{33} \end{bmatrix}$

对单应性矩阵的列向量进行分解： $h_{i}=\begin{bmatrix} h_{i1} &h_{i2} & h_{i3} \end{bmatrix}$

代入上述公式得到： $h_{i}^{T}Bh_{j}=v_{ij}b$

$v_{ij}=\begin{bmatrix} h_{i1}h_{j1}& h_{i1}h_{j2}+h_{i2}h_{j1} &h_{i2}h_{j2} &h_{i3}h_{j1}+h_{i1}h_{j3} &h_{i3}h_{j2}+h_{i2}h_{j3} & h_{i3}h_{j3} \end{bmatrix}$

代入上面的公式可得：

$\begin{bmatrix} v_{12}^{T}\\ (v_{11}-v_{22})^{T}) \end{bmatrix}b=0$

通过拍摄N张标定板图片，可将上述N个方程放在一起，可以得到 $Vb=0$

V是2n*6的矩阵，b有6个参数，通过求解方程就可以计算出b，由于b有6个参数，因此至少需要3个单应性矩阵，也就是说至少需要拍摄3张不同角度的标定板图片。

计算出b后，就可以求解相机内参：

$v_{0}=(B_{12}B_{13}-B_{11}B_{23})/(B_{11}B_{22}-B_{12}^{2})$

$\lambda =B_{33}-[B_{13}^2+v_{0}(B_{12}B_{13}-B_{11}B_{23})]/B_{11}$

$f_{x}=\sqrt{\lambda /B_{11}}$

$f_{y}=\sqrt{\lambda B_{11}/(B_{11}B_{22}-B_{12}^2)}$

$\gamma =-B_{12}f_{x^2}f_{y}/\lambda$

$u_{0}=\gamma v_{0}/f_{y}-B_{13}f_{x}^2/\lambda$

计算旋转平移矩阵： $\begin{bmatrix} h_{1} &h_{2} &h_{3} \end{bmatrix}=\lambda A\begin{bmatrix} r_{1} &r_{2} &t \end{bmatrix}$ , 从而得：

$r_{1}=\lambda ^{-1}\cdot A^{-1}\cdot h_{1}$

$r_{2}=\lambda ^{-1}\cdot A^{-1}\cdot h_{2}$

$r_{3}=r_{1}\times r_{2}$

$t=\lambda ^{-1}\cdot A^{-1}\cdot h_{3}$

4. 求解径向畸变参数

前面我们有谈到径向畸变模型：

$\left\{\begin{matrix} x_{distorted} = x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}))\\ y_{distorted} = y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6})) \end{matrix}\right.$

其中 $r^{2}=x^{2}+y^{2}$ . 径向畸变一般只考虑前2项：

故：

$\left\{\begin{matrix} \widehat{x}=x+x[k_{1}(x^2+y^2)+k_{2}(x^2+y^2)^2)]\\ \widehat{y}=y+y[k_{1}(x^2+y^2)+k_{2}(x^2+y^2)^2)] \end{matrix}\right.$

其中 $(x,y)$ 为点在图像坐标系中的理想坐标，也即没有任何畸变时的坐标， $(\widehat{x},\widehat{y})$ 为图像坐标系中真实点的坐标，也即畸变后的坐标。

在像素坐标系中:

$\left\{\begin{matrix} \widehat{u}=u+(u-u_{0})[k_{1}(x^2+y^2)+k_{2}(x^2+y^2)^2)]\\ \widehat{v}=v+(v-v_{0})[k_{1}(x^2+y^2)+k_{2}(x^2+y^2)^2)] \end{matrix}\right.$

其中 $(u,v)$ 为点在像素坐标系中的理想坐标，也即没有任何畸变时的坐标， $(\widehat{u},\widehat{v})$ 为像素坐标系中真实点的坐标，也即畸变后的坐标。

上述方程表述为矩阵形式：

$\begin{bmatrix} (u-u_{0}(x^2+y^2) &(u-u_{0}(x^2+y^2)^2 \\ (v-v_{0}(x^2+y^2) & (v-v_{0}(x^2+y^2)^2 \end{bmatrix}\begin{bmatrix} k_{1}\\k_{2} \end{bmatrix}=\begin{bmatrix} \widehat{u}-u\\ \widehat{v}-v \end{bmatrix}$

其中( $u,v$ )可以通过相机内外参求解得到， $(\widehat{u},\widehat{v})$ 通过图像识别角点获取，(x,y)同样的道理。

假设拍摄n张标定板图片，每张图片有m个角点，则可以得到 $2mn$ 个方程，写成矩阵形式：

$DK=d$ ，其中 $K=\begin{bmatrix} k_{1} &k_{2} \end{bmatrix}^{T}$

根据最小二乘法求解方程组（最小化均方根误差寻找数据的最佳匹配，用于求解方程个数比未知数多的情况），根据上述方程就可以求解出畸变系数。