张正友标定法过程推导笔记

张正友标定法

1.相机标定的含义

相机标定是世界坐标系与图像坐标系关系建立过程中，确定相机的内部固定参数的过程。比如工业相机在设计时有一个具体的焦距，光心等参数，但是实际做出来可能会和设计时有所偏差，相机标定就是确定相机在实际拍摄使用时的参数。（可以类比电器的额定功率和实际功率）

2.相机标定的输出参数（内参）

相机标定的参数称为内参。主要有光心位置($\delta x$,$\delta y$)，焦距($f_x$,$f_y$)。这些参数构成了一个内参矩阵。此外如果考虑畸变的影响，根据不同的畸变模型，输出畸变的参数。

3.外参

外参是拍摄的图像与相机坐标系之间的关系。每张照片不同位置不同角度，外参都不一样。该参数并没有很重要。

4.世界坐标系和图像坐标系转换

这个推导过程因为不是很难也不是重点，此处省略，可以看看这篇文章。https://blog.csdn.net/byyastal/article/details/108474083
它们的转换关系为：

解释一个里面的重要参数含义：
$f_x$：x方向的焦距，注意此处$f=f_x\ast dx$，$f$才是真正光学意义上的焦距（mm），$dx$是像元单位，一般是μm级别
$f_y$：y方向的焦距，注意此处$f=f_x\ast dx$
$u_0$：图像坐标系x轴起始坐标，一般都是0
$v_0$：图像坐标系y轴起始坐标，一般都是0
$R$：旋转矩阵，具体参考旋转矩阵的特点
$T$：平移向量
$X_w,Y_w,Z_w$：世界坐标系坐标，是一个三维坐标系
$u,v$：是图像上的坐标，是一个二维坐标
$Z_c$：是一个缩放因子，同一幅图像中不同点的缩放因子不同，后续公式中的$\lambda$也指的是这个值。

5.求解单应矩阵

要求解内参，第一步是要求解单应矩阵，求解了单应矩阵，后面再进一步通过某些方法（如张正友标定法）求解内参矩阵。一般定标的方法第一步都是如此。
单应矩阵为内参矩阵和外参矩阵的乘积。
即：

其中，$M$是内参矩阵，$s$为缩放因子的倒数，$r_1$、$r_2$是旋转矩阵的两个列向量，$t$为平移向量。

单应矩阵的求解过程：
设单应矩阵为H：
$$H=\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}^{\prime }\\ y^{\prime }\\ 1\end{array}\right]\sim \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}{l}x\\ y\\ 1\end{array}\right]$$
矩阵展开后有3个等式，将第3个等式代入前两个等式中可得：
$$\begin{array}{rl}{x}^{\prime }& =\frac{h_{11}x+h_{12}y+h_{13}}{h_{31}x+h_{32}y+h_{33}}\\ y^{\prime }& =\frac{h_{21}x+h_{22}y+h_{23}}{h_{31}x+h_{32}y+h_{33}}\end{array}$$

也就是说，一个点对对应两个等式。H矩阵我们令$h_{3}3$为1，则一共有8个未知数，所以需要至少四个点求解。

一般都用大于4个的点求解，然后利用LM等优化方法得到最合适的单应矩阵H。具体的话，可以使用matlab和OpenCV中的函数来求解单应矩阵。分别是estimateGeometricTransform和findHomography函数。

5.张正友标定算法

上述步骤中求得了单应矩阵H，则：
$H=s\left[\begin{array}{ccc}{f}_x& \gamma & u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{lll}{r}_1& r_2& t\end{array}\right]=sM\left[\begin{array}{lll}{r}_1& r_2& t\end{array}\right]$

我们知道H是内参矩阵和外参矩阵的混合体，而我们想要最终分别获得内参和外参。所以需要想个办法，先把内参求出来（先求内参是因为更容易，因为每张图片的内参都是固定的，而外参是变化的），得到内参后，那张标定图片的外参也就随之解出了。

为了利用旋转向量之间的约束关系，我们先将单应性矩阵H化为3个列向量，即H=[$h_1$ $h_2$ $h_3$]，则有
$$H=\left[\begin{array}{lll}{\mathbf{h}}_1& {\mathbf{h}}_2& {\mathbf{h}}_3\end{array}\right]=sM\left[\begin{array}{lll}{r}_1& r_2& t\end{array}\right]$$按元素对应关系可得：

因为旋转向量在构造中是相互正交的，即r1和r2相互正交，由此我们就可以利用“正交”的两个含义，得出每个单应矩阵提供的两个约束条件：
约束条件1： 旋转向量点积为0（两垂直平面上的旋转向量互相垂直），这个可以参考上面对旋转矩阵的性质介绍 – 旋转矩阵的每一列都是彼此正交，且模为1，即：

约束条件2： 旋转向量长度相等（旋转不改变尺度），（我认为还是上面的那个性质–旋转矩阵的每一列都是彼此正交，且模为1）即：

记${\left(M^{-1}\right)}^T{M}^{-1}$为B展开：

