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

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

        請各位讀者朋友注意，這裏面很多東西涉及到我的畢設，寫作辛苦，請勿濫用，轉載請務必註明出處！
        雙目標定的目的是計算兩個攝像機的相對位姿信息，並進行矯正，使得兩臺攝像機處於完全平行的狀態。下面建立雙攝像機模型，將相機相對位姿用數學表達式表達出來，並進行求解。

對極幾何

        對極幾何是雙視圖幾何學理論的一部分，通常情況下，同一場景的兩幅圖像對應點之間存在着特殊的幾何關係。這種關係僅取決於攝像機的內參數和兩攝像機之間的相對位姿。
        1、極線約束
        對於單一的攝影機觀測3D點w的情況，如圖所示，w必定位於一條穿過光心和攝像機平面中$x_1$的光線上。

        但是，並不能確定該點位於這條射線上的具體位置（圖紙標記了四個可能的位置）。現在考慮觀測同一個3D點的第二個攝像機，從第一個攝像機可得，該點必定位於3D空間中的一條特定光線上，因此第二幅圖像中該點的投影位置$x_2$必定唯一在第二幅圖像中這條光線投影上的某個位置上。真實物理世界中的光線在二維平面中的投影稱爲極線。
        綜上可以得出這樣一個結論：對於第一幅圖像中的任意點，它在第二幅圖象中的對應點一定會在一條線上。這就是極線約束。該受約束的特定極線依賴於攝像機的內參數和外參數。
極線約束具有兩個重要的實際意義：
        1) 在已知攝像機焦距、偏移量和相機間相對位姿的情況下，對於左視圖中的二維像素點，只需在右攝像機拍攝圖像中的極線進行一維搜索，即可得到該點在第二幅圖像上的對應點位置。
        2) 對應點的約束是攝像機內在參數和外在參數的函數，已知攝像機內在參數的情況下，可利用對應點的觀測模式來確定攝像機的外在參數，進而確定兩臺攝像機的相對位姿。
        2、極點
        在極線約束的基礎上從另一個方面考量，第一幅圖像中的每一個點都是物理世界中的一條光線的投影，而這些光線同時又在第二幅圖像上投影形成極線。由於所有光線都聚集到攝像機1的光心，那麼，所有光線在第二幅圖像中形成的極線也將聚集於一點。該點即爲攝像機1的光心在攝像機2中的對應點，也稱極點。

        當然，極點並不一定在觀測的圖像內部，也有可能在圖像之外的某一點上，下圖描述了兩種極端的情況：

        當兩臺攝像機位於一條直線上時，極點位於圖像中心，且極線呈放射狀分佈；當兩臺攝像機都位於同一方向且垂直於光軸，此時極線都是互相平行的，而極點位於無窮遠處。
        以上即爲對極幾何的主要內容，它對於計算攝像機相對位姿信息（求解攝像機外參數），進行立體匹配（尋找對應點）具有重要的作用。

基礎矩陣與實矩陣

        爲了研究方便，在研究雙攝像機時，假定第一個攝像機爲世界座標的座標中心，因此第一個攝像機的外在參數爲：{I,0}。第二個攝像機處於任意位置。這樣，在齊次座標中，一個3D點投影到兩個攝像機上，可表示爲：

$λ_1 \overline x_1=Λ_1 [1,0] \overline w=Λ_1 \overline w$
$λ_2 x_2=Λ_2 [Ω,τ] \overline w=Λ_2 Ω\overline ω+Λ_2 τ$

        將上面兩個公式合併，可得：

$λ_2 \overline x_2=λ_1 Λ_2 ΩΛ_1^{-1} \overline x_1+Λ_2 τ$

        使用平移向量τ乘以兩邊求它們的向量外積，可將$Λ_2 τ$這一項約去：

$λ_2 τ×\overline x_2=λ_1 τ×(Λ_2 ΩΛ_1^{-1} \overline x_1 )$

        使用$\overline x_2$乘以兩邊求它們的向量內積，可將等式左邊約去：

$x_2^T Λ_2^{-T} τ×ΩΛ_1^{-1} \overline x_1=0$

        其中外積算子$τ×$可以表達爲一個秩爲2的非對稱3x3的矩陣$τ_×$：

