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

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

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

全针孔摄像机模型的建立

1、针孔摄像机模型

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

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

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

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

2、归一化相机模型

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

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

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

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

3、模型改进

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

$x=({\phi }_{x}u)/w$
$y=({\phi }_{y}v)/w$

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

$x=({\phi }_{x}u+\gamma v)/w+{\delta }_x$
$y=({\phi }_{y}v)/w+{\delta }_y$

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

$\left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\\ w^{\prime }\end{array}\right]=\left[\begin{array}{ccc}{w}_{11}& w_{12}& w_{13}\\ w_{21}& w_{22}& w_{23}\\ w_{31}& w_{32}& w_{33}\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\end{array}\right]+\left[\begin{array}{c}{\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]$

        可写为：$w^{\prime }=\Omega w+\tau$，其中：$w^{\prime }$是变换后的点，$\Omega$是3×3的旋转向量，$\tau$是3×1的平移向量。

4、全针孔摄像机模型

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

$x=({\phi }_x(w_{1}1u+w_{1}2v+w_{1}3w+{\tau }_x)+\gamma (w_{2}1u+w_{2}2v+w_{2}3w+{\tau }_y))/(w_{3}1u+w_{3}2v+w_{3}3w+{\tau }_z)+{\delta }_x$
$y=({\phi }_y(w_{2}1u+w_{2}2v+w_{2}3w+{\tau }_y))/(w_{3}1u+w_{3}2v+w_{3}3w+{\tau }_z)+{\delta }_y$

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

$x=pinhole[w,\Lambda ,\Omega ,\tau ]$

齐次坐标与单应性变换

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

$\overline{x}=\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$

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

$\overline{w}=\lambda \left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]$

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

$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\phi }_x& \gamma & {\delta }_x& 0\\ 0& {\phi }_y& {\delta }_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]$

        可写为：

$\lambda x={\phi }_{x}u+\gamma v+{\delta }_{x}w$
$\lambda y={\phi }_{y}v+{\delta }_{y}w$
$\lambda =w$

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

$\Lambda =\left[\begin{array}{ccc}{\phi }_x& \gamma & {\delta }_x\\ 0& {\phi }_y& {\delta }_y\\ 0& 0& 1\end{array}\right]$

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

$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\phi }_x& \gamma & {\delta }_x& 0\\ 0& {\phi }_y& {\delta }_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cccc}{w}_{11}& w_{12}& w_{13}& {\tau }_x\\ w_{21}& w_{22}& w_{23}& {\tau }_y\\ w_{31}& w_{32}& w_{33}& {\tau }_z\\ 0& 0& 0& 1\end{array}\right]+\left[\begin{array}{c}u\\ v\\ w\end{array}\right]$

        可简写为：

$\lambda \overline{x}=(\Lambda ,0)\left[\begin{array}{cc}\Omega & \tau \\ 0^T& 1\end{array}\right]=\Lambda (\Omega ,\tau )$

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

$H=(\Lambda ,0)\left[\begin{array}{cc}\Omega & \tau \\ 0^T& 1\end{array}\right]=\Lambda (\Omega ,\tau )$

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

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

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

张正友标定法

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

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

$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\Lambda (\Omega ,\tau )\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\Lambda (w_1,w_2,w_3,\tau )\left[\begin{array}{c}X\\ Y\\ 0\\ 1\end{array}\right]=\Lambda (w_1,w_2,\tau )\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$

        用单应矩阵可以表示为：

$\lambda \left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=H\left[\begin{array}{c}X\\ Y\\ 1\end{array}\right]$

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

$H=s\Lambda ((w_1,w_2,\tau ))$

        通过平面间的单应可得：

$H=[h_1,h_2,h_3]=s\Lambda (w_1,w_2,\tau )$

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

$w_1=s{\Lambda }^{-1}{h}_1$
$w_2=s{\Lambda }^{-1}{h}_2$
$\tau =s{\Lambda }^{-1}{h}_2$

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

