項目實戰——基於計算機視覺的物體位姿定位及機械臂抓取（單目標定）

項目實戰——基於計算機視覺的物體位姿定位及機械臂抓取（單目標定）

        請各位讀者朋友注意，這裏面很多東西涉及到我的畢設，寫作辛苦，請勿濫用，轉載請務必註明出處！
        單目標定主要分爲兩個部分，一是確定攝像機的內在參數，二是確定攝像頭畸變係數，消除畸變。在進行標定之前首先要建立攝像機模型，用數學語言描述三維世界中的點在攝像機中成像的過程。

全針孔攝像機模型的建立

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=(φ_x u)/w$
$y=(φ_y v)/w$

        （2）像素座標系的座標原點和圖像座標系的並不重合，這就需要轉移x、y的位置，因此增加偏移量參數：δ_x，δ_y。同時引入偏移參數γ用於控制投影位置x作爲真實世界中高度v的函數，原本的映射關係就變成了：

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

        這就是：攝像機的映射模型。
        另外考慮到攝像機外部因素，攝像機的位置並非總是位於世界座標系的原點，尤其是在考慮兩臺及以上攝像機的情形下。爲此，需要通過座標變換，在真實世界通過投影模型前，得出攝像機座標系中真實世界點$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]$

        可寫爲：$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$
$y=(φ_y (w_21 u+w_22 v+w_23 w+τ_y ))/(w_31 u+w_32 v+w_33 w+τ_z )+δ_y$

        該映射有兩套參數，其一是攝像機的內在參數：${φ_x,φ_y,γ,δ_x,δ_y }$，反映的是攝像機自身的性質，其二是攝像機的外在參數：${Ω,τ}$，反映的是攝像機在現實世界中的位置與方向。
於是，最終得到全投影模型的簡寫形式:

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

齊次座標與單應性變換

        觀察單目攝像機的投影模型不難發現：3D世界座標中的點，在圖像的投影上都要除以分母w，這就使得投影成爲非線性的，不方便研究。因此對2D 圖像點和3D世界點的表達形式做出修改，使得投影方程變爲線性的。
        將2D圖像點的座標轉化爲3D齊次座標x ̃：

$\overline{x}=λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]$

        將3D世界點的座標轉化爲4D齊次座標w^：

$\overline{w}=λ\left[\begin{matrix}u\\v\\w\\1\end{matrix}\right]$

        這樣在不考慮相機位姿的情況下，原本的投影方程就轉化爲：

$λ\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]$

        可寫爲：

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

        經過升維處理，原本的非線性映射關係就變成了線性的了。同時不難看出，等式右側是攝像機內在參數矩陣，它被儲存在內在矩陣Λ中：

$Λ=\left[\begin{matrix}φ_x&γ&δ_x\\0&φ_y&δ_y\\0&0&1\end{matrix}\right]$

        考慮到相機座標系和世界座標系並不重合，需要在投影方程的右邊乘以一個姿態變換的矩陣，即上述座標轉換矩陣。因此，真實世界的三維點投影到二維的成像平面的映射方程爲：

$\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]$

        可簡寫爲：

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

        爲了方便研究，這裏引入一個新的概念：單應性變換。它的定義爲：

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

不難看出，該變換實際上描述的是：真實物理世界中的點向所拍攝圖像中的像素點之間的映射，用單應性矩陣這種映射關係：

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

        至此，攝像機投影模型建立完畢，現在所有問題的矛盾都指向攝像機內參矩陣Λ的求解。

張正友標定法

        張正友于1998年提出了求解相機內參Λ的方法，它僅需要一個標定棋盤即可完成（如圖2.2-4所示）。該方法以其簡單、實用、高效的特性，成爲了目前單目相機標定的主流算法。

        張氏標定法的具體思路是：用於標定的棋盤是三維世界的一個平面$Π$，它在成像平面所成的像爲另一個平面$π$。由於標定棋盤的角點座標已知，圖像的角點可以通過角點識別算法求得，通過兩個平面的對應點的座標，就可以求解出單應矩陣H。從而求出攝像機的內參數，完成標定。下面具體說明其算法。
        設棋盤所在平面爲世界座標系下的$z=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]$

        用單應矩陣可以表示爲：

