【张正友标定法解析与C++代码实现】

介绍

张正友标定法是一种求解相机内外参的标定方法。相比使用三维标定板的标定方法，张正友标定法使用二维平面标定板，优点是标定板制作成本低，应用更为灵活，但缺点是需要拍摄多张不同姿态的标定板图像（PS：也可以在一张图片中同时拍摄多个不同姿态的标定板），效率上总体不如三维标定板标定方法。

本文介绍张正友标定法的数学原理，并给出代码实现。数学原理方面，介绍了相机模型，对单应矩阵、内参矩阵、外参矩阵的求解做了详细解释，但内容上为了更为简洁实用，省略skewness参数以及畸变参数的估计，原因如下：

数学原理

相机模型

相机模型表征了三维世界坐标系或物体坐标系中的点，如何经过镜头和图像传感器成像，映射为二维像素坐标的过程。相机模型的关键参数是内参和外参，内参包括主点、焦距、畸变参数，外参包括每幅图像中标定板在相机坐标系的旋转和平移参数。计算机视觉中常用的相机模型是“针孔+畸变”相机模型。，本节仅简要介绍其中的重点内容。

1. 世界/物体坐标系中的点$P=[X,Y,Z{]}^T$经过旋转平移变换后成为相机坐标系中的点$P^{\prime }=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$，表示为：$$\begin{gathered} P^{\prime }=RP+t \\ \left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right] \end{gathered}$$其中，$R$为旋转矩阵，$t$为平移矩阵。
2. 点$P^{\prime }=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$经过沿相机坐标系$Z$的透视投影后，成为归一化坐标系中的二维点$[x,y{]}^T$，表示为：$$\begin{gathered} x=\frac{X^{\prime }}{Z^{\prime }} \\ y=\frac{Y^{\prime }}{Z^{\prime }} \end{gathered}$$
3. 由于畸变的存在，点$[x,y{]}^T$偏移至点$[x_d,y_d{]}^T$，这一过程由畸变模型描述。这里使用Brown-Conrady模型描述，表示为：$$\begin{gathered} r^2=x^2+y^2 \\ {x}_d=x(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_d=y(1+k_1{r}^2+k_2{r}^4+k_3{r}^6)+p_1(r^2+2y^2)+2p_{2}xy \end{gathered}$$其中，$k_1,k_2,k_3$为描述径向畸变的畸变系数，$p_1,p_2$为描述切向畸变的畸变系数。
4. $[x_d,y_d{]}^T$经过像素采样，成为像素坐标系中的点$U=[u,v{]}^T$，表示为：$$\begin{gathered} u=f_x{x}_d+c_x \\ v=f_y{y}_d+c_y \\ \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=K\left[\begin{array}{c}{x}_d\\ y_d\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\\ 1\end{array}\right] \end{gathered}$$其中，$K$为内参矩阵，$f_x,f_y$为焦距（物理意义为镜头焦距与像素大小的比值），$c_x,c_y$为主点（光轴与图像传感器交点的像素坐标）。

相机标定

相机标定的目的是找到最小化如下目标函数的相机参数$$\underset{K,D,R_i,t_i}{\min }\sum _i\sum _j(U_{i,j}-\text{cam}(R_i{P}_{i,j}+t_i){)}^2$$其中，$i$表示标定板姿态的索引，$j$表示标定板中特征点的索引，$\text{cam}()$表示相机对相机坐标系中的3D点的成像过程，包含了透视投影、畸变模型、像素采样。

相机标定过程中的已知量为：

1. 特征点在物体坐标系的坐标$P=[X,Y,Z{]}^T$
2. 通过特征点检测方法获得的标定板特征点在像素坐标系中的坐标$U=[u,v{]}^T$

需要求解的未知量为：

1. 相机内参矩阵$K$：包含主点$c_x,c_y$、焦距$f_x,f_y$。
2. 畸变参数$D$：包含$k_1,k_2,k_3,p_1,p_2$。
3. 外参，标定板在每幅图像中的位姿$R_i,t_i$。

目标函数需要通过迭代进行求解，常用的迭代方法是L-M法，但由于畸变函数比较复杂，雅可比矩阵解析形式太复杂，不好实现，但好在有很多非线性优化库提供了自动求导和数值求导方法，本文的程序调用Ceres实现目标函数构造与求解。
同时，迭代需要提供好的初值参数才能正确收敛。通常采用的方法是在忽略畸变的情况下使用解析的方法求解内参（主点$c_x,c_y$、焦距$f_x,f_y$）和外参（$R_i,t_i$），以此为初值进行迭代优化。根据标定板特征点的维度（在三维空间中、在二维平面上、在一维线上），有几类不同的解析求解方法【1】。本文介绍的是张正友提出的基于二维平面标定板的解析求解方法【2，3】。

单应、内参、外参初值估计

单应矩阵计算

无畸变情况下，相机模型表示为：$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\backsimeq \left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$$其中，$\backsimeq$表示经过透视投影之后的相等，即左侧向量等于右侧向量除以其最后一个元素。

假定标定平面位于物体坐标系中$Z=0$的平面上，对上述公式进行简化，得$$\begin{gathered} \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\backsimeq \left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_1\\ r_{21}& r_{22}& r_{23}& t_2\\ r_{31}& r_{32}& r_{33}& t_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right] \\ \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]\backsimeq \left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& t_1\\ r_{21}& r_{22}& t_2\\ r_{31}& r_{32}& t_3\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right] \end{gathered}$$定义矩阵$h$，表示为$$h=\left[\begin{array}{ccc}{h}_1& h_2& h_3\end{array}\right]=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& t_1\\ r_{21}& r_{22}& t_2\\ r_{31}& r_{32}& t_3\end{array}\right]=K\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]$$这个矩阵表示了物体平面上的点，经过透视变换，投影到图像传感器平面的坐标，称为单应矩阵。有以下等式成立
$$\begin{gathered} u=\frac{h_{11}X+h_{12}Y+h_{13}}{h_{31}X+h_{32}Y+h_{33}} \\ v=\frac{h_{21}X+h_{22}Y+h_{23}}{h_{31}X+h_{32}Y+h_{33}} \\ 1=\frac{h_{31}X+h_{32}Y+h_{33}}{h_{31}X+h_{32}Y+h_{33}} \end{gathered}$$
对前两个等式进行变换，可得$$\begin{gathered} h_{11}X+h_{12}Y+h_{13}-u(h_{31}X+h_{32}Y+h_{33})=0 \\ {h}_{21}X+h_{22}Y+h_{23}-v(h_{31}X+h_{32}Y+h_{33})=0 \end{gathered}$$写成矩阵形式为$$\left[\begin{array}{ccccccccc}X& Y& 1& 0& 0& 0& -uX& -uY& -u\\ 0& 0& 0& X& Y& 1& -vX& -vY& -v\end{array}\right]\left[\begin{array}{c}{h}_{11}\\ h_{12}\\ h_{13}\\ h_{21}\\ h_{22}\\ h_{23}\\ h_{31}\\ h_{32}\\ h_{33}\end{array}\right]=0\text{\hspace{1em}(1)}$$
虽然$h$有九个未知数，但矩阵元素乘以任意非零常数后，等式仍然成立，因此$h$的自由度实际只有8。只要找到任意三点不共线的四个点，提供八组约束方程就能求解，实际标定板的特征点数更多，多出来的约束可以用最小二乘法求解。这里求解的是$Ax=0$问题的最小二乘解，目标函数为$\min ||Ax|{|}_2^2$，求解时需固定$x$的尺度，满足$||x|{|}_2^2=1$，否则$x$模长越接近0，目标函数越小，得不到有效解。下面给出了有关$Ax=0$求解的总结。

