项目实战——基于计算机视觉的物体位姿定位及机械臂抓取（单目标定）

        请各位读者朋友注意，这里面很多东西涉及到我的毕设，写作辛苦，请勿滥用，转载请务必注明出处！
        单目标定主要分为两个部分，一是确定摄像机的内在参数，二是确定摄像头畸变系数，消除畸变。在进行标定之前首先要建立摄像机模型，用数学语言描述三维世界中的点在摄像机中成像的过程。

全针孔摄像机模型的建立

1、针孔摄像机模型

        在实际应用中，所用摄像机成像的基本原理是小孔成像原理（如图2.2-1所示）：真实物理世界中的光线穿过小孔，在摄像机的图像平面上形成一幅倒立的图像。

        由于成像是倒置的，因此将上述物理模型转化为便于计算的数学模型，处理方法是将成的像放在小孔和实物之间，如图所示，如此一来所成的像便为正像。这样做的目的是为了方便计算，计算结果与上图结果一致。

        为了方便说明，在这里首先定义几个坐标系，后面的内容很多都涉及到坐标系之间的变换：
（1） 世界坐标系：真实世界的三维空间坐标系，其坐标原点可以任意指定。
（2） 相机坐标系：以摄像机焦点为坐标原点创建的三维直角坐标系。
（3） 图像坐标系：以图像中心为原点，创建的二维坐标系,单位长度为毫米。
（4） 像素坐标系：以图像左上角为原点，创建的二维坐标系，单位长度为pixel。

        设上述坐标系（1）（2）重合，并且真实世界中的点$w=[u,v,w]^T$w=[u,v,w]T通过小孔在摄像机图像平面的投影为$x=[x,y]^T$x=[x,y]T。这就是：针孔摄像机模型。

2、归一化相机模型

为了便于分析，首先引入归一化相机的模型。在该模型中，相机的$f=1$f=1，且成像点的中心是图像坐标系的原点$(x,y)$(x,y)。图像在vw平面的投影如图所示：

        根据相似三角形原理，不难得出：

$x=u/w$x=u/w
$y=v/w$y=v/w

        这样，就完成了从3D世界向2D平面的映射。

3、模型改进

        归一化相机在实际情况下是不存在的，它与真实摄像机存在两点差异：
        （1）焦距不为1，且由于图像的最终位置是利用像素进行测量的，因此模型必须将感光体的间距考虑在内，考虑到感光体在x方向和y方向上的间距是不同的，因此引入两个比例因子：φ_x 〖，φ〗_y称为焦距参数。原本的映射关系就变成了：

$x=(φ_x u)/w$x=(φx​u)/w
$y=(φ_y v)/w$y=(φy​v)/w

        （2）像素坐标系的坐标原点和图像坐标系的并不重合，这就需要转移x、y的位置，因此增加偏移量参数：δ_x，δ_y。同时引入偏移参数γ用于控制投影位置x作为真实世界中高度v的函数，原本的映射关系就变成了：

$x=(φ_x u+γv)/w+δ_x$x=(φx​u+γv)/w+δx​
$y=(φ_y v)/w+δ_y$y=(φy​v)/w+δy​

        这就是：摄像机的映射模型。
        另外考虑到摄像机外部因素，摄像机的位置并非总是位于世界坐标系的原点，尤其是在考虑两台及以上摄像机的情形下。为此，需要通过坐标变换，在真实世界通过投影模型前，得出摄像机坐标系中真实世界点$w$w，即:

$\left[\begin{matrix}u'\\v'\\w'\end{matrix}\right]=\left[\begin{matrix}w_{11}&w_{12}&w_{13}\\w_{21}&w_{22}&w_{23}\\w_{31}&w_{32}&w_{33}\\\end{matrix}\right]\left[\begin{matrix}u\\v\\w\end{matrix}\right]+\left[\begin{matrix}τ_x\\τ_y\\τ_z\end{matrix}\right]$⎣⎡​u′v′w′​⎦⎤​=⎣⎡​w11​w21​w31​​w12​w22​w32​​w13​w23​w33​​⎦⎤​⎣⎡​uvw​⎦⎤​+⎣⎡​τx​τy​τz​​⎦⎤​

        可写为：$w'=Ωw+τ$w′=Ωw+τ，其中：$w'$w′是变换后的点，$Ω$Ω是3×3的旋转向量，$τ$τ是3×1的平移向量。

4、全针孔摄像机模型

        综上所述，可以得到从3D世界中的点在2D图像上的映射关系：

$x=(φ_x (w_11 u+w_12 v+w_13 w+τ_x )+γ(w_21 u+w_22 v+w_23 w+τ_y ))/(w_31 u+w_32 v+w_33 w+τ_z )+δ_x$x=(φx​(w1​1u+w1​2v+w1​3w+τx​)+γ(w2​1u+w2​2v+w2​3w+τy​))/(w3​1u+w3​2v+w3​3w+τz​)+δx​
$y=(φ_y (w_21 u+w_22 v+w_23 w+τ_y ))/(w_31 u+w_32 v+w_33 w+τ_z )+δ_y$y=(φy​(w2​1u+w2​2v+w2​3w+τy​))/(w3​1u+w3​2v+w3​3w+τz​)+δy​

        该映射有两套参数，其一是摄像机的内在参数：${φ_x,φ_y,γ,δ_x,δ_y }$φx​,φy​,γ,δx​,δy​，反映的是摄像机自身的性质，其二是摄像机的外在参数：${Ω,τ}$Ω,τ，反映的是摄像机在现实世界中的位置与方向。
于是，最终得到全投影模型的简写形式:

$x=pⅈnholⅇ[w,Λ,Ω,τ]$x=pⅈnholⅇ[w,Λ,Ω,τ]

齐次坐标与单应性变换

        观察单目摄像机的投影模型不难发现：3D世界坐标中的点，在图像的投影上都要除以分母w，这就使得投影成为非线性的，不方便研究。因此对2D 图像点和3D世界点的表达形式做出修改，使得投影方程变为线性的。
        将2D图像点的坐标转化为3D齐次坐标x ̃：

$\overline{x}=λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]$x=λ⎣⎡​xy1​⎦⎤​

        将3D世界点的坐标转化为4D齐次坐标w^：