$$B={\left(M^{-1}\right)}^T{M}^{-1}=\left[\begin{array}{ccc}\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}& -\gamma \frac{v_0\gamma -u_0{f}_y}{f_x^2{f}_{yy}^2}-\frac{v_0}{f_y^2}\\ \frac{v_0\gamma -u_0{f}_y}{f_x^2{f}_y}& -\gamma \frac{v_0\gamma -u_0{f}_y}{f_x^2{f}_y^2}-\frac{v_0}{f_y^2}& \frac{{\left(v_0\gamma -u_0{f}_y\right)}^2}{f_x^2{f}_y^2}+\frac{v_0^2}{f_y^2}+1\end{array}\right]=\left[\begin{array}{lll}{B}_{11}& B_{12}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{31}& B_{32}& B_{33}\end{array}\right]$$
这里其实B中包含了一个尺度因子$\lambda$，因为求模运算两边相等时，有一个$\lambda$被消去。B为对称矩阵，真正有用的元素只有6个（主对角线任意一侧的6个元素）。把B带入前面两个约束条件后可转化为：
$$\{\begin{array}{l}{h}_1^{T}B{h}_2=0\\ h_1^{T}B{h}_1=h_2^{T}B{h}_2\end{array}$$
上面两约束中的式子均可写为通式：$h_i^{T}B{h}_j$，定义3X3的单应矩阵H=[h1 h2 h3]的第i列列向量：
$$h_i=\left[h_{i1},h_{i2},h_{i3}\right]$$将如下表达式代入上述的约束单项式：

$$h_i^{T}B{h}_j={\left[\begin{array}{c}{h}_{\mathrm{i}1}{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{array}\right]}^T\left[\begin{array}{c}{B}_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33}\end{array}\right]$$为了简化表达形式，令：
$$v_{ij}=\left[\begin{array}{c}{h}_{\mathrm{i}}{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{array}\right],b=\left[\begin{array}{c}{B}_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33}\end{array}\right]$$则有：
$$h_i^{T}B{h}_j={\nu }_{ij}^{T}b$$由此，两约束条件最终可以转化为如下形式：
$$\begin{array}{l}{v}_{12}{}^{T}b=0\\ \left(v_{11}{}^T-v_{22}{}^T\right)b=0\end{array}\to \left[\begin{array}{c}{v}_{12}{}^T\\ v_{11}{}^T-v_{22}{}^T\end{array}\right]b=0$$如果我们拍摄了n张不同角度的标定图片，因为每张图片我们都可以得到一组（2个）上述的等式。其中，$v_{12}$,$v_{11}$,$v_{22}$可以通过前面已经计算好的单应矩阵得到，因此是已知的，而b中6个元素是待求的未知数。因此，至少需要保证图片数 n>=3，我们才能解出b。

当b向量求解出来以后：
$$B=\lambda M^{-T}M$$根据B展开的内容：
$$\{\begin{array}{rl}{f}_x& =\sqrt{\lambda /B_{11}}\\ f_y& =\sqrt{\lambda B_{11}/\left(B_{11}{B}_{22}-B_{12}^2\right)}\\ u0& =\gamma v0/f_y-B_{13}{f}_x^2/\lambda \\ v0& =\left(B_{12}{B}_{13}-B_{11}{B}_{23}\right)/\left(B_{11}{B}_{22}-B_{12}^2\right)\\ \gamma & =-B_{12}{f}_x^2{f}_y/\lambda \\ \lambda & =B_{33}-\left[B_{13}^2+v0\left(B_{12}{B}_{13}-B_{11}{B}_{23}\right)\right]/B_{11}\end{array}$$内参M已知后，也可以计算得外参：
$$\begin{array}{ll}{r}_1=\lambda M^{-1}{h}_1& r_2=\lambda M^{-1}{h}_2\\ r_3=r_1\times r_2& t=\lambda M^{-1}{h}_3\end{array}$$

最大似然估计
上述的推导结果是基于理想情况下的解，从理论上证明了张氏标定算法的可行性。但在实际标定过程中，一般使用最大似然估计进行优化。假设我们拍摄了n张标定图片，每张图片里有m个棋盘格角点。三维空间点$P$在图片上对应的二维像素为$p$，三维空间点经过相机内参$M$，外参$R$，$t$变换后得到的二维像素为$p^{\prime }$（假设噪声是独立同分布的，我们通过最小化$p$, $p’$的位置来求解上述最大似然估计问题：
$$\sum _{i=1}^n\sum _{j=1}^m{\Vert p_{ij}-p^{\prime }\left(M,R_i,t_i,P_{ij}\right)\Vert }^2$$

考虑畸变的影响：
畸变模型为：
$$\{\begin{array}{l}{x}_{\text{brown}}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\\ y_{\text{brown}}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\end{array}$$此时最大似然估计目标函数变为：
$$\sum _{i=1}^n\sum _{j=1}^m{\Vert p_{ij}-p^{\prime }\left(M,k_1,k_2,R_i,t_i,P_{ij}\right)\Vert }^2$$