在OpenCV里，可以使用函数SVD::solveZ得到$Ax=0$满足$||x|{|}_2^2=1$的解。注意这里求解得到的单应矩阵与内参矩阵和旋转平移矩阵的乘积差一个未知的尺度参数$\lambda$，表示为
$$\lambda h=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{r}_{11}& r_{12}& t_1\\ r_{21}& r_{22}& t_2\\ r_{31}& r_{32}& t_3\end{array}\right]$$由于旋转平移矩阵的特殊约束，这个未知尺度$\lambda$后续可以被求出来。

归一化处理

在求解单应矩阵时，对$[X,Y,1{]}^T$和$[u,v,1{]}^T$进行归一化处理可提高单应矩阵$h$求解的稳定性。对$[X,Y,1{]}^T$的归一化处理表示如下
$$\left[\begin{array}{c}\bar{X}\\ \bar{Y}\\ 1\end{array}\right]=N_P\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{X_{\text{std}}}& 0& -\frac{X_{\text{mean}}}{X_{\text{std}}}\\ 0& \frac{1}{Y_{\text{std}}}& -\frac{Y_{\text{mean}}}{Y_{\text{std}}}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]\text{\hspace{1em}(2)}$$得到归一化矩阵$N_P$和归一化的坐标$[\bar{X},\bar{Y},1{]}^T$，其中下标$\text{mean},\text{std}$表示均值和标准差。同理，得到$[u,v,1{]}^T$的归一化矩阵$N_U$和归一化的坐标$[\bar{u},\bar{v},1{]}^T$。
带入归一化的坐标，求解单应矩阵$h^{\prime }$
$$\left[\begin{array}{ccccccccc}\bar{X}& \bar{Y}& 1& 0& 0& 0& -\bar{u}\bar{X}& -\bar{u}\bar{Y}& -\bar{u}\\ 0& 0& 0& \bar{X}& \bar{Y}& 1& -\bar{v}\bar{X}& -\bar{v}\bar{Y}& -\bar{v}\end{array}\right]\left[\begin{array}{c}{h}_{11}^{\prime }\\ h_{12}^{\prime }\\ h_{13}^{\prime }\\ h_{21}^{\prime }\\ h_{22}^{\prime }\\ h_{23}^{\prime }\\ h_{31}^{\prime }\\ h_{32}^{\prime }\\ h_{33}^{\prime }\end{array}\right]=0$$最后，对单应矩阵进行补偿
$$h=\text{inv}(N_U)\cdot h^{\prime }\cdot N_P\text{\hspace{1em}(3)}$$