$\overline{w}=λ\left[\begin{matrix}u\\v\\w\\1\end{matrix}\right]$w=λ⎣⎢⎢⎡​uvw1​⎦⎥⎥⎤​

        这样在不考虑相机位姿的情况下，原本的投影方程就转化为：

$λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]=\left[\begin{matrix}φ_x&γ&δ_x&0\\0&φ_y&δ_y&0\\0&0&1&0\end{matrix}\right]\left[\begin{matrix}u\\v\\w\\1\end{matrix}\right]$λ⎣⎡​xy1​⎦⎤​=⎣⎡​φx​00​γφy​0​δx​δy​1​000​⎦⎤​⎣⎢⎢⎡​uvw1​⎦⎥⎥⎤​

        可写为：

$λx=φ_x u+γv+δ_x w$λx=φx​u+γv+δx​w
$λy=φ_y v+δ_y w$λy=φy​v+δy​w
$λ=w$λ=w

        经过升维处理，原本的非线性映射关系就变成了线性的了。同时不难看出，等式右侧是摄像机内在参数矩阵，它被储存在内在矩阵Λ中：

$Λ=\left[\begin{matrix}φ_x&γ&δ_x\\0&φ_y&δ_y\\0&0&1\end{matrix}\right]$Λ=⎣⎡​φx​00​γφy​0​δx​δy​1​⎦⎤​

        考虑到相机坐标系和世界坐标系并不重合，需要在投影方程的右边乘以一个姿态变换的矩阵，即上述坐标转换矩阵。因此，真实世界的三维点投影到二维的成像平面的映射方程为：

$\lambda\left[\begin{matrix}x\\y\\1\end{matrix}\right]=\left[\begin{matrix}φ_x&γ&δ_x&0\\0&φ_y&δ_y&0\\0&0&1&0\end{matrix}\right]\left[\begin{matrix}w_{11}&w_{12}&w_{13}&τ_x\\w_{21}&w_{22}&w_{23}&τ_y\\w_{31}&w_{32}&w_{33}&τ_z\\0&0&0&1\end{matrix}\right]+\left[\begin{matrix}u\\v\\w\end{matrix}\right]$λ⎣⎡​xy1​⎦⎤​=⎣⎡​φx​00​γφy​0​δx​δy​1​000​⎦⎤​⎣⎢⎢⎡​w11​w21​w31​0​w12​w22​w32​0​w13​w23​w33​0​τx​τy​τz​1​⎦⎥⎥⎤​+⎣⎡​uvw​⎦⎤​

        可简写为：

$λ\overline{x}=(Λ,0)\left[\begin{matrix}Ω &τ\\0^T& 1\end{matrix}\right]=Λ(Ω,τ)$λx=(Λ,0)[Ω0T​τ1​]=Λ(Ω,τ)

        为了方便研究，这里引入一个新的概念：单应性变换。它的定义为：

$H=(Λ,0)\left[\begin{matrix}Ω &τ\\0^T& 1\end{matrix}\right]=Λ(Ω,τ)$H=(Λ,0)[Ω0T​τ1​]=Λ(Ω,τ)

不难看出，该变换实际上描述的是：真实物理世界中的点向所拍摄图像中的像素点之间的映射，用单应性矩阵这种映射关系：

$\overline{x}=H\overline{w}$x=Hw

        至此，摄像机投影模型建立完毕，现在所有问题的矛盾都指向摄像机内参矩阵Λ的求解。

张正友标定法

        张正友于1998年提出了求解相机内参Λ的方法，它仅需要一个标定棋盘即可完成（如图2.2-4所示）。该方法以其简单、实用、高效的特性，成为了目前单目相机标定的主流算法。

        张氏标定法的具体思路是：用于标定的棋盘是三维世界的一个平面$Π$Π，它在成像平面所成的像为另一个平面$π$π。由于标定棋盘的角点坐标已知，图像的角点可以通过角点识别算法求得，通过两个平面的对应点的坐标，就可以求解出单应矩阵H。从而求出摄像机的内参数，完成标定。下面具体说明其算法。
        设棋盘所在平面为世界坐标系下的$z=0$z=0的平面。这样，棋盘上的任意角点在世界坐标系中可表示为$(X,Y,0)$(X,Y,0)。代入映射方程可得:

$λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]=Λ(Ω,τ)\left[\begin{matrix}X\\Y\\0\\1\end{matrix}\right]=Λ(w_1, w_2, w_3, τ)\left[\begin{matrix}X\\Y\\0\\1\end{matrix}\right]=Λ(w_1,w_2,τ)\left[\begin{matrix}X\\Y\\1\end{matrix}\right]$λ⎣⎡​xy1​⎦⎤​=Λ(Ω,τ)⎣⎢⎢⎡​XY01​⎦⎥⎥⎤​=Λ(w1​,w2​,w3​,τ)⎣⎢⎢⎡​XY01​⎦⎥⎥⎤​=Λ(w1​,w2​,τ)⎣⎡​XY1​⎦⎤​

        用单应矩阵可以表示为：

$λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]=H\left[\begin{matrix}X\\Y\\1\end{matrix}\right]$λ⎣⎡​xy1​⎦⎤​=H⎣⎡​XY1​⎦⎤​

        联立两式可得（由于求出的H与实际上的H之间可能存在一定的比例误差，这里增加一个比例因子s）：

$H=sΛ((w_1,w_2,τ))$H=sΛ((w1​,w2​,τ))

        通过平面间的单应可得：

$H=[h_1,h_2,h_3 ]=sΛ(w_1,w_2,τ)$H=[h1​,h2​,h3​]=sΛ(w1​,w2​,τ)

        将矩阵Ω中的列向量用单应矩阵的列向量表示:

$w_1=sΛ^{-1}h_1$w1​=sΛ−1h1​
$w_2=sΛ^{-1}h_2$w2​=sΛ−1h2​
$τ=sΛ^{-1}h_2$τ=sΛ−1h2​

        矩阵Ω是旋转矩阵，本质上它也是正交矩阵，根据正交矩阵的性质：列向量的模为1，且任意两个列向量的向量内积均为0。则有:

$w_1^T w_2=0$w1T​w2​=0
$‖w_1 ‖=‖w_2 ‖=1)$‖w1​‖=‖w2​‖=1)

        将等式用单应矩阵的列向量表示，可以得到在一幅标定板图像（即：一个单应矩阵）中的两组约束条件。

$h_1^T Λ^{-T} Λ^{-1} h_2=0$h1T​Λ−TΛ−1h2​=0
$h_1^T Λ^{-T} Λ^{-1} h_1=h_2^T Λ^{-T} Λ^{-1} h_2=1$h1T​Λ−TΛ−1h1​=h2T​Λ−TΛ−1h2​=1

观察等式，注意到两个等式中都有$Λ^{-T} Λ^{-1}$Λ−TΛ−1。于是，令$B=Λ^{-T} Λ^{-1}$B=Λ−TΛ−1，则有：

$B=Λ^{-T} Λ^{-1}=\left[\begin{matrix}B_11&B_{12}&B_{13}\\B_{21}&B_{22}&B_{23}\\B_{31}&B_{32}&B_{33}\end{matrix}\right]=\left[\begin{matrix}1/φ_x^2 &-γ/φ_x^2 φ_y &(δ_y γ-δ_x φ_y)/(φ_x^2 φ_y ) \\-γ/(φ_x^2 φ_y )&γ/(φ_x^2 φ_y^2 )+1/φ_y^2 &-γ(δ_y γ-δ_x φ_y )/(φ_x^2 φ_y^2 )-δ_y/φ_y^2\\(δ_y γ-δ_x φ_y)/(φ_x^2 φ_y )&-γ(δ_y γ-δ_x φ_y )/(φ_x^2 φ_y^2 )-δ_y/φ_y^2 &(δ_y γ-δ_x φ_y )^2/(φ_x^2 φ_y^2 )+δ_y/φ_y^2 +1\end{matrix}\right]$B=Λ−TΛ−1=⎣⎡​B1​1B21​B31​​B12​B22​B32​​B13​B23​B33​​⎦⎤​=⎣⎡​1/φx2​−γ/(φx2​φy​)(δy​γ−δx​φy​)/(φx2​φy​)​−γ/φx2​φy​γ/(φx2​φy2​)+1/φy2​−γ(δy​γ−δx​φy​)/(φx2​φy2​)−δy​/φy2​​(δy​γ−δx​φy​)/(φx2​φy​)−γ(δy​γ−δx​φy​)/(φx2​φy2​)−δy​/φy2​(δy​γ−δx​φy​)2/(φx2​φy2​)+δy​/φy2​+1​⎦⎤​

        不难看出B是一个3×3的对称阵，因此只有六个未知量。将六个未知量写成向量形式：

$b=\left[\begin{matrix}B_11&B_12&B_22&B_13&B_23&B_33\end{matrix}\right]$b=[B1​1​B1​2​B2​2​B1​3​B2​3​B3​3​]

        用$h_ij$hi​j表示矩阵H的第$i$i行第$j$j列，则约束方程中的$h_1^T$h1T​为：

$h_1^T=\left[\begin{matrix}h_i1\\h_i2\\h_i3\end{matrix}\right]$h1T​=⎣⎡​hi​1hi​2hi​3​⎦⎤​

        因此：

$h_j^T Λ^{-T}Λ^{-1} h_j=h_i Λ^{-T} Λ^{-1} h_j=v_ij^T b$hjT​Λ−TΛ−1hj​=hi​Λ−TΛ−1hj​=vi​jTb

        其中：

$v_ij=\left[\begin{matrix}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{matrix}\right]^T$vi​j=[hi​1hj​1​hi​1hj​2+hi​2hj​1​hi​2hj​2​hi​3hj​1+hi​1hj​3​hi​3hj​2+hi​2hj​3​hi​3hj​3​]T

        现在再来看之前的两个约束公式，可转换为：