$τ_×=\left[\begin{matrix}0&-τ_z&τ_y\\-τ_z&0&-τ_x\\-τ_y&τ_x&0\end{matrix}\right]$

        因此，令$F=Λ_2^{-T} τ×ΩΛ_1^{-1}$，則：$x_2^T F\overline x_1=0$可以看作是攝像機2中的點向攝像機1中點的對應。
        令$E=τ×Ω$，則：$F=Λ_2^{-T} EΛ_1^{-1}$。其中$Λ_1$，$Λ_2$是兩臺攝像機的內參數，E實際上就是兩攝像機間的位姿變換矩陣。稱F爲基礎矩陣，E爲實矩陣（本徵矩陣）。因此，通過對兩部攝像機的標定，得到$Λ_1$，$Λ_2$後，只要求解出基礎矩陣，就能知道實矩陣，也就找到了兩個相機位姿對應信息，即：

$E=Λ_2^T FΛ_1$

實矩陣與對極幾何

        由$E=τ×Ω$，可知實矩陣E時一個3×3的矩陣，儲存的是兩個攝像機間的幾何關係信息。由於外積算子$τ_×$的秩爲2，因此$det[E]=0$。同時實矩陣只依賴於兩個攝像機之間的旋轉和平移，每個攝像機由 3個參數，所以一般認爲實矩陣具有6個自由度（也可以認爲：第一個攝像機位於世界座標的座標中心，自由度可略去不計，第二個攝像機在空間中具有位置和方向的六個自由度，因此加起來共6個自由度）。
        由於矩陣比未知量具有較少的自由度，因此矩陣的3×3共9個項必須服從一組代數約束，可以簡單地表示爲：

$2EE^T E-trace[EE^T ]E=0 （2-71）$

        矩陣獲取極線信息。極線上的點可以表示爲：

$ax+by+c=0$

        寫成矩陣形式可表示爲：

$\left[\begin{matrix}a&b&c\end{matrix}\right]\left[\begin{matrix}x\\y\\1\end{matrix}\right]=0$
$l\overline x=0$

        根據實矩陣的定義，不難有：

$\overline x_2^T E\overline x_1=0$

        其中$\overline x_2^TE$ 是一個1×3的向量，所以該關係可表示爲$l_1 \overline x_1=0$。不難推斷：
$l_1=\overline x_2^T E$是點$x_2$在圖像1中對應的極線。同樣地，也可以得出$x_1$在圖像2中對應的極線：

$l_1=\overline x_2^T E$
$l_2=\overline x_1^T E^T$

        即爲實矩陣與極線的對應關係。當然極點信息也可以通過實矩陣獲得，設左右兩個攝像機的兩個極點用$\overline e_1$和$\overline e_2$表示。第一幅圖像中每條極線均通過極點$\overline e_1$，因此有：

$l_1 \overline e_1 =\overline x_2^T E\overline e_1=0$

        根據矩陣知識，$\overline e_1$一定位於E的右零空間，同理$\overline e_2$也必定位於E的左零空間，即：

$\overline e_1=null[E]$
$\overline e_2=null[E^T ]$

        可以對E進行SVD分解，$E=ULV^T$，令$\overline e_1$等於V的最後一列，$\overline e_2$爲U的最後一行，以此來求解極點信息。
        總結一下，實矩陣與極點、極線的關係可以用以下公式表示:

$l_1=\overline x_2^T E$
$l_2=\overline x_1^T E^T$
$\overline e_1=null[E]$
$\overline e_2=null[E^T ] )$

矩陣求解（8點算法）

        由雙攝像機模型，得到了攝像機2中的點向攝像機1中點的對應關係：$x_2^T F\overline x_1=0$，由於$x_2^T$爲1x3的矩陣，$\overline x_1$爲3x1的矩陣，因此F爲3x3的矩陣。將表達式展開寫，在齊次座標中，圖像1中第i個點與圖像2中第i個點的對應關係爲：

$\left[\begin{matrix}x_{i2}&y_{i2}&1\end{matrix}\right]\left[\begin{matrix}f_{11}&f_{12}&f_{13}\\f_{21}&f_{22}&f_{23}\\f_{31}&f_{32}&f_{33}\end{matrix}\right]\left[\begin{matrix}x_i1\\y_i1\\1\end{matrix}\right]=0$

        繼續展開，可寫爲：