$λ\left[\begin{matrix}x\\y\\1\end{matrix}\right]=H\left[\begin{matrix}X\\Y\\1\end{matrix}\right]$

        聯立兩式可得（由於求出的H與實際上的H之間可能存在一定的比例誤差，這裏增加一個比例因子s）：

$H=sΛ((w_1,w_2,τ))$

        通過平面間的單應可得：

$H=[h_1,h_2,h_3 ]=sΛ(w_1,w_2,τ)$

        將矩陣Ω中的列向量用單應矩陣的列向量表示:

$w_1=sΛ^{-1}h_1$
$w_2=sΛ^{-1}h_2$
$τ=sΛ^{-1}h_2$

        矩陣Ω是旋轉矩陣，本質上它也是正交矩陣，根據正交矩陣的性質：列向量的模爲1，且任意兩個列向量的向量內積均爲0。則有:

$w_1^T w_2=0$
$‖w_1 ‖=‖w_2 ‖=1)$

        將等式用單應矩陣的列向量表示，可以得到在一幅標定板圖像（即：一個單應矩陣）中的兩組約束條件。

$h_1^T Λ^{-T} Λ^{-1} h_2=0$
$h_1^T Λ^{-T} Λ^{-1} h_1=h_2^T Λ^{-T} Λ^{-1} h_2=1$

觀察等式，注意到兩個等式中都有$Λ^{-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是一個3×3的對稱陣，因此只有六個未知量。將六個未知量寫成向量形式：

$b=\left[\begin{matrix}B_11&B_12&B_22&B_13&B_23&B_33\end{matrix}\right]$

        用$h_ij$表示矩陣H的第$i$行第$j$列，則約束方程中的$h_1^T$爲：

$h_1^T=\left[\begin{matrix}h_i1\\h_i2\\h_i3\end{matrix}\right]$

        因此：

$h_j^T Λ^{-T}Λ^{-1} h_j=h_i Λ^{-T} Λ^{-1} h_j=v_ij^T b$

        其中：

$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$

        現在再來看之前的兩個約束公式，可轉換爲：

$h_1^T Λ^{-T} Λ^{-1} h_2=v_12^T b=0$
$h_1^T Λ^{-T}Λ^{-1}h_1=v_{11} b=h_2^T Λ^{-T} Λ^{-1}h_2=v_{22} b=1$

        寫爲矩陣形式則有：

$\left[\begin{matrix}v_12^T\\(v_{11}-{v_22})^T \end{matrix}\right]b=0$

        或：

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

        對於上述方程，V是一個2n×6的矩陣，一共有六個維度。因此對於一幅標定板圖像，可以得到兩個方程，而未知量有：$φ_x$、$φ_y$ 、$δ_x$、 $δ_y$ 、 $γ$。因此，使得方程有解的充要條件是$n≥3$，即：需要拍攝三張以上的棋盤圖像。張正友標定法，就是通過獲取標定圖像的棋盤信息，求解方程$Vb=0$得到矩陣$Λ$，完成單目標定。下面對該方程進行求解。

內參數求解

        對於m個線性無關的方程組求解n個未知數，共有三種情況：
        1、$m>n$，約束的數目大於未知數的數目，稱爲超定問題。
        2、$m=n$，約束的數目等於未知數的數目，方程的解唯一。
        3、$m<n$，約束的數目小於未知數的數目，稱爲欠定問題。
        對於求解$Vb=0$的問題，當$n≥3$時，共有六個以上的方程，而需要求解的未知數共五個，因此$m>n$，該求解問題爲超定問題。也就是說，該方程的解是不存在的，此時求方程解的問題就轉換爲求解最小二乘解的問題，即：$min‖Vb-0‖$。
        本文采用奇異值（SVD）分解法求解最小二乘問題。
        SVD的定義：對於任意一個m×n維的矩陣A，都可以分解爲：$A=USV^T$。 SVD分解法，可以通過下圖形象地說明：

        V是一個$n×n$的正交陣，它的列向量是$A^T A$的特徵向量。
        U是一個$m×m$的正交陣，它的列向量是$AA^T$的特徵向量。
        S（可寫爲$Σ_r$）是r個沿對角線降序排列的奇異值組成的方陣。
        $A^T$A和$AA^T$用SVD分解可表示爲:

$A^T A=(VS^T U^T )USV^T=VS^T (U^T U)SV^T=V(S^T S) V^T$
$AA^T=USV^T (VS^T U^T )=US^T (V^T V)SU^T=U(SS^T ) U^T$
        $AA^T$是實對稱陣，它的特徵值爲$λ$，則S中的奇異值爲：$σ=√λ$，V爲$A^T A$的特徵向量。S中的奇異值是降序陳列的，通常情況下，可以用前幾個奇異值近似的描述A而不損失過多的信息。這就是求解該方程的關鍵。據此，矩陣A可以用前r大的奇異值近似描述爲：
$A_(m×n)≈U_(m×r)×S_(r×r)×V_(r×n)^T (r<n)$

        下面，利用奇異值（SVD）分解法求解一開始的問題。
        設$A∈R^(m×n)$列滿秩， A=UΣV^T是對A的奇異值分解。令$U_n$爲U的前n列矩陣，即：$U=[U_n，\overline 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‖‖$

        當且僅當$ΣV^T x-U_n^T b=0$時等號成立，所以：

$x=(ΣV^T )^(-1) U_n^T b=VΣ^(-1) U_n^T b$

        即爲最小二乘問題的解。
        因此，$Vb=0$的最小二乘解爲：$V^T V$的$λ_min$對應的特徵向量。採用SVD法，最終求得攝像機的內參數爲：

$δ_x=(γδ_y)/φ_y -(B_{13} φ_x^2)/λ$
$δ_y=B_{12} B_{13}-B_{11} B_{23})/B_{11} B_{22}-B_{12}^2$
$φ_x=√(λ/B_{11} )$
$φ_y=√((λB_11)/(B_{11} B_{22}-B_{12}^2 ))$
$γ=(-B_{12} φ_x^2 φ_y)/λ$
$λ=B_{33}-(B_{13}^2+δ_y (B_{12}B_{13} -B_{11} B_{23} ))/B_{11} )$

        上述通過最小二乘法得到的內參數的解，是貼近攝像機真實內參數的（由於求解精確解是不可能的，只能轉而求解近似解）。爲了進一步增加標定精度，本文采用最大似然估計法對上述求解結果進行優化。
        假設一臺攝像機拍攝得到了n幅的不同位置的標定板圖像，每幅圖像上有m個像點，用$M_ij$表示第i幅圖像上的第j個像點對應世界座標系中標定板的三維角點。則像點可表示爲：