$h_1^T Λ^{-T} Λ^{-1} h_2=v_12^T b=0$h1T​Λ−TΛ−1h2​=v1​2Tb=0
$h_1^T Λ^{-T}Λ^{-1}h_1=v_{11} b=h_2^T Λ^{-T} Λ^{-1}h_2=v_{22} b=1$h1T​Λ−TΛ−1h1​=v11​b=h2T​Λ−TΛ−1h2​=v22​b=1

        写为矩阵形式则有：

$\left[\begin{matrix}v_12^T\\(v_{11}-{v_22})^T \end{matrix}\right]b=0$[v1​2T(v11​−v2​2)T​]b=0

        或：

$Vb=0$Vb=0
$V=\left[\begin{matrix}v_12^T\\(v_{11}-{v_22})^T \end{matrix}\right]$V=[v1​2T(v11​−v2​2)T​]

        对于上述方程，V是一个2n×6的矩阵，一共有六个维度。因此对于一幅标定板图像，可以得到两个方程，而未知量有：$φ_x$φx​、$φ_y$φy​ 、$δ_x$δx​、 $δ_y$δy​ 、 $γ$γ。因此，使得方程有解的充要条件是$n≥3$n≥3，即：需要拍摄三张以上的棋盘图像。张正友标定法，就是通过获取标定图像的棋盘信息，求解方程$Vb=0$Vb=0得到矩阵$Λ$Λ，完成单目标定。下面对该方程进行求解。

内参数求解

        对于m个线性无关的方程组求解n个未知数，共有三种情况：
        1、$m>n$m>n，约束的数目大于未知数的数目，称为超定问题。
        2、$m=n$m=n，约束的数目等于未知数的数目，方程的解唯一。
        3、$m<n$m<n，约束的数目小于未知数的数目，称为欠定问题。
        对于求解$Vb=0$Vb=0的问题，当$n≥3$n≥3时，共有六个以上的方程，而需要求解的未知数共五个，因此$m>n$m>n，该求解问题为超定问题。也就是说，该方程的解是不存在的，此时求方程解的问题就转换为求解最小二乘解的问题，即：$min‖Vb-0‖$min‖Vb−0‖。
        本文采用奇异值（SVD）分解法求解最小二乘问题。
        SVD的定义：对于任意一个m×n维的矩阵A，都可以分解为：$A=USV^T$A=USVT。 SVD分解法，可以通过下图形象地说明：

        V是一个$n×n$n×n的正交阵，它的列向量是$A^T A$ATA的特征向量。
        U是一个$m×m$m×m的正交阵，它的列向量是$AA^T$AAT的特征向量。
        S（可写为$Σ_r$Σr​）是r个沿对角线降序排列的奇异值组成的方阵。
        $A^T$ATA和$AA^T$AAT用SVD分解可表示为:

$A^T A=(VS^T U^T )USV^T=VS^T (U^T U)SV^T=V(S^T S) V^T$ATA=(VSTUT)USVT=VST(UTU)SVT=V(STS)VT
$AA^T=USV^T (VS^T U^T )=US^T (V^T V)SU^T=U(SS^T ) U^T$AAT=USVT(VSTUT)=UST(VTV)SUT=U(SST)UT
        $AA^T$AAT是实对称阵，它的特征值为$λ$λ，则S中的奇异值为：$σ=√λ$σ=√λ，V为$A^T A$ATA的特征向量。S中的奇异值是降序陈列的，通常情况下，可以用前几个奇异值近似的描述A而不损失过多的信息。这就是求解该方程的关键。据此，矩阵A可以用前r大的奇异值近似描述为：
$A_(m×n)≈U_(m×r)×S_(r×r)×V_(r×n)^T (r<n)$A(​m×n)≈U(​m×r)×S(​r×r)×V(​r×n)T(r<n)

        下面，利用奇异值（SVD）分解法求解一开始的问题。
        设$A∈R^(m×n)$A∈R(m×n)列满秩， A=UΣV^T是对A的奇异值分解。令$U_n$Un​为U的前n列矩阵，即：$U=[U_n，\overline U]$U=[Un​，U]，则：

$‖Ax-b‖_2^2=‖UΣV^T x-b‖=‖ΣV^T x-[U_n，\overline{U}]^T b‖=‖ΣV^T x-U_n^T b-\overline U^T b‖=‖ΣV^T x-U_n^T b+‖\overline{U}^T b‖≥‖\overline{U}^T b‖‖$‖Ax−b‖22​=‖UΣVTx−b‖=‖ΣVTx−[Un​，U]Tb‖=‖ΣVTx−UnT​b−UTb‖=‖ΣVTx−UnT​b+‖UTb‖≥‖UTb‖‖

        当且仅当$ΣV^T x-U_n^T b=0$ΣVTx−UnT​b=0时等号成立，所以：

$x=(ΣV^T )^(-1) U_n^T b=VΣ^(-1) U_n^T b$x=(ΣVT)(−1)UnT​b=VΣ(−1)UnT​b

        即为最小二乘问题的解。
        因此，$Vb=0$Vb=0的最小二乘解为：$V^T V$VTV的$λ_min$λm​in对应的特征向量。采用SVD法，最终求得摄像机的内参数为：

