基于雙目視覺的點集配準(zhǔn)

雙目原理

雙目視覺是利用視差原理的一種視覺方法。

如圖所示為空間中一點P在左右相機中的成像點Pleft=（Xleft，Yleft），Pright=（Xright，Yright）。將兩相機固定在同一平面上，則點P在Y方向的坐標(biāo)是相同的，即Yleft = Yright =Y。根據(jù)三角原理，可得：

視差被定義為相同點在左右相機X方向的偏差，即：Disparity=Xleft-Xright。則點P在左相機坐標(biāo)系下的位置可以表示為：

因此，對于空間中任意一點，若能在嚴(yán)格對齊的兩相機中找到其成像點，即可計算出其三維坐標(biāo)。

雙目標(biāo)定

雙目標(biāo)定和普通相機的標(biāo)定一樣，其標(biāo)定時不僅標(biāo)定出兩個相機的內(nèi)參，還要標(biāo)定出兩個相機之間的關(guān)系。因此，在進行雙目標(biāo)定時，需要使兩個相機對同一標(biāo)定板進行多次取圖，分別標(biāo)定出各自的內(nèi)參和相對于標(biāo)定板的外參，然后便可計算出兩相機位置間的關(guān)系：

其中，R為兩攝像頭間的旋轉(zhuǎn)矩陣，T為兩攝像頭間的平移矩陣。Rr為右攝像頭經(jīng)過張氏標(biāo)定得到的相對標(biāo)定物的旋轉(zhuǎn)矩陣，Tr為右攝像頭通過張氏標(biāo)定得到的相對標(biāo)定物的平移向量。Rl為左攝像頭經(jīng)過張氏標(biāo)定得到的相對相同標(biāo)定物的旋轉(zhuǎn)矩陣，Tl為左攝像頭經(jīng)過張氏標(biāo)定得到的相對相同標(biāo)定物的平移向量。

上述只是理論公式，opencv的steroCalibrate函數(shù)在實現(xiàn)時會對多次計算的R、T進行優(yōu)化，最終得到兩相機間的最優(yōu)變換。

雙目校正

經(jīng)過雙目標(biāo)定后的兩相機在拍攝同一對象后，需要進行雙目校正。雙目校正多利用極線約束，來使同一特征點位于左右相機兩張圖像水平方向的同一條直線上，即“把實際中非共面行對準(zhǔn)的兩幅圖像，校正成共面行對準(zhǔn)”。當(dāng)然，在這個過程中也會進行一些畸變校正。利用極線約束進行雙目校正后可以使特征點在兩幅圖像中都位于極線上，這樣在進行特征點匹配時僅需要在極線上進行搜索而不需要在整個二維圖像上進行搜索，大大減少了計算量。

（1）雙目校正前的左右相機圖像

（2）雙目校正后的左右相機圖像

立體匹配

立體匹配，即將左右相機圖像上的對應(yīng)點匹配起來，由此計算視差。立體匹配有多種算法：

1、根據(jù)最優(yōu)化的理論方法不同，分為局部匹配算法和全局匹配算法；

2、根據(jù)采用圖像表示的基元不同，分為：

1.區(qū)域立體匹配算法

給定一幅圖像上的一點，選取該點鄰域內(nèi)的一個子窗口，在另一幅圖像中的一個區(qū)域內(nèi)，根據(jù)某種相似性，尋找與子窗口圖像最相似的窗口，得到的匹配窗口中對應(yīng)的像素點就為該像素的匹配點?？色@得稠密視差圖。

2.基于特征的立方匹配算法

基于幾何特征信息（邊緣、線、輪廓、興趣點、角點和幾何基元等）提取圖像的幾何特征點，針對幾何特征點進行視差估計，利用得到的視差信息重建三維空間場景?？色@得稀疏視差圖，通過插值可獲得稠密視差圖。算法速度快，由于需要插值算法來計算缺失像素點的視差值，應(yīng)用場景受限。

3.基于相位立體匹配算法

假設(shè)在圖像對應(yīng)點中，其頻率范圍內(nèi)局部相位相等，在頻率范圍內(nèi)進行視差估計。

各種立體匹配算法的對比如下：

點集配準(zhǔn)

點集配準(zhǔn)是用于將兩個點云集進行匹配，計算其旋轉(zhuǎn)矩陣R和平移矩陣T及尺度變換（Scale），使用兩個點集上的點最大可能的重合，這個變換過程就是點集配準(zhǔn)。

點集配準(zhǔn)算法的經(jīng)典算法是ICP（Iterative Closet Points）

1）ICP算法核心是最小化一個目標(biāo)函數(shù)：

（這里的表述與原文略微有些不同，原文是用四元數(shù)加上一個偏移向量來表達旋轉(zhuǎn)平移變換。

這個目標(biāo)函數(shù)實際上就是所有對應(yīng)點之間的歐式距離的平方和。

2）尋找對應(yīng)點?？墒?，我們現(xiàn)在并不知道有哪些對應(yīng)點。因此，我們在有初值的情況下，假設(shè)用初始的旋轉(zhuǎn)平移矩陣對source cloud進行變換，得到的一個變換后的點云。然后將這個變換后的點云與target cloud進行比較，只要兩個點云中存在距離小于一定閾值（這就是題主所說的ICP中的一個參數(shù)），我們就認(rèn)為這兩個點就是對應(yīng)點。這也是“最鄰近點”這個說法的來源。

3）R、T優(yōu)化。有了對應(yīng)點之后，我們就可以用對應(yīng)點對旋轉(zhuǎn)R與平移T進行估計。這里R和T中只有6個自由度，而我們的對應(yīng)點數(shù)量是龐大的（存在多余觀測值）。因此，我們可以采用最小二乘等方法求解最優(yōu)的旋轉(zhuǎn)平移矩陣。一個數(shù)值優(yōu)化問題，這里就不詳細(xì)講了。

4）迭代。我們優(yōu)化得到了一個新的R與T，導(dǎo)致了一些點轉(zhuǎn)換后的位置發(fā)生變化，一些最鄰近點對也相應(yīng)的發(fā)生了變化。因此，我們又回到了步驟2）中的尋找最鄰近點方法。2）3）步驟不停迭代進行，直到滿足一些迭代終止條件，如R、T的變化量小于一定值，或者上述目標(biāo)函數(shù)的變化小于一定值，或者鄰近點對不再變化等。（這里也是題主所說的ICP算法中的一個參數(shù)）

算法大致流程就是上面這樣。這里的優(yōu)化過程是一個貪心的策略。首先固定R跟T利用最鄰近算法找到最優(yōu)的點對，然后固定最優(yōu)的點對來優(yōu)化R和T，依次反復(fù)迭代進行。這兩個步驟都使得目標(biāo)函數(shù)值下降，所以ICP算法總是收斂的，這也就是原文中收斂性的證明過程。這種優(yōu)化思想與K均值聚類的優(yōu)化思想非常相似，固定類中心優(yōu)化每個點的類別，固定每個點的類別優(yōu)化類中心。

審核編輯：郭婷