$x_{i2} x_{i1 } f_{11}+x_{i2} y_{i1} f_{12}+x_{i2} f_{13}+y_{i2} x_{i1} f_{21}+y_{i2} y_{i1} f_{22}+y_{i2} f_{23} x_{i1} f_{31}+y_{i1} f_{32}+f_{33}=0$

        將上式用向量內積的形式表達：

$[x_{i2} x_{i1},x_{i2} y_{i1},x_{i2},y_{i2} x_{i1},y_{i2} y_{i1},y_{i2},x_{i1},y_{i1},1]f=0$

        其中：$f=[f_{11},f_{12},f_{13},f_{21},f_{22},f_{23},f_{31},f_{32} f_{33} ]$即爲基礎矩陣F的矢量形式。這樣就爲F中的元素增加了一個線性約束，因此，對於I個已知的匹配點，可以得到：

$\left[\begin{matrix}x_{12}& x_{11}& x_{12}& y_{11} & x_{12} & y_{12}& x_{11} & y_{12}& y_{11} & y_{12}& x_{11} & y_{11} & 1\\x_{22}& x_{21} & x_{22}& y_{21}& x_{22} & y_{22}& x_{21}& y_{22} &y_{21} & y_{22 }& x_{21}& y_{21} & 1\\.&.&.&.&.&.&.&.&.&.&.&.&.\\.&.&.&.&.&.&.&.&.&.&.&.&.\\.&.&.&.&.&.&.&.&.&.&.&.&.\\x_{I2} &x_{I1} & x_{I2}& y_{I1} & x_{I2} & y_{I2} &x_{I1} & y_{I2} &y_{I1} & y_{I2} & x_{I1} & y_{I1}& 1\end{matrix}\right]f=0$
$|f|=1$

        需要說明的是，觀察上述公式，F可以看作是一個常數，因此在其中增加了範數爲1的約束。對於f中的9個參數，至少需要$I=8$對點。以上即爲求解攝像機對應矩陣的方法，由於用到了8個對應點信息，所以該算法也稱8點算法。
        該算法求解過程較爲簡單，但以上考慮的因素只是理想中的情況，在實際情況下會存在各種各樣的誤差，如：數據噪聲、匹配點匹配錯誤、數據的誤差，只要有任意一點誤差，就足以對計算結果產生嚴重影響，進而影響到整個雙目測距結果。換句話說，該算法的魯棒性很差，因此在8點算法的步驟上提出了很多修正算法，以減少誤差。
        1、圖像座標歸一化處理
設八點算法等式左邊的係數矩陣爲A，以上公式可表示爲：

$Af=0$

        造成八點算法不穩定的原因主要在於係數矩陣A， A直接使用的是原圖像的像素座標，沒有經過任何處理，而每個像素點座標分量的數量級相差太大，這就造成了矩陣A的不穩定。因此，在帶入矩陣進行計算之前，首先要對像素座標進行歸一化處理，這樣就可以消除數量級不同引起的巨大誤差，提高矩陣A的穩定性。處理方法主要是對原始圖像座標進行同向性變換。
        歸一化處理主要分爲兩步：
        1、變換選定像素點的位移，使得像素點集與原圖像的原點重合；
        2、對圖像點進行放縮變換，使他們到原點的平均距離爲√2。

        上圖形象地描述了歸一化處理的過程：左側爲原始圖像點，右側爲處理後的點，其中H爲歸一化變換的矩陣，可表示爲：

$H=\left[\begin{matrix}k_m&0&-k_m u_0\\0&k_m&-k_m v_0\\0&0&1\end{matrix}\right]$
$u_0=1/N ∑_{i=1}^Nu_i$
$v_0=1/N ∑_{i=1}^Nv_i$

        $σ_m$表示尺度，其表達式爲：