$δ_x=(γδ_y)/φ_y -(B_{13} φ_x^2)/λ$δx​=(γδy​)/φy​−(B13​φx2​)/λ
$δ_y=B_{12} B_{13}-B_{11} B_{23})/B_{11} B_{22}-B_{12}^2$δy​=B12​B13​−B11​B23​)/B11​B22​−B122​
$φ_x=√(λ/B_{11} )$φx​=√(λ/B11​)
$φ_y=√((λB_11)/(B_{11} B_{22}-B_{12}^2 ))$φy​=√((λB1​1)/(B11​B22​−B122​))
$γ=(-B_{12} φ_x^2 φ_y)/λ$γ=(−B12​φx2​φy​)/λ
$λ=B_{33}-(B_{13}^2+δ_y (B_{12}B_{13} -B_{11} B_{23} ))/B_{11} )$λ=B33​−(B132​+δy​(B12​B13​−B11​B23​))/B11​)

        上述通过最小二乘法得到的内参数的解，是贴近摄像机真实内参数的（由于求解精确解是不可能的，只能转而求解近似解）。为了进一步增加标定精度，本文采用最大似然估计法对上述求解结果进行优化。
        假设一台摄像机拍摄得到了n幅的不同位置的标定板图像，每幅图像上有m个像点，用$M_ij$Mi​j表示第i幅图像上的第j个像点对应世界坐标系中标定板的三维角点。则像点可表示为：

$\overline m(Λ,Ω_i,τ_i,M_ij)=Λ[Ω,τ]M_ij$m(Λ,Ωi​,τi​,Mi​j)=Λ[Ω,τ]Mi​j

        图像中对应的像点m_ij符合正态分布，它的概率密度函数可表示为：

$f(m_ij )=\frac{1}{√2π} e^{-(\overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij )^2/σ^2}$f(mi​j)=√2π1​e−(m(Λ,Ωi​,τi​,Mi​j)−mi​j)2/σ2

        紧接着，构造似然函数：

$L(Λ,Ω_i,τ_i,M_ij )=∏_{i=1,j=1}^{n,m}f(m_ij )= \frac{1}{√2π}e^{((-∑_{i=1}^n∑_{j=1}^m(\overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij )^2 )/σ^2)}$L(Λ,Ωi​,τi​,Mi​j)=∏i=1,j=1n,m​f(mi​j)=√2π1​e((−∑i=1n​∑j=1m​(m(Λ,Ωi​,τi​,Mi​j)−mi​j)2)/σ2)

        欲求得L的最大值，只需使以下值最小化：

$∑_{i=1}^n∑_{j=1}^m ‖ \overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij ‖^2$∑i=1n​∑j=1m​‖m(Λ,Ωi​,τi​,Mi​j)−mi​j‖2

        这是一个非线性公式，求解非线性函数最小化，目前主流算法有Newton method、Gauss Newton iteration、Steepest Descent。Levenberg和Marquardt结合Gauss-Newton algorithm以及Steepest Descent的优点，并对两者的不足之处作改善，提出了Levenberg–Marquardt 方法。该方法通过迭代，能最大程度上避免局部最小值的出现。在冗余参数较多时，优化效果更为出色。
        Levenberg–Marquardt algorithm的具体算法为:
        目标：$minF(x)$minF(x),本文中$F(x)=∑_{i=1}^n∑_{j=1}^m‖\overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij ‖^2$F(x)=∑i=1n​∑j=1m​‖m(Λ,Ωi​,τi​,Mi​j)−mi​j‖2
        计算步骤：
        Step1：选取初始点$x_0$x0​, 令$k=0$k=0并选取参数$ε$ε、$μ_0$μ0​ 、$γ_1$γ1​ 、$γ_2$γ2​ 、$η_1$η1​ 、$η_2$η2​使得：

$0<ε<1$0<ε<1、$μ_0>0$μ0​>0、$0<γ_1<1<γ_2$0<γ1​<1<γ2​、$0<η_1≤η_2≤1$0<η1​≤η2​≤1

        本文中，迭代的初始点$x_0$x0​即为上述利用SVD方法求解出的相机内参数值。
        Step2：若$‖J_k^T F_k ‖≤ε$‖JkT​Fk​‖≤ε，则结束，否则执行Step3；
        Step3：计算：

$d_k (μ_k )=-(J_k^T J_k+μ_k I)^(-1) J_k^T F_k$dk​(μk​)=−(JkT​Jk​+μk​I)(−1)JkT​Fk​

        Step4：计算：

$ρ_k (d_k (μ_k ),μ_k )=(ϕ(x_k )-ϕ(x_k+d_k (μ_k )))/(ϕ(x_k )-ψ_k (d_k (μ_k ),μ_k ) )$ρk​(dk​(μk​),μk​)=(ϕ(xk​)−ϕ(xk​+dk​(μk​)))/(ϕ(xk​)−ψk​(dk​(μk​),μk​))

        Step5:

若$ρ_k (d_k (μ_k ),μ_k )<η_1$ρk​(dk​(μk​),μk​)<η1​，则取$μ_(k+1)=γ_2 μ_k$μ(​k+1)=γ2​μk​；
若$η_2>ρ_k (d_k (μ_k ),μ_k )≥η_1$η2​>ρk​(dk​(μk​),μk​)≥η1​，则取$μ_(k+1)=μ_k$μ(​k+1)=μk​；
若$ρ_k (d_k (μ_k ),μ_k )≥η_2$ρk​(dk​(μk​),μk​)≥η2​，则取$μ_(k+1)=γ_1 μ_k$μ(​k+1)=γ1​μk​；

        Step6：取$x_(k+1)=x_k+d_k (μ_k )$x(​k+1)=xk​+dk​(μk​)，令$k=k+1$k=k+1，返回Step2.
        对相关公式的说明：
        $J(x)$J(x)是$F(x)$F(x)的雅可比（Jacobian）矩阵。
        $μ_k$μk​是一个正参数，其目的是：防止当$J_k^T J_k$JkT​Jk​接近奇异时$d_k$dk​过大。
        $ϕ(x)=1/2 ‖F(x)‖^2$ϕ(x)=1/2‖F(x)‖2
        以上即为Levenberg–Marquardt algorithm，通过选取合适的梯度迭代，使得用SVD方法求解得到的结果更加逼近摄像机的真实内参数。

摄像头畸变

        在进行理论计算时，假定透镜是没有畸变的，但在实际情况下，不存在真正无畸变的透镜。摄像机在生产的过程中，透镜的制造精度存在限制，同时在装配的时候，很难将透镜与成像装置完全对齐，因此，会使成像产生多种形式的畸变。

1、径向畸变

        径向畸变是一种沿着透镜半径方向分布的畸变，一般来说越靠近边缘，光线就越容易弯曲，畸变也就越严重。径向畸变根据畸变情况不同，可分为桶形畸变和枕形畸变。图2.2-1和图2.2-2展示了两种畸变情况。

        对于镜头来说，径向畸变程度从光轴中心（畸变值为零）向边缘逐渐加剧，这种畸变可以用r=0附近的泰勒数级展开式的前两项来描述，即：k1、k2。对于畸变较大的相机，可以引入第三个径向畸变系数k3来矫正。成像装置上某点的径向位置可以根据下面的公式调整：

$x'=x×(1+k_1 r^2+k_2 r^4 )$x′=x×(1+k1​r2+k2​r4)
$y'=y×(1+k_1 r^2+k_2 r^4 )$y′=y×(1+k1​r2+k2​r4)

2、切向畸变

        切向畸变通常是由于在机械装配的过程中，透镜与成像平面不平行导致的。同样地，它可以用p1、p2这两个参数描述。可根据公式调整：

$x'=x+[2p_1 xy+p_2 (r^2+2x^2 )]$x′=x+[2p1​xy+p2​(r2+2x2)]
$y'=y+[2p_2 xy+p_1 (r^2+2y^2)]$y′=y+[2p2​xy+p1​(r2+2y2)]

        综上，摄像头的畸变可以用k1、k2、（k3）、p1、p2，共四（五）个参数表示。由于对摄像头影响较大的为径向畸变，而切向畸变很小，尤其是对于工业相机，其装配过程更为严格。因此，这里只需要进行径向畸变矫正。

畸变矫正

        张正友标定法中也涉及到了对摄像头畸变矫正的标定方法。根据2.2.5，径向畸变在图像坐标系下可表示为：

$\overline x=x+x[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^2 ]$x=x+x[k1​(x2+y2)+k2​(x2+y2)2]
$\overline y=y+y[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^2 ]$y​=y+y[k1​(x2+y2)+k2​(x2+y2)2]

        其中：
        $(x,y)$(x,y)：无畸变的归一化的图像坐标；
        $(\overline x,\overline y )$(x,y​)：畸变后的图像坐标。
        在像素坐标系下可表示为：

$\overline μ=μ_0+φ_x \overline x+γ\overline y$μ​=μ0​+φx​x+γy​
$\overline υ=ν_0+φ_y \overline y$υ=ν0​+φy​y​

        其中：
         $(μ,υ)$(μ,υ)：无畸变的像素坐标点；
         $(\overline μ,\overline υ)$(μ​,υ)：畸变后的像素坐标点；
         $(μ_0,υ_0 )$(μ0​,υ0​)：摄像机的主心位置。
        将二者结合起来，就可以得到像素坐标系下的径向畸变公式，如果不考虑γ的影响，令$γ=0$γ=0，则:

$\overlineμ=μ+(μ-μ_0)[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^2 ]$μ​=μ+(μ−μ0​)[k1​(x2+y2)+k2​(x2+y2)2]
$\overlineν=ν+(ν-ν_0)[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^2 ]$ν=ν+(ν−ν0​)[k1​(x2+y2)+k2​(x2+y2)2]

        改用矩阵形来写为：

$\left[\begin{matrix}((μ-μ_0)(x^2+y^2 )&(μ-μ_0)(x^2+y^2 )^2\\(ν-ν_0)(x^2+y^2 )&(ν-ν_0)(x^2+y^2 )^2 )^T \end{matrix}\right]\left[\begin{matrix}k_1\\k_2\end{matrix}\right]=\left[\begin{matrix}\overline μ-μ\\\overline v-v\end{matrix}\right]$[((μ−μ0​)(x2+y2)(ν−ν0​)(x2+y2)​(μ−μ0​)(x2+y2)2(ν−ν0​)(x2+y2)2)T​][k1​k2​​]=[μ​−μv−v​]

        上述公式是通过一张标定图像上的一个棋盘角点取得的，设摄像机共拍摄n张棋盘图，每幅图上有m个棋盘角点。因此，最终可以得到2mn个等式，写成矩阵形式：