内参估计

求解完单应矩阵后，可以利用旋转矩阵的单位正交性从单应矩阵中恢复内参。$$\begin{gathered} r_1=\lambda K^{-1}{h}_1 \\ {r}_2=\lambda K^{-1}{h}_2 \\ {r}_1^T{r}_2=0 \\ {r}_1^T{r}_1-r_2^T{r}_2=0 \\ \Downarrow \\ {h}_1^T{K}^{-T}{K}^{-1}{h}_2=0 \\ {h}_1^T{K}^{-T}{K}^{-1}{h}_1-h_2^T{K}^{-T}{K}^{-1}{h}_2=0 \end{gathered}$$由于是两个等于0的等式，求解并不涉及未知尺度$\lambda$。令$B=K^{-T}{K}^{-1}$，则有$$B=K^{-T}{K}^{-1}\iff \left[\begin{array}{ccc}{B}_{11}& 0& B_{13}\\ 0& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{f_x^2}& 0& -\frac{c_x}{f_x^2}\\ 0& \frac{1}{f_y^2}& -\frac{c_y}{f_y^2}\\ -\frac{c_x}{f_x^2}& -\frac{c_y}{f_y^2}& 1+\frac{c_x^2}{f_x^2}+\frac{c_y^2}{f_y^2}\end{array}\right]$$结合上述两个公式，整理为$Ax=0$的形式，表示为：$$\left[\begin{array}{ccccc}{h}_{11}{h}_{12}& h_{21}{h}_{22}& h_{31}{h}_{12}+h_{11}{h}_{32}& h_{31}{h}_{22}+h_{21}{h}_{32}& h_{31}{h}_{32}\\ h_{11}^2-h_{12}^2& h_{21}^2-h_{22}^2& 2h_{11}{h}_{31}-2h_{12}{h}_{32}& 2h_{21}{h}_{31}-2h_{22}{h}_{32}& h_{31}^2-h_{32}^2\end{array}\right]\left[\begin{array}{c}{B}_{11}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33}\end{array}\right]=0\text{\hspace{1em}(4)}$$同样是求解$Ax=0$问题，令解为${\bar{B}}_{ij}$，这个解与内参之间存在未知尺度$\eta$，表示为
$$\left[\begin{array}{ccc}{\bar{B}}_{11}& 0& {\bar{B}}_{13}\\ 0& {\bar{B}}_{22}& {\bar{B}}_{23}\\ {\bar{B}}_{13}& {\bar{B}}_{23}& {\bar{B}}_{33}\end{array}\right]=\eta \left[\begin{array}{ccc}\frac{1}{f_x^2}& 0& -\frac{c_x}{f_x^2}\\ 0& \frac{1}{f_y^2}& -\frac{c_y}{f_y^2}\\ -\frac{c_x}{f_x^2}& -\frac{c_y}{f_y^2}& 1+\frac{c_x^2}{f_x^2}+\frac{c_y^2}{f_y^2}\end{array}\right]$$但由于内参矩阵的最后一个元素为1，可以将内参与尺度参数$\eta$一同求出。
$$\{\begin{array}{l}\frac{\eta }{f_x^2}={\bar{B}}_{11}\\ \frac{\eta }{f_y^2}={\bar{B}}_{22}\\ -\eta \frac{c_x}{f_x^2}={\bar{B}}_{13}\\ -\eta \frac{c_y}{f_y^2}={\bar{B}}_{23}\\ \eta (1+\frac{c_x^2}{f_x^2}+\frac{c_y^2}{f_y^2})={\bar{B}}_{33}\end{array}\iff \{\begin{array}{l}{c}_x=-\frac{{\bar{B}}_{13}}{{\bar{B}}_{11}}\\ c_y=-\frac{{\bar{B}}_{23}}{{\bar{B}}_{22}}\\ \eta ={\bar{B}}_{33}-\frac{{\bar{B}}_{13}^2}{{\bar{B}}_{11}}-\frac{{\bar{B}}_{23}^2}{{\bar{B}}_{22}}\\ f_x=\sqrt{\frac{\eta }{{\bar{B}}_{11}}}\\ f_y=\sqrt{\frac{\eta }{{\bar{B}}_{22}}}\end{array}\text{\hspace{1em}(5)}$$OpenCV相机标定程序使用了另一种内参估计方法，在函数cvInitIntrinsicParams2D()中，假定主点位于图像中间，即$c_x,c_y$已知，仅求解$f_x,f_y$。详细可参考博客：

外参估计

得到内参之后，因$\lambda \left[\begin{array}{ccc}{h}_1& h_2& h_3\end{array}\right]=K\left[\begin{array}{ccc}{r}_1& r_2& t\end{array}\right]$，易得外参
$$\begin{gathered} r_1=\lambda K^{-1}{h}_1 \\ {r}_2=\lambda K^{-1}{h}_2 \\ t=\lambda K^{-1}{h}_3 \\ {r}_3=r_1\times r_2\text{\hspace{1em}(6)} \end{gathered}$$其中，利用$r_1^T{r}_1=r_2^T{r}_2=1$的性质，可得$\lambda =1/||K^{-1}{h}_1|{|}_2=1/||K^{-1}{h}_2|{|}_2$。

这里还需要注意，单应矩阵的正负号会导致外参有两种相反的结果。但是，由于所有特征点都需要在相机前方才可以被观测到，故可以用其中一个特征点的经过外参变换后$Z$值的正负号做检验。一个简单的方法是，假定标定板原点的齐次坐标$[0,0,0,1{]}^T$在相机前，则有$t_3>0$，经过内参矩阵变换后，单应矩阵应满足$h_{33}>0$，因此求解单应矩阵后，可将结果直接除以$h_{33}$做归一化，以消除另一组错误的外参。

另外，由于噪声的影响，求得的旋转矩阵$R=\left[\begin{array}{ccc}{r}_1& r_2& r_3\end{array}\right]$不一定满足$R{R}^T=I,\det (R)=1$，可对矩阵做如下SVD分解处理以确保$R$满足旋转矩阵约束
$$\begin{gathered} [U,\Sigma ,V]=\text{SVD}(R) \\ R=U{V}^T\text{\hspace{1em}(7)} \end{gathered}$$证明如下：

找到矩阵$M$最接近的旋转矩阵$R$，定义如下优化问题$$\begin{gathered} \min ||M-R|{|}_F^2=\text{trace}((M-R{)}^T(M-R)) \\ \text{subject to }{R}^{T}R=I,\det (R)=1 \end{gathered}$$使用拉格朗日乘子法，定义$$L(R,\Lambda )=\text{trace}((M-R{)}^T(M-R))+\text{trace}(\Lambda (R^{T}R-I))$$求偏导可得
$$\frac{\partial L}{\partial R}=-2(M-R)+R(\Lambda +{\Lambda }^T)=0$$因为$\Lambda ={\Lambda }^T$，可得$$M=R(I+\Lambda )$$注意到$$M^{T}M=(I+\Lambda {)}^T{R}^{T}R(I+\Lambda )=(I+\Lambda {)}^2$$故有$$I+\Lambda =(M^{T}M{)}^{1/2}$$综上可得$$R=M(M^{T}M{)}^{-1/2}$$
定义矩阵$M$的SVD分解，表示为$M=U\Sigma V^T$，则有$$\begin{gathered} R=U\Sigma V^T(V{\Sigma }^T{U}^{T}U\Sigma V^T{)}^{-1/2} \\ =U\Sigma V^T(V{\Sigma }^2{V}^T{)}^{-1/2} \\ =U\Sigma V^T(V\Sigma V^{T}V\Sigma V^T{)}^{-1/2} \\ =U\Sigma V^T(V\Sigma V^T{)}^{-1} \\ =U\Sigma V^T{V}^{-T}{\Sigma }^{-1}{V}^{-1} \\ =U{V}^T \end{gathered}$$

程序实现

参考文献

【1】
【2】Zhang Z. A flexible new technique for camera calibration[J]. IEEE Transactions on pattern analysis and machine intelligence, 2000, 22(11): 1330-1334.
【3】Burger W. Zhang’s camera calibration algorithm: in-depth tutorial and implementation[J]. HGB16-05, 2016: 1-6.