$σ_m=√((∑_(i=1)^N[(u_i-u_0 )^2+(v_i-v_0 )^2 ] )/N)$
$k_m=√2/σ_m$

        至此，改進後的八點算法步驟如下:
        (1)、對兩幅圖像分別進行歸一化處理：$\overline p_l =Hp_i$，$\overline p_l' =H'p_i'$。
        (2)、根據八點算法，重新計算基礎矩陣$\overline F$。
        (3)、進行反歸一化根據變換$F=H_2^T \overline FH_1$，還原基礎矩陣F。
        2、最小二乘法處理
        這裏採用的最小二乘法處理類似於單目標定中對攝像機內參的求解。雖然八點算法要求的時至少有八個對應點，但在實際計算的過程中，匹配點的數量遠超八個，顯然，此時$Af=0$成爲一個超定方程。根據單目標定中的經驗，此時方程已然無解，轉而求解方程的最小二乘解。
        這樣由I個已知匹配點，組成矩陣$A_{I×9}$，採用最小二乘法求解的約束目標可表示爲：

$min^{‖f‖=1} ‖Af‖^2$

        其中A的每一行表示一個匹配點對應的係數，$f{9x1}$爲基礎矩陣F的求解向量，根據範數的定義：

$‖Af‖^2=(Af)^T (Af)=f^T (A^T A)f$

        令$M=A^T A$，M是一個9x9的向量，根據最小二乘求解的結論，採用奇異值分解法，分解矩陣A：

$A=USV^T$

        最小二乘解就是$V^T$的第九個列向量，即:$f=V^T$。
        另外還可以根據基礎矩陣F的一個重要性質對最小二乘解進行優化，即：
$Rank(F)=2$
        由於噪聲等因素的影響，通常情況下$Rank(F)>2$，因此這裏選取前兩個特徵值在信息損失最小的情況下近似代替：

$F=SVD=S\left[\begin{matrix}v_1&0&0\\0&v_2&0\\0&0&v_3 \end{matrix}\right]D$

        令$v_3=0$，即

：
$F=SVD=S\left[\begin{matrix}v_1&0&0\\0&v_2&0\\0&0&0\end{matrix}\right]D$

        上述求得的F即爲最終得到的基礎矩陣。
        3、隨機採樣一致性（RANdom SAmple Consensus，RANSAC）
        通過兩個攝像機的圖像估計基礎矩陣，唯一的輸入來源就是匹配好的對應的點。在實際計算時，匹配點一定會存在誤差，這種誤差一般有以下兩種情況:
        (1)測量點位置不精確引起的系統誤差，這種誤差服從高斯分佈。
        (2)匹配點錯誤（錯誤的匹配點也稱外點）引起的誤差，這種誤差不服從高斯分佈。
        在計算的過程中，錯誤的匹配點能夠造成極大的誤差。對於外點，即使只有一組，也會造成很大的影響。因此，需要通過一種方法剔除外點，篩選出正確的匹配點（也稱內點）。這種方法就是隨機採樣一致性（RANSAC）。
        該方法適用於外點較多時的情況（當外點較少時，可以採用最小二乘法來確定）。它的基本假設主要有兩條：
        (1)數據中包含有內點，且內點數據符合某種數學分佈；
        (2)外點並不符合內點的數學分佈，一般是無規律的，其產生原因主要有：數據噪聲、計算錯誤、測量錯誤等；
        下面給出RANSAC的算法：
        Step1：假設共有N個數據組成的集合P，集合P包含了內點和外點，並至少可以通過n個點擬合出模型；
        Step2：從集合P中隨機選取n個點構成集合p；
        Step3：根據集合p估計出數學模型M；
        Step4：對剩餘N-n個點分別判斷是否適用模型M，適用的爲內點，否則爲外點，並統計內點的個數m_i。
        Step5：重複Step2~Step4步驟k次，並選取m_i最大（即:內點最多）的情況作爲最終結果。
        該算法的核心是對於k值的選取。k值過小，則會對內點、外點估計錯誤；k值過大，則會增加計算工作量。實際上，可以通過理論計算來推斷k值。Step2每次從集合P中選取n個點，假設選取到內點的概率爲ϖ，則:
$ϖ=內點數目/數據集數目$
        那麼所有的選取點均爲內點的概率爲$ϖ^n$，所有的選取點均不是內點的概率爲$1-ϖ^n$，${1-ϖ^n}^k$是k次選取均沒有選到內點的概率。設p爲k次迭代中至少有一次選擇的點均爲內點的概率，則1-p表示k次迭代均沒有選到內點。因此數值上等於${1-ϖ^n}^k$，即：