$Dk=d$Dk=d

        这种形式类似于Vb=0的形式，因此可用相似的思路解决。即:利用最大似然法估计取得最优解，采用Levenberg–Marquardt方法优化公式求得最优解：

$F(x)=∑_{i=1}^n∑_{j=1}^m‖\overline m(Λ,k_1,k_2,Ω_i,τ_i,M_ij )-m_ij ‖^2$F(x)=∑i=1n​∑j=1m​‖m(Λ,k1​,k2​,Ωi​,τi​,Mi​j)−mi​j‖2

        用此方法得到的畸变系数$k_1$k1​,$k_2$k2​，可以对图像进行去畸变处理，使得摄像机成的像与真实世界的一致。

Harris角点检测

        在张氏标定法的具体思路中，对于标定棋盘图像角点坐标的获取，是通过Harris角点提取算法获得的，现在对这一算法方法进行详细说明。
        该算法的主要思路是：令一个$n×n$n×n的窗口在图像上移动，通过比较临近像素点的灰度差，判断灰度是否发生较大变化，从而判断是否为角点、边缘、平滑区域。
        定义$E(u,v)$E(u,v)为窗口W在图像上移动$(u,v)$(u,v)个像素的灰度变换：

$E(u,v)=∑_{x,y}w(x,y)[I(x,y)-I(x+u,y+v)^2 ]$E(u,v)=∑x,y​w(x,y)[I(x,y)−I(x+u,y+v)2]

        其中w(x,y)为高斯核函数：

$w(x,y)=e^{-((x^2+y^2 ))/σ^2 }$w(x,y)=e−((x2+y2))/σ2

        由泰勒级数，可得：

$I(x+u,y+v)= I(x,y)+I_x u+I_y v$I(x+u,y+v)=I(x,y)+Ix​u+Iy​v

        其中$I_x=δI/δx$Ix​=δI/δx,$I_y=δI/δy$Iy​=δI/δy，将其代入$E(u,v)$E(u,v)，可得：

$E(u,v)=∑_{x,y}w(x,y) [I_x u+I_y v]^2= ∑_{x,y}w(x,y)[u,v]\left[\begin{matrix}I_x^2&I_x I_y\\I_x I_y&I_y^2\end{matrix}\right]\left[\begin{matrix}μ\\v\end{matrix}\right]$E(u,v)=∑x,y​w(x,y)[Ix​u+Iy​v]2=∑x,y​w(x,y)[u,v][Ix2​Ix​Iy​​Ix​Iy​Iy2​​][μv​]

令：

$M=∑_{x,y}w(x,y)\left[\begin{matrix}I_x^2&I_x I_y\\I_x I_y&I_y^2\end{matrix}\right]=\left[\begin{matrix}A&C\\C&B\end{matrix}\right]$M=∑x,y​w(x,y)[Ix2​Ix​Iy​​Ix​Iy​Iy2​​]=[AC​CB​]

其中：

$A=I_x×w$A=Ix​×w
$B=I_y×w$B=Iy​×w
$C=(I_x I_y )×w$C=(Ix​Iy​)×w

        矩阵M是一个自相关矩阵，因此该矩阵为海森矩阵，其特征值反映了自相关函数E(u,v)的曲率。根据这两个特征值（记为：λ_1,λ_2）判断区域特征：
        $λ_1$λ1​,$λ_2$λ2​都比较小时，灰度相差不大，表示该区域为平坦区域；
        $λ_1$λ1​,$λ_2$λ2​有一较小，另一较大时，灰度相差明显，表示该区域为边缘区域；
        $λ_1$λ1​,$λ_2$λ2​都比较大时，灰度相差显著，表示该区域为角点区域。
        由于这里只需要判断$λ_1$λ1​,$λ_2$λ2​的相对大小，并不需要知道它们具体的值，可以通过角点响应函数进行判断：

$R=(λ_1 λ_2 )-k(λ_1 +λ_2 )^2=|M|-k×tr^2 (M)$R=(λ1​λ2​)−k(λ1​+λ2​)2=∣M∣−k×tr2(M)

        这里的系数k一般取0.04~0.06。此时，可根据R值间接判断目标像素点的特征：

        当|R|比较小时，表示该区域为平坦区域；
        当R<0且|R|较大时，表示该区域为边缘区域；
        当R>0且|R|较大时，表示该区域为角点区域。

        综上，Harris角点检测算法的步骤总结如下：
        Step1：根据图像I计算梯度图像$I_x$Ix​,$I_y$Iy​，并计算$I_x^2,I_y^2,I_x I_y$Ix2​,Iy2​,Ix​Iy​；
        Step2：对$I_x^2$Ix2​,$I_y^2$Iy2​,$I_x I_y$Ix​Iy​进行高斯滤波处理；
        Stap3：对目标像素点构造自相关矩阵M，$M=[I_x^2,I_x I_y;I_x I_y,I_y^2 ]$M=[Ix2​,Ix​Iy​;Ix​Iy​,Iy2​]；
        Step4：构造角点响应函数$R=|M|-k×tr^2 (M)$R=∣M∣−k×tr2(M)；
        Step5：对R进行非极大值抑制，选取大于阈值T且是局部极大值的点作为角点。