$w_1^T{w}_2=0$
$\Vert w_1\Vert =\Vert w_2\Vert =1)$

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

$h_1^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_2=0$
$h_1^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_1=h_2^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_2=1$

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

$B={\Lambda }^{-T}{\Lambda }^{-1}=\left[\begin{array}{ccc}{B}_{1}1& B_{12}& B_{13}\\ B_{21}& B_{22}& B_{23}\\ B_{31}& B_{32}& B_{33}\end{array}\right]=\left[\begin{array}{ccc}1/{\phi }_x^2& -\gamma /{\phi }_x^2{\phi }_y& ({\delta }_y\gamma -{\delta }_x{\phi }_y)/({\phi }_x^2{\phi }_y)\\ -\gamma /({\phi }_x^2{\phi }_y)& \gamma /({\phi }_x^2{\phi }_y^2)+1/{\phi }_y^2& -\gamma ({\delta }_y\gamma -{\delta }_x{\phi }_y)/({\phi }_x^2{\phi }_y^2)-{\delta }_y/{\phi }_y^2\\ ({\delta }_y\gamma -{\delta }_x{\phi }_y)/({\phi }_x^2{\phi }_y)& -\gamma ({\delta }_y\gamma -{\delta }_x{\phi }_y)/({\phi }_x^2{\phi }_y^2)-{\delta }_y/{\phi }_y^2& ({\delta }_y\gamma -{\delta }_x{\phi }_y{)}^2/({\phi }_x^2{\phi }_y^2)+{\delta }_y/{\phi }_y^2+1\end{array}\right]$

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

$b=\left[\begin{array}{cccccc}{B}_{1}1& B_{1}2& B_{2}2& B_{1}3& B_{2}3& B_{3}3\end{array}\right]$

        用$h_{i}j$表示矩阵H的第$i$行第$j$列，则约束方程中的$h_1^T$为：

$h_1^T=\left[\begin{array}{c}{h}_{i}1\\ h_{i}2\\ h_{i}3\end{array}\right]$

        因此：

$h_j^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_j=h_i{\Lambda }^{-T}{\Lambda }^{-1}{h}_j=v_i{j}^{T}b$

        其中：

$v_{i}j={\left[\begin{array}{cccccc}{h}_{i}1h_{j}1& h_{i}1h_{j}2+h_{i}2h_{j}1& h_{i}2h_{j}2& h_{i}3h_{j}1+h_{i}1h_{j}3& h_{i}3h_{j}2+h_{i}2h_{j}3& h_{i}3h_{j}3\end{array}\right]}^T$

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

$h_1^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_2=v_1{2}^{T}b=0$
$h_1^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_1=v_{11}b=h_2^T{\Lambda }^{-T}{\Lambda }^{-1}{h}_2=v_{22}b=1$

        写为矩阵形式则有：

$\left[\begin{array}{c}{v}_1{2}^T\\ (v_{11}-v_{2}2{)}^T\end{array}\right]b=0$

        或：

$Vb=0$
$V=\left[\begin{array}{c}{v}_1{2}^T\\ (v_{11}-v_{2}2{)}^T\end{array}\right]$

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

内参数求解

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

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

$A^{T}A=(V{S}^T{U}^T)US{V}^T=V{S}^T(U^{T}U)S{V}^T=V(S^{T}S)V^T$
$A{A}^T=US{V}^T(V{S}^T{U}^T)=U{S}^T(V^{T}V)S{U}^T=U(S{S}^T)U^T$
         $A{A}^T$是实对称阵，它的特征值为 $\lambda$，则S中的奇异值为： $\sigma =\surd \lambda$，V为 $A^{T}A$的特征向量。S中的奇异值是降序陈列的，通常情况下，可以用前几个奇异值近似的描述A而不损失过多的信息。这就是求解该方程的关键。据此，矩阵A可以用前r大的奇异值近似描述为：