$1-p={1-ϖ^n }^k$

        兩邊取對數則有：

$k=log⁡(1-p)/log⁡（1-ϖ^n)$

        一般取p≥95%，由此可以選取k值。另外需要注意的是：以上計算的一個重要的前提是，n個點的選取是獨立隨機的，也就是說，被選到的點在後續迭代過程中仍有可能被選到。而由此估計出的k值一般認爲是選取不重複點的最大迭代次數。
        爲了計算出更可靠的參數，k值計算出後一般會疊加一個標準偏差，k的標準偏差定義爲：

$SD(k)=√(1-ω^n )/ω^n$

        除了k值的選取，還有一點需要考慮：用於判定是否符合模型M的判定依據。這裏用點到另一幅圖像上匹配點與預測極線之間的平方距離來表示。對於二維點x=[x,y]^T，與極線l=[a,b,c]，定義它們的距離爲：

$dist[x,l]=(ax+by+c)/√(a^2+b^2 )$

        綜上，利用RANSAC方法估算基礎矩陣的步驟爲:
        Step1：從匹配點中選取8個點，計算基礎矩陣$F_i$;
        Step2：計算其餘點到極線的距離$dist[x,l]$，如果$dist[x,l]<d$（d是設定好的閾值），則爲內點，否則爲外點。並統計外點個數$m_i$；
        Step3：迭代k次，直到得到內點個數大於95%時即停止。此時選取$m_i$的最大值的情況作爲最終結果。
        綜合以上1、2、3點，總結一下求解兩個攝像機對應矩陣的步驟。
        Step1：對兩顆攝像頭分別進行單目標定，求得相機的內在參數$Λ_1$，$Λ_2$；
        Step2：通過半塊全局匹配算法（後續會詳細說明）取得兩幅對應圖像中的匹配點；
        Step3：在Step2的匹配點中隨機選取八個點，進行歸一化處理；
        Step4：根據匹配點確定係數矩陣A；
        Step5：對A進行奇異值分解：$A=USV^T$，則f的解爲：$f=V^T$；
        Step6：對求得的矩陣$\overline F$，進行SVD分解：$\overline F=S×diag(v_1 v_2 v_3 ) V^T$，並令$v_3=0$得到基礎矩陣的估計$F_i=S×diag(v_1 v_2 0) V^T$
        Step7：分別計算其他匹配點到極線的距離，如果$dist[x,l]<d$則爲外點，否則爲內點。並統計外點個數$m_i$；
        Step8：返回Step3並執行Step3~7，迭代k次，直到得到內點個數大於95%時即停止。此時選取m_i的最大值的基礎矩陣值F ̅作爲最終結果。
        Step9：根據公式$E=Λ_2^T FΛ_1$，代入$Λ_1$，$Λ_2$，F得到實矩陣，即：兩臺相機的轉換矩陣。

立體矯正

        通過計算出本徵矩陣，並得到攝像機的相機轉換矩陣後，根據2.1的基本原理，通過兩個攝像機計算視深的前提是兩個攝像機必須保證絕對的平行。因此下一步任務就是：使得兩個攝像機的光軸完全平行，將攝像機所拍攝圖像轉換成完全平行攝像機所拍攝的圖像。這一步也稱爲立體校正。
        在上圖中，由於極線是相互平行的，因此三維世界的點在左右像面成的像高度是一致的。後面在進行立體匹配時，只需要在同一行上進行搜索即可，這樣匹配效率就會大大提高。