OpenCV单目标定

        上面详细阐述了张正友标定法的具体算法，下面详细阐述其具体操作方法。
        目前有很多关于计算机视觉方面的库，其中以OpenCV（Open Source Computer Vision Library）最为出名。它以其丰富的视觉函数库，和强大的平台适用性，被广泛应用在图像降噪、产品质检、图像拼接、人脸识别、无人驾驶、人机交互、动作识别等领域，最新的OpenCV版本为4.1，还提供了机器学习模块。
        OpenCV提供了张正友标定法的实现。本文采用的C++语言开发，IDE为VS2015，OpenCV版本为4.0版本。以下介绍其主要实现函数：

1、寻找棋盘

        bool cv::findChessboardCorners( //如果寻找到角点则返回1，否则返回0
        cv::InputArray image, //输入的棋盘图
        cv::Size board_sz, //标定棋盘图像中的角点的个数
        cv::OutputArray corners， //记录角点位置的输出矩阵
        Int flags //实现一个或多个附加滤波：
        );
        /对flags的说明：
        CV_CALIB_CB_ADAPTIVE_THRESH – 采用自适应阈值滤波。
        CV_CALIB_CB_NORMALIZE_IMAGE – 首先对图像亮度进行平均化处理（采用函数: cvNormalizeHist）,随后再进行滤波处理
        CV_CALIB_CB_FILTER_QUADS – 采用其他规则，剔除错误棋盘格块
        /

2、绘制棋盘

        void cv::drawChessboardCorners(
        cv::InputOutputArray image, //输入和输出的棋盘格图
        cv::Size patternSize, //标定棋盘图像中的角点的个数
        cv::InputArray corners， //从函数1中返回的角点
        bool patternWasFound //指出是否已找到所有的角点，0表示未找到
        )

3、摄像机单目标定

        double cv::calibrateCamera(
        cv::InputArrayOfArrays objectPoints, //世界坐标系中的点
        cv::InputArrayOfArrays imagePoints, //对应的图像点
        cv::Size imageSize, //仅用于储存摄像机内参矩阵图像
        cv::InputOutputArray cameraMatrix, //摄像机内参矩阵（3x3）
        cv::InputOutputArray distCoeffs, //畸变系数的输出矢量
        cv::OutputArrayOfArrays rvecs, //为每个模式视图估计旋转矩阵
        cv::OutputArrayOfArrays tvecs, //为每个模式视图估计平移矩阵
        int flags
        )
        /对flag的说明：
        CV_CALIB_USE_INTRINSIC_GUESS cameraMatrix采用高斯法优化摄像机内参矩阵；
        CV_CALIB_FIX_PRINCIPAL_POINT在进行优化的时候不改变主点位置；
        CV_CALIB_FIX_ASPECT_RATIO仅设置δ_y为可变参数，而不改变δ_y/δ_x 的值；
        CV_CALIB_ZERO_TANGENT_DIST k1=0,k2=0；
        CV_CALIB_RATIONAL_MODEL计算系数k4、k5和k6；
        CALIB_THIN_PRISM_MODEL计算系数s1，s2，s3和s4 ；
        CALIB_FIX_S1_S2_S3_S4在进行优化的过程中，不改变系数s的值
        CALIB_TILTED_MODEL计算系数tauX和tauY已启用；
        /

4、计算矫正映射

        void cv :: initUndistortRectifyMap（
        cv::InputArray cameraMatrix， //输入相机矩阵
        cv::InputArray distcoeffs， //畸变系数的输入向量
        cv::InputArray R， //对象空间中的可选整流变换（3x3矩阵）
        cv::InputArray newCameraMatrix， //校正后的相机矩阵
        cv::Size ， //理想无畸变的图像大小
        int mltype，//指定输出映射类型
        cv::OutputArray map1，//第一个输出图
        cv::OutputArray map2 //第二个输出图
        ）

5、重映射（对图像进行无畸变化处理）

        void cv :: remap （
        InputArray src，//原始图像
        OutputArray dst，//目标图像
        InputArray map1，//（x,y）的第一个映射点
        InputArray map2，//（x,y）的第二个映射点
        int interpolation，//插值方法，有四中插值方式：
        /*
        （1）INTER_NEAREST——最近邻插值
        （2）INTER_LINEAR——双线性插值
        （3）INTER_CUBIC——双三样条插值
        （4）INTER_LANCZOS4——lanczos插值
        */
        int borderMode = BORDER_CONSTANT，//像素外推方法
        const Scalar＆ borderValue =Scalar() //一般取值为0
        ）
        在本文程序中，利用张正友方法进行单目标定写成了一个单独的.c文件，名称为Camera_calibration，其主函数为：
        vectorcv::Mat Camera_calibration(
         int board_w, //棋盘的宽度
         int board_h, //棋盘的高度
         int n_boards, //监测标定图像的数目，后面在输入参数里面获取，为了保证参数的求解精度，我们至少需要10张以上的图像
        int delay, //相机的拍摄延时为1s
         double image_sf, //缩放比例为0.5
         int cap //选择调用相机
）
        对两个摄像机分别拍摄15幅棋盘标定板图像运用OpenCV进行单目标定，下面截取其中的两张标定图像：

        最终求得的相机参数如表所示：

        Hunt Tiger Tonight
        2019-10-29
        联系方式：[email protected]（请勿使用其他联系方式，谢谢！）
        PS:再次请各位读者朋友注意，这里面很多东西涉及到我的毕设，写作辛苦，请勿滥用，转载请务必注明出处！