$A_{(}m\times n)\approx U_{(}m\times r)\times S_{(}r\times r)\times V_{(}r\times n{)}^T(r<n)$

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

$\Vert Ax-b{\Vert }_2^2=\Vert U\Sigma V^{T}x-b\Vert =\Vert \Sigma V^{T}x-[U_n，\overline{U}{]}^{T}b\Vert =\Vert \Sigma V^{T}x-U_n^{T}b-{\overline{U}}^{T}b\Vert =\Vert \Sigma V^{T}x-U_n^{T}b+\Vert {\overline{U}}^{T}b\Vert \ge \Vert {\overline{U}}^{T}b\Vert \Vert$

        当且仅当$\Sigma V^{T}x-U_n^{T}b=0$时等号成立，所以：

$x=(\Sigma V^T{)}^{(}-1)U_n^{T}b=V{\Sigma }^{(}-1)U_n^{T}b$

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

${\delta }_x=(\gamma {\delta }_y)/{\phi }_y-(B_{13}{\phi }_x^2)/\lambda$
${\delta }_y=B_{12}{B}_{13}-B_{11}{B}_{23})/B_{11}{B}_{22}-B_{12}^2$
${\phi }_x=\surd (\lambda /B_{11})$
${\phi }_y=\surd ((\lambda B_{1}1)/(B_{11}{B}_{22}-B_{12}^2))$
$\gamma =(-B_{12}{\phi }_x^2{\phi }_y)/\lambda$
$\lambda =B_{33}-(B_{13}^2+{\delta }_y(B_{12}{B}_{13}-B_{11}{B}_{23}))/B_{11})$

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

$\overline{m}(\Lambda ,{\Omega }_i,{\tau }_i,M_{i}j)=\Lambda [\Omega ,\tau ]M_{i}j$

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

$f(m_{i}j)=\frac{1}{\surd 2\pi }{e}^{-(\overline{m}(\Lambda ,{\Omega }_i,{\tau }_i,M_{i}j)-m_{i}j{)}^2/{\sigma }^2}$

        紧接着，构造似然函数：

$L(\Lambda ,{\Omega }_i,{\tau }_i,M_{i}j)={\prod }_{i=1,j=1}^{n,m}f(m_{i}j)=\frac{1}{\surd 2\pi }{e}^{((-{\sum }_{i=1}^n{\sum }_{j=1}^m(\overline{m}(\Lambda ,{\Omega }_i,{\tau }_i,M_{i}j)-m_{i}j{)}^2)/{\sigma }^2)}$

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

${\sum }_{i=1}^n{\sum }_{j=1}^m\Vert \overline{m}(\Lambda ,{\Omega }_i,{\tau }_i,M_{i}j)-m_{i}j{\Vert }^2$

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

$0<\epsilon <1$、 ${\mu }_0>0$、 $0<{\gamma }_1<1<{\gamma }_2$、 $0<{\eta }_1\le {\eta }_2\le 1$

        本文中，迭代的初始点$x_0$即为上述利用SVD方法求解出的相机内参数值。
        Step2：若$\Vert J_k^T{F}_k\Vert \le \epsilon$，则结束，否则执行Step3；
        Step3：计算：

$d_k({\mu }_k)=-(J_k^T{J}_k+{\mu }_{k}I{)}^{(}-1)J_k^T{F}_k$

        Step4：计算：

${\rho }_k(d_k({\mu }_k),{\mu }_k)=(\varphi (x_k)-\varphi (x_k+d_k({\mu }_k)))/(\varphi (x_k)-{\psi }_k(d_k({\mu }_k),{\mu }_k))$

        Step5:

若 ${\rho }_k(d_k({\mu }_k),{\mu }_k)<{\eta }_1$，则取 ${\mu }_{(}k+1)={\gamma }_2{\mu }_k$；
若 ${\eta }_2>{\rho }_k(d_k({\mu }_k),{\mu }_k)\ge {\eta }_1$，则取 ${\mu }_{(}k+1)={\mu }_k$；
若 ${\rho }_k(d_k({\mu }_k),{\mu }_k)\ge {\eta }_2$，则取 ${\mu }_{(}k+1)={\gamma }_1{\mu }_k$；

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