本文采用Bouguet算法對圖像進行立體校正，該算法的效果是：使左右視圖中每幅圖像的重投影畸變最小化，同時使視圖的公共視野最大化。主要思路是：將旋轉矩陣R在兩個攝像機之間分爲兩半（如果單純旋轉右側攝像機與左側平行，或者旋轉左側攝像機與右側平行，那麼它們的光軸方向就會發生很大的偏移，會使圖像重投影畸變較大，因此折中的方法就是兩側都旋轉一半，這樣就可以最小化圖像重投影畸變）。這裏分別將左右攝像機的合成旋轉矩陣稱爲$r_l$,$r_r$。將每個攝像機旋轉二分之一，則它們的光軸最終平行於原始光軸所指的矢量和。需要注意的是，這樣做會將兩個攝像機設置爲共面而非行對準。
        定義矩陣$R_rect$爲左攝像機的變換矩陣，目的是將其極線變爲水平，且極點移至無窮遠處。爲了計算該矩陣，設圖像主點$(c_x,c_y )$爲原極點方向，則歸一化後的極點方向爲兩臺攝像機投影中心之間的平移矢量方向，這裏構造一個由極點$(e_1 ) ⃗$方向開始的旋轉矩陣：

$(e_1 ) ⃗=T ⃗/‖T ⃗ ‖ =\left[\begin{matrix}(T_x&T_y&T_z \end{matrix}\right]^T/‖T ⃗ ‖$

        下一個矢量$e_2$必須和$e_1$正交，否則不受約束。對於$e_2$，選擇一個正交於主光線的方向（趨於沿像平面）是一個不錯的選擇。通過使用$e_1$與主光線方向的叉乘乘積來實現，然後歸一化即可獲得另一個單位矢量。

$(e_2 ) ⃗=1/√(T_x^2+T_y^2 ) ((-T_yT_x0))^T$

        由於第三個矢量方向$e_3$與$e_1$和$e_2$垂直，因此：

$(e_3 ) =(e_1 ) ×(e_2 )$

        由此可以得到矩陣$R_rect$爲：

$R_rect=\left[\begin{matrix}(e_1 ) ^T\\(e_2 ) ^T\\(e_3)^T\end{matrix}\right]$

        該矩陣將左側攝像機圍繞投影中心旋轉，左右攝像機通過R_rect與合成旋轉矩陣的乘積實現對準：

$R_l=R_{rect}∙r_l$
$R_r=R_{rect}∙r_r$

        同樣，可以計算標定的左右攝像機矩陣$Λ_(rect,l)$與$Λ_(rect,r)$，且它們返回與投影矩陣$p_l'$與$p_r'$結合:

$p_l=Λ_(rect,l)∙p_l'=\left[\begin{matrix}φ_x&γ&δ_x\\0&φ_y&δ_y\\0&0&1\end{matrix}\right]\left[\begin{matrix}1&0&0&0\\0&1&0&0\\0&0&1&0\end{matrix}\right]$

$p_l=Λ_(rect,r)∙p_l'=\left[\begin{matrix}φ_x&γ&δ_x\\0&φ_y&δ_y\\0&0&1\end{matrix}\right]\left[\begin{matrix}1&0&0&T_x\\0&1&0&0\\0&0&1&0\end{matrix}\right]$

        投影矩陣通過以下步驟將單應座標系中的一個三維點轉換到一個二維單應座標系：

$P∙\left[\begin{matrix}X\\ Y \\Z\\ 1\end{matrix}\right]=\left[\begin{matrix}x\\y\\z\end{matrix}\right]$

        另外可以根據$(x,y)=(x⁄w,y∕w)$來計算圖像座標。當然也可以將圖像中的點還原到真實物理世界中，重投影矩陣爲：

$Q=\left[\begin{matrix}1&0&0&-δ_x\\0&1&0&-δ_y\\0&0&1&f\\0&0&-1/T_x &(δ_x-δ_x')/T_x \end{matrix}\right]$

        矩陣Q中的參數來源於左攝像機，但$δ_x'$是右側視圖中x座標軸主點，如果兩個相機完全平行，則$δ_x'=δ_x$，即$Q(4,4)=0$。因此，重投影關係可表示爲：

$Q∙\left[\begin{matrix}x\\y\\d\\1\end{matrix}\right]=\left[\begin{matrix}X\\Y\\Z\\W\end{matrix}\right]$

        式中，d表示偏差，三維世界座標即爲：$\left[\begin{matrix}X/W &Y/W &Z/W\end{matrix}\right]^T$

OpenCV雙目標定

        上面詳細闡述了雙目標定的具體算法，下面詳細闡述其具體操作方法。
        OpenCV提供了雙目標定的實現。本文采用的C++語言開發，IDE爲VS2015，OpenCV版本爲4.0版本。以下介紹其主要實現函數：
        1、雙目標定
        double cv :: stereoCalibrate （
        InputArrayOfArrays objectPoints，
        InputArrayOfArrays imagePoints1，
        InputArrayOfArrays imagePoints2，
        InputOutputArray cameraMatrix1，
        InputOutputArray distCoeffs1，
        InputOutputArray cameraMatrix2，
        InputOutputArray distCoeffs2，
        Size imageSize，
        OutputArray R，
        OutputArray T，
        OutputArray E，
        OutputArray F，
        INT flags = CALIB_FIX_INTRINSIC，
        TermCriteria standard =TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6)
        /*
        objectPoints 儲存標定角點在世界座標系中的位置；
        imagePoints1、imagePoints2 攝像機1、2的棋盤角點像素座標；
        cameraMatrix1 左攝像機的內參矩陣；
        distCoeffs1 左攝像機的畸變係數矩陣；
        cameraMatrix2 右攝像機的內參矩陣；
        distCoeffs2 右攝像機的畸變係數矩陣；
        imageSize 圖像的大小；
        R，左右攝像機的旋轉矩陣；
        T，左右攝像機的平移矩陣；
        E，本徵矩陣；
        F，基本矩陣；
        Flag 可能爲零的不同標誌或以下值的組合：
        CV_CALIB_FIX_INTRINSIC固定輸入的cameraMatrix和distCoeffs不變，這樣只估算R，T，E和F矩陣。
        CV_CALIB_USE_INTRINSIC_GUESS以單目標定的相機內參和畸變係數爲初始值進行迭代。
        CV_CALIB_FIX_PRINCIPAL_POINT 不改變圖像原點。
        CV_CALIB_FIX_FOCAL_LENGTH迭代過程中不會改變焦距
        CV_CALIB_ZERO_TANGENT_DIST不考慮兩臺攝像機的切向畸變係數。
        CALIB_THIN_PRISM_MODEL計算係數s1，s2，s3和s4。
        CALIB_TILTED_MODEL係數tauX和tauY已啓用。
        /
        2、根據觀察到的點座標計算理想點座標
        void cv :: undistortPoints （
        InputArray src，
        OutputArray dst，
        InputArray cameraMatrix，
        InputArray distcoeffs，
        InputArray R = noArray()，
        InputArray P =noArray()
        ）
        /
        src， 觀察到的點座標
        dst，在非畸變和反向透視變換後輸出理想點座標。
        cameraMatrix 相機內參矩陣
        distCoeffs 相機畸變矩陣
        R - 對象空間中的整流變換（3x3矩陣），如果矩陣爲空，則使用身份轉換。
        P - 新的相機內參矩陣。
        /
        3、極線計算
        void cv :: computeCorrespondEpilines （
        InputArray points，
        // 輸入矩陣。
        //包含點的圖像索引（1或2）
        InputArray F，
        //基本矩陣
        OutputArray line
        //輸出的極線參數矩陣
         ）;
        4、基本矩陣計算
        Mat cv :: findFundamentalMat（
        InputArray points1，
        //第一幅圖像的陣列點。
        InputArray points2，
        //與points1相同。
        Int method = FM_RANSAC，
        //用於計算基本矩陣的方法。
        /
        CV_FM_7POINT用於7點算法。ñ= 7
        CV_FM_8POINT通用的8點算法。ñ≥ 8
        用於RANSAC算法的        CV_FM_RANSAC。ñ≥ 8
        CV_FM_LMEDS爲LMedS算法。ñ≥ 8
        /
        double param1 = 3.0，
        //用於隨機一致性採樣的參數。用於判斷是否爲噪點的閾值
        double param2 = 0.99，
        //用於隨機一致性採樣和LM算法的參數，表示置信水平
        OutputArray mask =noArray()
        ）
        5、計算校準立體相機的每個攝像機的校正變換
        void cv :: stereoRectify （
        InputArray cameraMatrix1，
        InputArray distCoeffs1，
        InputArray cameraMatrix2，
        InputArray distCoeffs2，
        Size imageSize，
        InputArray R，
        InputArray T，
        OutputArray R1，
        OutputArray R2，
        OutputArray P1，
        OutputArray P2，
        OutputArray Q：
        Int flags = CALIB_ZERO_DISPARITY，
        double alpha = -1，
        Size newImageSize = Size()，
        Rect * validPixROI1 = 0，
        Rect * validPixROI2 =0
        ）
        /
        cameraMatrix1– 第一個相機矩陣.
        distCoeffs1– 第一個相機畸變參數.
        cameraMatrix2
        distCoeffs2
        imageSize– 用於校正的圖像大小；
        R–旋轉矩陣；
        T– 平移矩陣；
        R1、R2–矯正旋轉矩陣；
        P1、P2–投影矩陣（3x4）；
        Q–輸出深度視差映射矩陣
        Flags-操作標誌一般設爲CV_CALIB_ZERO_DISPARITY，也可設置爲0。
        Alpha-自由縮放參數。
        validPixROI1/ validPixROI2 -如果選擇，則會輸出矩形。
        /
        6、計算無畸變和裁剪後的轉換圖
        void cv :: initUndistortRectifyMap（
        InputArray cameraMatrix，
        InputArray distcoeffs，
        InputArray R，
        InputArray newCameraMatrix，
        Size size，
        Int m1type，
        OutputArray map1，
        OutputArray map2
        ）
        /
         cameraMatrix——攝像機內參矩陣
         distCoeffs——攝像機畸變係數矩陣
         R——兩臺攝像機之間的旋轉矩陣
         newCameraMatrix——校正後的攝像機內參矩陣
         size——攝像機採集的無畸變圖像尺寸
        m1type——map1的數據類型
        map1——輸出的X座標重映射參數
        map2——輸出的Y座標重映射參數
        */
        在本文程序中，雙目標定寫成了一個單獨的.c文件，名稱爲StereoCalib，其主函數爲：
        result *StereoCalib(
        int board_w, //棋盤的寬度
        int board_h, //棋盤的高度
        cv::Mat M1,
        cv::Mat D1,
         cv::Mat M2,
         cv::Mat D2,
         const char *imageList, //圖像文件列表
        bool useUncalibrated, //是否使用未校準方法
        bool displayCorners, //是否顯示角點
         bool showUndistores, //是否顯示校正後的圖像
         bool isVerticalStereo //判斷圖像是垂直還是水平
        )
        雙目標定利用的是單目標定時所拍攝的圖像，即:在單目標定時，當檢測到棋盤圖案時，左攝像機和右攝像機同時拍攝。經過雙目標定，校正並對應後的圖像如圖所示:

        圖像中包括左右攝像機拍攝棋盤圖像經過單目標定、雙目標定、裁剪後的對應圖案，圖中綠線即爲極線（極點位於無窮遠處），重映射平均誤差爲1.75537。
        經過計算，雙目相機的R、T、E、F四個參數分別爲：
        旋轉矩陣（R）：

$\left[\begin{matrix}0.9981761553222738&0.008033309321453018&-0.05983167127258031\\-0.00426022359904628&0.9980092129495719&0.06292425099007747\\0.06021804912922742&-0.06255459063187922&0.9962232228521618\end{matrix}\right]$

        平移矩陣（T）：

$\left[\begin{matrix}10.43727100758126\\-10.19376973116046\\-4.541974218262003\end{matrix}\right]$

        本徵矩陣（E）：

$\left[\begin{matrix}-0.6331987522339517&5.170599207333293&-9.869469808889111\\-5.162202461067245&0.6164121313678637&-10.1260978525939\\10.13071277023418&10.49838232901963&0.04684718093913587\end{matrix}\right]$

        基本矩陣（F）：

$\left[\begin{matrix}-1.629360687398105e^{-7}&1.516148714271131e^{-6}&0.02439611694235628\\-1.513686578734253e^{-6}&2.05965718081223e^{-7}&-0.0280764078560412\\0.02513265792029904&0.02909431391207914&1\end{matrix}\right]$

        Hunt Tiger Tonight
        2019-10-29
        聯繫方式：[email protected]（請勿使用其他聯繫方式，謝謝！）
        PS:再次請各位讀者朋友注意，這裏面很多東西涉及到我的畢設，寫作辛苦，請勿濫用，轉載請務必註明出處！