张正友标定法

张正友标定法介绍

张正友标定法是基于针孔相机模型的，张正友标定法利用如下图所示的棋盘格标定板，在得到一张标定板的图像之后，可以利用相应的图像检测算法得到每一个角点的像素坐标 $(u,v)$ 。

张正友标定法将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标 $Z=0$，由于标定板的世界坐标系是人为事先定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到每一个角点在世界坐标系下的物理坐标$(X,Y,Z=0)$。

我们将利用这些信息：每一个角点的像素坐标 $(u,v=0)$ 、每一个角点在世界坐标系下的物理坐标$(X,Y,Z=0)$，来进行相机的标定，获得相机的内外参矩阵、畸变参数。

标定相机的内参矩阵和外参矩阵

张正友标定法标定相机的内外参数的思路如下：

1）求解内参矩阵与外参矩阵的积；

2）求解内参矩阵；

3）求解外参矩阵。

1、求解内参矩阵与外参矩阵的积
将世界坐标系固定于棋盘格上，则棋盘格上任一点的物理坐标 $Z=0$ ，因此，原单点无畸变的成像模型可以化为下式。其中， $R_1,R_2$ 为旋转矩阵 $R$ 的前两列。为了简便，将内参矩阵记为 $A$ 。

$Z\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}\frac{f}{dX}& -\frac{f\cot \theta }{dX}& u_0\\ 0& \frac{f}{dY\sin \theta }& v_0\\ 0& 0& 1\end{array}\right)(\begin{array}{ll}R1& R2\end{array}$
$T)\left(\begin{array}{l}X\\ Y\\ 1\end{array}\right)=A\left(\begin{array}{lll}R1& R2& T\end{array}\right)\left(\begin{array}{l}X\\ Y\\ 1\end{array}\right)$

我们对于上式做一定的说明。
对于不同的图片，内参矩阵 $A$ 为定值；
对于同一张图片，内参矩阵$A$，外参矩阵 $(R_1,R_2,T)$ 为定值；
对于同一张图片上的单点，内参矩阵$A$，外参矩阵 $(R_1,R_2,T)$ ，尺度因子$Z$为定值。

我们将 $A(R_1,R_2,T)$ 记为矩阵 $H$， $H$ 即为内参矩阵和外参矩阵的积，记矩阵 $H$ 的三列为 $(H_1,H_2,H_3)$ ，则有：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\frac{1}{Z}H\left(\begin{array}{l}X\\ Y\\ 1\end{array}\right)=\frac{1}{Z}\left[\begin{array}{lll}{H}_{11}& H_{12}& H_{13}\\ H_{21}& H_{22}& H_{23}\\ H_{31}& H_{32}& H_{33}\end{array}\right]\left(\begin{array}{c}X\\ Y\\ 1\end{array}\right)$

利用上式，消去尺度因子 $Z$，可得：

$\begin{array}{rl}u& =\frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}}\\ v& =\frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}}\end{array}$

此时，尺度因子 $Z$ 已经被消去，因此上式对于同一张图片上所有的角点均成立。$(u,v)$ 是像素坐标系下的标定板角点的坐标， $(U,V)$ 是世界坐标系下的标定板角点的坐标。通过图像识别算法，我们可以得到标定板角点的像素坐标$(u,v)$ ，又由于标定板的世界坐标系是人为定义好的，标定板上每一个格子的大小是已知的，我们可以计算得到世界坐标系下的$(U,V)$ 。

由这里的 $H$ 是齐次矩阵，有8个独立未知元素。每一个标定板角点可以提供两个约束方程（ $u,U,V$ 的对应关系、 $u,U,V$ 的对应关系提供了两个约束方程），因此，当一张图片上的标定板角点数量等于4时，即可求得该图片对应的矩阵 $H$ 。

当一张图片上的标定板角点数量大于4时，利用最小二乘法回归最佳的矩阵$H$。

此时$H$矩阵已知，

2、求解内参矩阵

我们已知了矩阵 $H=A(R_1,R_2,T)$ ，接下来需要求解相机的内参矩阵 $A$ 。
我们利用$R_1,R_2$作为旋转矩阵 $R$ 的两列，存在单位正交的关系，即：
$R{1}^{T}R2=0$
$R{1}^{T}R1=R{2}^{T}R2=1$

则由 $H$ 和 $R_1,R_2$ 的关系，可知：
$R1=A^{-1}H1$
$R2=A^{-1}H2$

代入可得：
$H{1}^T{A}^{-T}{A}^{-1}H2=0$
$H{1}^T{A}^{-T}{A}^{-1}H1=H{2}^T{A}^{-T}{A}^{-1}H2=1$