$\overline m(Λ,Ω_i,τ_i,M_ij)=Λ[Ω,τ]M_ij$

        圖像中對應的像點m_ij符合正態分佈，它的概率密度函數可表示爲：

$f(m_ij )=\frac{1}{√2π} e^{-(\overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij )^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=1}^n∑_{j=1}^m ‖ \overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij ‖^2$

        這是一個非線性公式，求解非線性函數最小化，目前主流算法有Newton method、Gauss Newton iteration、Steepest Descent。Levenberg和Marquardt結合Gauss-Newton algorithm以及Steepest Descent的優點，並對兩者的不足之處作改善，提出了Levenberg–Marquardt 方法。該方法通過迭代，能最大程度上避免局部最小值的出現。在冗餘參數較多時，優化效果更爲出色。
        Levenberg–Marquardt algorithm的具體算法爲:
        目標：$minF(x)$,本文中$F(x)=∑_{i=1}^n∑_{j=1}^m‖\overline{m}(Λ,Ω_i,τ_i,M_ij )-m_ij ‖^2$
        計算步驟：
        Step1：選取初始點$x_0$, 令$k=0$並選取參數$ε$、$μ_0$ 、$γ_1$ 、$γ_2$ 、$η_1$ 、$η_2$使得：

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

        本文中，迭代的初始點$x_0$即爲上述利用SVD方法求解出的相機內參數值。
        Step2：若$‖J_k^T F_k ‖≤ε$，則結束，否則執行Step3；
        Step3：計算：