摄像头畸变

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

1、径向畸变

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

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

$x^{\prime }=x\times (1+k_1{r}^2+k_2{r}^4)$
$y^{\prime }=y\times (1+k_1{r}^2+k_2{r}^4)$

2、切向畸变

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

$x^{\prime }=x+[2p_{1}xy+p_2(r^2+2x^2)]$
$y^{\prime }=y+[2p_{2}xy+p_1(r^2+2y^2)]$

        综上，摄像头的畸变可以用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]$
$\overline{y}=y+y[k_1(x^2+y^2)+k_2(x^2+y^2{)}^2]$

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

$\overline{\mu }={\mu }_0+{\phi }_x\overline{x}+\gamma \overline{y}$
$\overline{\upsilon }={\nu }_0+{\phi }_y\overline{y}$

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

$\overline{\mu }=\mu +(\mu -{\mu }_0)[k_1(x^2+y^2)+k_2(x^2+y^2{)}^2]$
$\overline{\nu }=\nu +(\nu -{\nu }_0)[k_1(x^2+y^2)+k_2(x^2+y^2{)}^2]$

        改用矩阵形来写为：

$\left[\begin{array}{cc}((\mu -{\mu }_0)(x^2+y^2)& (\mu -{\mu }_0)(x^2+y^2{)}^2\\ (\nu -{\nu }_0)(x^2+y^2)& (\nu -{\nu }_0)(x^2+y^2{)}^2{)}^T\end{array}\right]\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]=\left[\begin{array}{c}\overline{\mu }-\mu \\ \overline{v}-v\end{array}\right]$

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

$Dk=d$

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

$F(x)={\sum }_{i=1}^n{\sum }_{j=1}^m\Vert \overline{m}(\Lambda ,k_1,k_2,{\Omega }_i,{\tau }_i,M_{i}j)-m_{i}j{\Vert }^2$

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

Harris角点检测

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

$E(u,v)={\sum }_{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))/{\sigma }^2}$

        由泰勒级数，可得：

$I(x+u,y+v)=I(x,y)+I_{x}u+I_{y}v$

        其中$I_x=\delta I/\delta x$,$I_y=\delta I/\delta y$，将其代入$E(u,v)$，可得：

$E(u,v)={\sum }_{x,y}w(x,y)[I_{x}u+I_{y}v{]}^2={\sum }_{x,y}w(x,y)[u,v]\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]\left[\begin{array}{c}\mu \\ v\end{array}\right]$

令：

$M={\sum }_{x,y}w(x,y)\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]=\left[\begin{array}{cc}A& C\\ C& B\end{array}\right]$

其中：

$A=I_x\times w$
$B=I_y\times w$
$C=(I_x{I}_y)\times w$

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

$R=({\lambda }_1{\lambda }_2)-k({\lambda }_1+{\lambda }_2{)}^2=|M|-k\times t{r}^2(M)$

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

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

        综上，Harris角点检测算法的步骤总结如下：
        Step1：根据图像I计算梯度图像$I_x$,$I_y$，并计算$I_x^2,I_y^2,I_x{I}_y$；
        Step2：对$I_x^2$,$I_y^2$,$I_x{I}_y$进行高斯滤波处理；
        Stap3：对目标像素点构造自相关矩阵M，$M=[I_x^2,I_x{I}_y;I_x{I}_y,I_y^2]$；
        Step4：构造角点响应函数$R=|M|-k\times t{r}^2(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
        联系方式：18398621916@163.com（请勿使用其他联系方式，谢谢！）
        PS:再次请各位读者朋友注意，这里面很多东西涉及到我的毕设，写作辛苦，请勿滥用，转载请务必注明出处！