另外，我们发现，上述两个约束方程中均存在矩阵 $A^{-T}{A}^{-1}$ 。因此，我们记$B=A^{-T}{A}^{-1}$。我们试图先求解出矩阵 $B$ ，通过矩阵 $B$ 再求解相机的内参矩阵 $A$。

同时，为了简便，我们记相机内参矩阵 $A$ 为：

$A=\left(\begin{array}{cccc}\frac{f}{dX}& -\frac{f\cot \theta }{dX}& u_0& 0\\ 0& \frac{f}{dY\sin \theta }& v_0& 0\\ 0& 0& 1& 0\end{array}\right)=\left[\begin{array}{ccc}\alpha & \gamma & u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$

$A^{-1}=\left[\begin{array}{ccc}\frac{1}{\alpha }& -\frac{\gamma }{\alpha \beta }& \frac{\gamma v_0-\beta u_0}{\alpha \beta }\\ 0& \frac{1}{\beta }& -\frac{v_0}{\beta }\\ 0& 0& 1\end{array}\right]$

则用矩阵 $A$ 表示矩阵 $B$ 得：
$B=A^{-T}{A}^{-1}=\left[\begin{array}{ccc}\frac{1}{{\alpha }^2}& -\frac{\gamma }{{\alpha }^2\beta }& \frac{\gamma v_0-\beta u_0}{{\alpha }^2\beta }\\ -\frac{\gamma }{{\alpha }^2\beta }& \frac{1}{{\beta }^2}+\frac{{\gamma }^2}{{\alpha }^2{\beta }^2}& \frac{\gamma \left(\beta u_0-\gamma v_0\right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}\\ \frac{\gamma v_0-\beta u_0}{{\alpha }^2\beta }& \frac{\gamma \left(\beta u_0-\gamma v_0\right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}& \frac{{\left(\beta u_0-\gamma v_0\right)}^2}{{\alpha }^2{\beta }^2}+\frac{v_0^2}{{\beta }^2}+1\end{array}\right]=\left[\begin{array}{ccc}{B}_{11}& B_{12}& B_{13}\\ B_{12}& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]$

注意：由于$B$为对称阵，上式出现了两次 $B_{12},B13,B23$ 。

这里，我们可以使用 $B=A^{-T}{A}^{-1}$ 将前面通过 $R_1,R_2$ 单位正交得到的约束方程化为：
$H{1}^{T}BH2=0$
$H{1}^{T}BH1=H{2}^{T}BH2=1$

因此，为了求解矩阵$B$ ，我们必须计算 $H{i}^{T}BHj=0$ 。则：
$H_i^{T}B{H}_j=\left[\begin{array}{lll}{H}_{1i}& H_{2i}& H_{3i}\end{array}\right]\left[\begin{array}{lll}{B}_{11}& B_{12}& B_{13}\\ B_{12}& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]\left[\begin{array}{c}{H}_{1j}\\ H_{2j}\\ H_{3j}\end{array}\right]$
$=\left[\begin{array}{lllllc}{H}_{1i}{H}_{1j}& H_{1i}{H}_{2j}+H_{2i}{H}_{1j}& H_{2i}{H}_{2j}& H_{1i}{H}_{3j}& +H_{3i}{H}_{1j}{H}_{2i}{H}_{3j}+H_{3i}{H}_{2j}& H_{3i}{H}_{3j}\end{array}\right]\left[\begin{array}{l}{B}_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33}\end{array}\right]$

上述方程看起来有点复杂，但是其实不然，我们可以记：
$\begin{array}{cc}{v}_{ij}={\left[\begin{array}{lllllc}{H}_{1i}{H}_{1j}& H_{1i}{H}_{2j}+H_{2i}{H}_{1j}& H_{2i}{H}_{2j}& H_{1i}{H}_{3j}+H_{3i}{H}_{1j}& H_{2i}{H}_{3j}+H_{3i}{H}_{2j}& H_{3i}{H}_{3j}\end{array}\right]}^T& \\ b={\left[\begin{array}{llllll}{B}_{11}& B_{12}& B_{22}& B_{13}& B_{23}& B_{33}\end{array}\right]}^T& \end{array}$

则上述方程化为：$H_i^{T}B{H}_j=v_{ij}b$

此时，通过 $R_1,R_2$ 单位正交得到的约束方程可化为：

$v_{12}^{T}b=0$
$v_{11}^{T}b=v_{22}^{T}b=1$

即：

$\left[\begin{array}{c}{v}_{12}^T\\ v_{11}^T-v_{22}^T\end{array}\right]b=vb=0$

其中：$v=\left[\begin{array}{c}{v}_{12}^T\\ v_{11}^T-v_{22}^T\end{array}\right]$

由于矩阵 $H$ 已知，矩阵 $v$ 又全部由矩阵 $H$ 的元素构成，因此矩阵$v$已知。

此时，我们只要求解出向量$b$ ，即可得到矩阵 $B$。每张标定板图片可以提供一个 $vb=0$ 的约束关系，该约束关系含有两个约束方程。但是，向量 $b$ 有6个未知元素。因此，单张图片提供的两个约束方程是不足以解出来向量 $b$。因此，我们只要取3张标定板照片，得到3个$vb=0$ 的约束关系，即6个方程，即可求解向量 $b$。当标定板图片的个数大于3时（事实上一般需要15到20张标定板图片），可采用最小二乘拟合最佳的向量 $b$，并得到矩阵 $B$。($B$只与内参有关)

$B=\left[\begin{array}{ccc}\frac{1}{{\alpha }^2}& -\frac{\gamma }{{\alpha }^2\beta }& \frac{\gamma v_0-\beta u_0}{{\alpha }^2\beta }\\ -\frac{\gamma }{{\alpha }^2\beta }& \frac{1}{{\beta }^2}+\frac{{\gamma }^2}{{\alpha }^2{\beta }^2}& \frac{\gamma \left(\beta u_0-\gamma v_0\right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}\\ \frac{\gamma v_0-\beta u_0}{{\alpha }^2\beta }& \frac{\gamma \left(\beta u_0-\gamma v_0\right)}{{\alpha }^2{\beta }^2}-\frac{v_0}{{\beta }^2}& \frac{{\left(\beta u_0-\gamma v_0\right)}^2}{{\alpha }^2{\beta }^2}+\frac{v_0^2}{{\beta }^2}+1\end{array}\right]=\left[\begin{array}{ccc}{B}_{11}& B_{21}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]$

根据矩阵 $B$ 的元素和相机内参 $\alpha ,\beta ,\gamma ,u_0,v_0$ 的对应关系（如上式），可得到：

$\begin{array}{rl}{v}_0& =\frac{B_{12}{B}_{13}-B_{11}{B}_{23}}{B_{11}{B}_{22}-B_{12}^2}\\ & \alpha =\sqrt{\frac{1}{B_{11}}}\\ \beta & =\sqrt{\frac{B_{11}}{B_{11}{B}_{22}-B_{12}^2}}\\ \gamma & =-B_{12}{\alpha }^2\beta \\ u_0& =\frac{\gamma v_0}{\beta }-B_{13}{\alpha }^2\end{array}$

即可求得相机的内参矩阵 :
$A=\left(\begin{array}{cccc}\frac{f}{dX}& -\frac{f\cot \theta }{dX}& u_0& 0\\ 0& \frac{f}{dY\sin \theta }& v_0& 0\\ 0& 0& 1& 0\end{array}\right)=\left[\begin{array}{ccc}\alpha & \gamma & u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$

3、求解外参矩阵

这里再次强调一下，对于同一个相机，相机的内参矩阵取决于相机的内部参数，无论标定板和相机的位置关系是怎么样的，相机的内参矩阵不变。这也正是在第2部分“求解内参矩阵”中，我们可以利用不同的图片（标定板和相机位置关系不同）获取的矩阵 $H$ ，共同求解相机内参矩阵 $A$ 的原因。

但是，外参矩阵反映的是标定板和相机的位置关系。对于不同的图片，标定板和相机的位置关系已经改变，此时每一张图片对应的外参矩阵都是不同的。

在关系：$A(R1R2T)=H$ 中，我们已经求解得到了矩阵 $H$（对于同一张图片相同，对于不同的图片不同）、矩阵 $A$ （对于不同的图片都相同）。通过公式： $\left(\begin{array}{lll}R1& R2& T\end{array}\right)=A^{-1}H$ ，即可求得每一张图片对应的外参矩阵 $A(R1R2T)$ 。

注意，这里值得指出，完整的外参矩阵为 $\left(\begin{array}{ll}R& T\\ 0& 1\end{array}\right)$。但是，由于张正友标定板将世界坐标系的原点选取在棋盘格上，则棋盘格上任一点的物理坐标 $W=0$，将旋转矩阵的 $R$ 的第三列 $R3$ 消掉，因此， $R3$ 在坐标转化中并没有作用。但是 $R3$ 要使得 $R$ 满足旋转矩阵的性质，即列与列之间单位正交，因此可以通过向量 $R1,R2$ 的叉乘，即 $R3=R1\times R2$ ，计算得到 $R3$ 。

此时，相机的内参矩阵和外参矩阵均已得到。

注：以上推导都是假设不存在畸变参数的情况下成立的。但是事实上，相机是存在畸变参数的，因此，张正友标定法还需要通过L-M算法对于参数进行迭代优化。

4、标定相机的畸变参数

张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。

径向畸变公式（2阶）如下：

$\hat{x}=x\left(1+k_1{r}^2+k_2{r}^4\right)$
$\hat{y}=y\left(1+k_1{r}^2+k_2{r}^4\right)$

其中，$(x,y)(\hat{x},\hat{y})$分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标， $r$ 为图像像素点到图像中心点的距离，即 $r^2=x^2+y^2$ 。

图像坐标和像素坐标的转化关系为：
$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}\frac{1}{dX}& -\frac{\cot \theta }{dX}& u_0\\ 0& \frac{1}{dY\sin \theta }& v_0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{l}x\\ y\\ 1\end{array}\right)$

其中，$(u,v)$为理想的无畸变的像素坐标。由于 $\theta$ 接近于 $90°$ ，则上式近似为：
$u=\frac{x}{dX}+u_0$
$v=\frac{y}{dY}+v_0$

同理可得畸变后的像素坐标$(\hat{u},\hat{v})$的表达式为：

$\hat{u}=\frac{\hat{x}}{dX}+u_0$
$\hat{v}=\frac{\hat{y}}{dY}+v_0$

代入径向畸变公式（2阶）则有：

$\hat{u}-u_0=\left(u-u_0\right)\left(1+k_1{r}^2+k_2{r}^4\right)$
$\hat{v}-v_0=\left(v-v_0\right)\left(1+k_1{r}^2+k_2{r}^4\right)$

可化简得：
$\hat{u}=u+\left(u-u_0\right)\left(k_1{r}^2+k_2{r}^4\right)$
$\hat{v}=v+\left(v-v_0\right)\left(k_1{r}^2+k_2{r}^4\right)$

即为：
$\left[\begin{array}{cc}\left(u-u_0\right)r^2& \left(u-u_0\right)r^4\\ \left(v-v_0\right)r^2& \left(v-v_0\right)r^4\end{array}\right]\left[\begin{array}{l}{k}_1\\ k_2\end{array}\right]=\left[\begin{array}{c}\hat{u}-u\\ \hat{v}-v\end{array}\right]$

每一个角点，只要知道畸变后的像素坐标$(\hat{u},\hat{v})$、理想的无畸变的像素坐标 $(u,v)$ ，就可以构造两个上述等式。那么，有 $m$ 幅图像，每幅图像上有 $n$ 个标定板角点，则将得到的所有等式组合起来，可以得到 $mn$ 个未知数为 $k={\left[k_1,k_2\right]}^T$ 的约束方程，将约束方程系数矩阵记为 $D$ ，等式右端非齐次项记为 $d$，可将其记成矩阵形式：

$Dk=d$

之后，利用最小二乘法可得：

$k=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]={\left(D^{T}D\right)}^{-1}{D}^{T}d$

此时，相机的畸变矫正参数已经标定好。

那么，如何获得畸变后的像素坐标 $(\hat{u},\hat{v})$ 和理想的无畸变的像素坐标 $(u,v)$ 呢？

$(\hat{u},\hat{v})$ 可以通过识别标定板的角点获得，$(u,v)$可以通过如下方法近似求得。世界坐标系下每一个角点的坐标$(X,Y)$是可以计算得到的，我们利用已经求得的外参矩阵 $(R1,R2,T)$ 和内参矩阵 $A$ 进行反投影。

$Z\left(\begin{array}{l}u\\ v\\ 1\end{array}\right)=A\left(\begin{array}{lll}R1& R2& T\end{array}\right)\left(\begin{array}{l}X\\ Y\\ 1\end{array}\right)=H\left(\begin{array}{c}X\\ Y\\ 1\end{array}\right)$

利用上式，消去尺度因子$Z$，可得：

$\begin{array}{rl}u& =\frac{H_{11}U+H_{12}V+H_{13}}{H_{31}U+H_{32}V+H_{33}}\\ v& =\frac{H_{21}U+H_{22}V+H_{23}}{H_{31}U+H_{32}V+H_{33}}\end{array}$

即可得到理想的、无畸变的像素坐标 $(u,v)$ 。当然，由于外参矩阵$(R1,R2,T)$ 和内参矩阵$A$是在有畸变的情况下获得的，这里得到的像素坐标$(u,v)$并不是完全理想的、无畸变的。

我们的总逻辑是，
1 在进行内参矩阵和外参矩阵的求解的时候，我们假设不存在畸变；
2 在进行畸变系数的求解的时候，我们假设求得的内参矩阵和外参矩阵是无误差的。
3 我们再通过L-M算法对于参数进行迭代优化。

https://zhuanlan.zhihu.com/p/94244568