张正友标定法

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 = [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] H=\left[

\right] H=⎣⎡​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​⎦⎤​
则有：
[ x ′ y ′ 1 ] ∼ [ h 11 h 12 h 13 h 21 h 22 h 23 h 31 h 32 h 33 ] [ x y 1 ] \left[

x′y′1$$\begin{array}{c}{x}^{\mathrm{\prime }}\\ y^{\mathrm{\prime }}\\ 1\end{array}$$

\right] \sim\left[

h11h21h31h12h22h32h13h23h33$$\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[

xy1$$\begin{array}{l}x\\ y\\ 1\end{array}$$

\right] ⎣⎡​x′y′1​⎦⎤​∼⎣⎡​h11​h21​h31​​h12​h22​h32​​h13​h23​h33​​⎦⎤​⎣⎡​xy1​⎦⎤​
矩阵展开后有3个等式，将第3个等式代入前两个等式中可得：
x ′ = h 11 x + h 12 y + h 13 h 31 x + h 32 y + h 33 y ′ = h 21 x + h 22 y + h 23 h 31 x + h 32 y + h 33

x′y′=h11x+h12y+h13h31x+h32y+h33=h21x+h22y+h23h31x+h32y+h33$$\begin{array}{rl}{x}^{\mathrm{\prime }}& =\frac{h_{11}x+h_{12}y+h_{13}}{h_{31}x+h_{32}y+h_{33}}\\ y^{\mathrm{\prime }}& =\frac{h_{21}x+h_{22}y+h_{23}}{h_{31}x+h_{32}y+h_{33}}\end{array}$$

x′y′​=h31​x+h32​y+h33​h11​x+h12​y+h13​​=h31​x+h32​y+h33​h21​x+h22​y+h23​​​

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

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

5.张正友标定算法

上述步骤中求得了单应矩阵H，则：
H = s [ f x γ u 0 0 f y v 0 0 0 1 ] [ r 1 r 2 t ] = s M [ r 1 r 2 t ] H=s\left[

\right]\left[\right]=s M\left[\right] H=s⎣⎡​fx​00​γfy​0​u0​v0​1​⎦⎤​[r1​​r2​​t​]=sM[r1​​r2​​t​]

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

为了利用旋转向量之间的约束关系，我们先将单应性矩阵H化为3个列向量，即H=[$h_1$ $h_2$ $h_3$]，则有
H = [ h 1 h 2 h 3 ] = s M [ r 1 r 2 t ] H=\left[

\right]=s M\left[\right] H=[h1​​h2​​h3​​]=sM[r1​​r2​​t​]按元素对应关系可得：

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

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

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

B = ( M − 1 ) T M − 1 = [ 1 f x 2 − γ f x 2 f y v 0 γ − u 0 f y f x 2 f y − γ f x 2 f y γ 2 f x 2 f y 2 + 1 f y 2 − γ v 0 γ − u 0 f y f x 2 f y y 2 − v 0 f y 2 v 0 γ − u 0 f y f x 2 f y − γ v 0 γ − u 0 f y f x 2 f y 2 − v 0 f y 2 ( v 0 γ − u 0 f y ) 2 f x 2 f y 2 + v 0 2 f y 2 + 1 ] = [ B 11 B 12 B 13 B 21 B 22 B 23 B 31 B 32 B 33 ] B=\left(M^{-1}\right)^{T} M^{-1}=\left[

\right]=\left[\right] B=(M−1)TM−1=⎣⎢⎢⎡​fx2​1​−fx2​fy​γ​fx2​fy​v0​γ−u0​fy​​​−fx2​fy​γ​fx2​fy2​γ2​+fy2​1​−γfx2​fy2​v0​γ−u0​fy​​−fy2​v0​​​fx2​fy​v0​γ−u0​fy​​−γfx2​fyy2​v0​γ−u0​fy​​−fy2​v0​​fx2​fy2​(v0​γ−u0​fy​)2​+fy2​v02​​+1​⎦⎥⎥⎤​=⎣⎡​B11​B21​B31​​B12​B22​B32​​B13​B23​B33​​⎦⎤​
这里其实B中包含了一个尺度因子$\lambda$，因为求模运算两边相等时，有一个$\lambda$被消去。B为对称矩阵，真正有用的元素只有6个（主对角线任意一侧的6个元素）。把B带入前面两个约束条件后可转化为：
{ h 1 T B h 2 = 0 h 1 T B h 1 = h 2 T B h 2 \left\{

hT1Bh2=0hT1Bh1=hT2Bh2$$\begin{array}{l}{h}_1^{T}B{h}_2=0\\ h_1^{T}B{h}_1=h_2^{T}B{h}_2\end{array}$$

\right. {h1T​Bh2​=0h1T​Bh1​=h2T​Bh2​​
上面两约束中的式子均可写为通式：$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 = [ h i 1 h j 1 h i 1 h j 2 + h i 2 h j 1 h i 2 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3 ] T [ B 11 B 12 B 22 B 13 B 23 B 33 ] h_{i}^{T} B h_{j}=\left[

\right]^{T}\left[\right] hiT​Bhj​=⎣⎢⎢⎢⎢⎢⎢⎡​hi1​hj1​hi1​hj2​+hi2​hj1​hi2​hj2​hi3​hj1​+hi1​hj3​hi3​hj2​+hi2​hj3​hi3​hj3​​⎦⎥⎥⎥⎥⎥⎥⎤​T⎣⎢⎢⎢⎢⎢⎢⎡​B11​B12​B22​B13​B23​B33​​⎦⎥⎥⎥⎥⎥⎥⎤​为了简化表达形式，令：
v i j = [ h i h j 1 h i 1 h j 2 + h i 2 h j 1 h i 2 h j 2 h i 3 h j 1 + h i 1 h j 3 h i 3 h j 2 + h i 2 h j 3 h i 3 h j 3 ] , b = [ B 11 B 12 B 22 B 13 B 23 B 33 ] v_{i j}=\left[

hihj1hi1hj2+hi2hj1hi2hj2hi3hj1+hi1hj3hi3hj2+hi2hj3hi3hj3$$\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], \quad b=\left[

B11B12B22B13B23B33$$\begin{array}{c}{B}_{11}\\ B_{12}\\ B_{22}\\ B_{13}\\ B_{23}\\ B_{33}\end{array}$$

\right] vij​=⎣⎢⎢⎢⎢⎢⎢⎡​hi​hj1​hi1​hj2​+hi2​hj1​hi2​hj2​hi3​hj1​+hi1​hj3​hi3​hj2​+hi2​hj3​hi3​hj3​​⎦⎥⎥⎥⎥⎥⎥⎤​,b=⎣⎢⎢⎢⎢⎢⎢⎡​B11​B12​B22​B13​B23​B33​​⎦⎥⎥⎥⎥⎥⎥⎤​则有：
$$h_i^{T}B{h}_j={\nu }_{ij}^{T}b$$由此，两约束条件最终可以转化为如下形式：
v 12 T b = 0 ( v 11 T − v 22 T ) b = 0 → [ v 12 T v 11 T − v 22 T ] b = 0

v12Tb=0(v11T−v22T)b=0$$\begin{array}{l}{v}_{12}{}^{T}b=0\\ \left(v_{11}{}^T-v_{22}{}^T\right)b=0\end{array}$$

\rightarrow\left[

v12Tv11T−v22T$$\begin{array}{c}{v}_{12}{}^T\\ v_{11}{}^T-v_{22}{}^T\end{array}$$

\right] b=0 v12​Tb=0(v11​T−v22​T)b=0​→[v12​Tv11​T−v22​T​]b=0如果我们拍摄了n张不同角度的标定图片，因为每张图片我们都可以得到一组（2个）上述的等式。其中，$v_{12}$,$v_{11}$,$v_{22}$可以通过前面已经计算好的单应矩阵得到，因此是已知的，而b中6个元素是待求的未知数。因此，至少需要保证图片数 n>=3，我们才能解出b。

当b向量求解出来以后：
$$B=\lambda M^{-T}M$$根据B展开的内容：
{ f x = λ / B 11 f y = λ B 11 / ( B 11 B 22 − B 12 2 ) u 0 = γ v 0 / f y − B 13 f x 2 / λ v 0 = ( B 12 B 13 − B 11 B 23 ) / ( B 11 B 22 − B 12 2 ) γ = − B 12 f x 2 f y / λ λ = B 33 − [ B 13 2 + v 0 ( B 12 B 13 − B 11 B 23 ) ] / B 11 \left\{

\right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​fx​fy​u0v0γλ​=λ/B11​ ​=λB11​/(B11​B22​−B122​) ​=γv0/fy​−B13​fx2​/λ=(B12​B13​−B11​B23​)/(B11​B22​−B122​)=−B12​fx2​fy​/λ=B33​−[B132​+v0(B12​B13​−B11​B23​)]/B11​​内参M已知后，也可以计算得外参：
r 1 = λ M − 1 h 1 r 2 = λ M − 1 h 2 r 3 = r 1 × r 2 t = λ M − 1 h 3

r1=λM−1h1r3=r1×r2r2=λM−1h2t=λM−1h3$$\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}$$

r1​=λM−1h1​r3​=r1​×r2​​r2​=λM−1h2​t=λM−1h3​​

最大似然估计
上述的推导结果是基于理想情况下的解，从理论上证明了张氏标定算法的可行性。但在实际标定过程中，一般使用最大似然估计进行优化。假设我们拍摄了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$$

考虑畸变的影响：
畸变模型为：
{ x brown = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y brown = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) \left\{

\right. {xbrown​=x(1+k1​r2+k2​r4+k3​r6)ybrown​=y(1+k1​r2+k2​r4+k3​r6)​此时最大似然估计目标函数变为：
$$\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$$