$d_k (μ_k )=-(J_k^T J_k+μ_k I)^(-1) J_k^T F_k$

        Step4：計算：

$ρ_k (d_k (μ_k ),μ_k )=(ϕ(x_k )-ϕ(x_k+d_k (μ_k )))/(ϕ(x_k )-ψ_k (d_k (μ_k ),μ_k ) )$

        Step5:

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

        Step6：取$x_(k+1)=x_k+d_k (μ_k )$，令$k=k+1$，返回Step2.
        對相關公式的說明：
        $J(x)$是$F(x)$的雅可比（Jacobian）矩陣。
        $μ_k$是一個正參數，其目的是：防止當$J_k^T J_k$接近奇異時$d_k$過大。
        $ϕ(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 )$
$y'=y×(1+k_1 r^2+k_2 r^4 )$

2、切向畸變

        切向畸變通常是由於在機械裝配的過程中，透鏡與成像平面不平行導致的。同樣地，它可以用p1、p2這兩個參數描述。可根據公式調整：

$x'=x+[2p_1 xy+p_2 (r^2+2x^2 )]$
$y'=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 μ=μ_0+φ_x \overline x+γ\overline y$
$\overline υ=ν_0+φ_y \overline y$

        其中：
         $(μ,υ)$：無畸變的像素座標點；
         $(\overline μ,\overline υ)$：畸變後的像素座標點；
         $(μ_0,υ_0 )$：攝像機的主心位置。
        將二者結合起來，就可以得到像素座標系下的徑向畸變公式，如果不考慮γ的影響，令$γ=0$，則:

$\overlineμ=μ+(μ-μ_0)[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^2 ]$
$\overlineν=ν+(ν-ν_0)[k_1 (x^2+y^2 )+k_2 (x^2+y^2 )^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]$

        上述公式是通過一張標定圖像上的一個棋盤角點取得的，設攝像機共拍攝n張棋盤圖，每幅圖上有m個棋盤角點。因此，最終可以得到2mn個等式，寫成矩陣形式：

$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$

        用此方法得到的畸變係數$k_1$,$k_2$，可以對圖像進行去畸變處理，使得攝像機成的像與真實世界的一致。

Harris角點檢測

        在張氏標定法的具體思路中，對於標定棋盤圖像角點座標的獲取，是通過Harris角點提取算法獲得的，現在對這一算法方法進行詳細說明。
        該算法的主要思路是：令一個$n×n$的窗口在圖像上移動，通過比較臨近像素點的灰度差，判斷灰度是否發生較大變化，從而判斷是否爲角點、邊緣、平滑區域。
        定義$E(u,v)$爲窗口W在圖像上移動$(u,v)$個像素的灰度變換：

$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 }$

        由泰勒級數，可得：

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

        其中$I_x=δI/δx$,$I_y=δI/δy$，將其代入$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]$

令：

$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]$

其中：

$A=I_x×w$
$B=I_y×w$
$C=(I_x I_y )×w$

        矩陣M是一個自相關矩陣，因此該矩陣爲海森矩陣，其特徵值反映了自相關函數E(u,v)的曲率。根據這兩個特徵值（記爲：λ_1,λ_2）判斷區域特徵：
        $λ_1$,$λ_2$都比較小時，灰度相差不大，表示該區域爲平坦區域；
        $λ_1$,$λ_2$有一較小，另一較大時，灰度相差明顯，表示該區域爲邊緣區域；
        $λ_1$,$λ_2$都比較大時，灰度相差顯著，表示該區域爲角點區域。
        由於這裏只需要判斷$λ_1$,$λ_2$的相對大小，並不需要知道它們具體的值，可以通過角點響應函數進行判斷：

$R=(λ_1 λ_2 )-k(λ_1 +λ_2 )^2=|M|-k×tr^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×tr^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
        聯繫方式：[email protected]（請勿使用其他聯繫方式，謝謝！）
        PS:再次請各位讀者朋友注意，這裏面很多東西涉及到我的畢設，寫作辛苦，請勿濫用，轉載請務必註明出處！