用opencv實現立體匹配
轉自：http://blog.csdn.net/scyscyao/article/details/5443341
嘗試用OpenCV來實現立體視覺也有一段時間了，主要的參考資料就是Learning OpenCV十一、十二章和OpenCV論壇上一些前輩的討論。過程中磕磕碰碰，走了不少彎路，終於在前不久解決了最頭大的問題，把整個標定、校準、匹配的流程除錯成功。(雖然還有一些問題至今尚未搞清)
在這裡寫這篇文章，第一方面是給自己一個總結，第二方面是感覺OpenCV立體視覺方面的資料還是相當零散和不完整，新手入門需要花很長時間才能摸索出來，第三方面，也是自己在過程中有些問題仍舊迷迷糊糊，希望可以拋磚引玉。
1. 攝像頭
我用的攝像頭是淘寶上買的三維攝像頭，兩個USB Camera加一個可調節的支架。實物照片如下
1.1 三維攝像頭實物圖
雙USB攝像頭的OpenCV驅動可以參考以下連結
將上面程式碼複製到自己的工程之後還需要對工程或者編譯環境做一下設定
VC6下的詳盡設定可以見程式碼的註釋(修改工程的屬性)
VS2008中的設定也可以參照程式碼註釋中VC++2005的設定(修改編譯環境)
2. 標定
由於OpenCV中cvStereoCalibrate總是會得到很誇張的結果(見下文5.1問題描述)，所以最後還是決定用Bouguet的Matlab標定工具箱立體標定，再將標定的結果讀入OpenCV，來進行後續影象校準和匹配。
Matlab標定工具箱的參考連結如下：
上面有詳細的使用步驟，用起來相當的方便。
以下是我個人用Matlab工具箱進行立體標定的步驟，供參考，如果需要更詳細步驟的話還是參照上面的連結
把Matlab工具箱的檔案copy到對應目錄下，把所要標定的棋盤圖也放到.m檔案所在的目錄下，然後在Matlab命令列視窗中打入calib_gui，選擇Standard之後便出現以下視窗
2.1. calilb_gui面板
我們先對右攝像頭的標定，所以先把從右攝像頭上採集到的棋盤圖複製到工具箱目錄下。
點選Image names, 命令列視窗會提示你輸入圖片的basename以及圖片的格式(比如你圖片檔名是right1, right2, …, right10，basename就是right)，然後Matlab會自動幫你讀入這些圖片，如下圖所示，可以看到，讀入了10幅右攝像頭的棋盤圖。
採集棋盤圖的時候要注意，儘量讓棋盤佔據儘可能多的畫面，這樣可以得到更多有關攝像頭畸變方面的資訊
2.2. 影象basename讀入
2.3. 讀入的棋盤圖
然後再回到主控制介面，點選Extract grid corners，提取每幅圖的角點
2.4. calib_gui面板
點選完後，命令列會出現如下提示，主要是讓你輸入棋盤角點搜尋視窗的大小。視窗定的大一點的話提取角點會比較方便點（即便點得偏離了也能找到），但也要注意不能大過一個方格的大小。剩下的兩個選項，只要回車選用預設設定就可以了
2.5. 選擇視窗大小
然後就開始了角點的提取工作，按一定順序分別提取棋盤的最邊上的角點，程式會自動幫你找到所有對應的角點
2.6. 提取角點
2.7. 提取角點2
在提取第一幅圖的時候命令列視窗可能會提示你輸入方格大小，這裡輸入你方格的實際大小就行，比如我方格是27mm，就輸入27。這步事實上相當關鍵，它定義了空間的尺度，如果要對物體進行測量的話，這步是必須的。
按相同的方法提取完10幅圖後，點選Calibration，開始攝像頭標定
2.8. calib_gui面板
經過多次迭代後，程式會最終得到攝像頭的內外引數，如下圖所示(圖中符號由於字型關係沒有完全顯示，中間的問號是表示誤差的加減號)
2.9. Calibration迭代過程及結果
可以通過面板上的Show Extrinsic檢視一下標定結果，可以驗證一下標定外引數的結果
2.10. 外部引數圖示
驗證標定結果無誤之後，就點選面板上的Save按鈕，程式會把標定結果放在一個叫Calib_Result.mat中，為了方便後續立體標定，把這個檔名改為Calib_Result_right.mat。
左攝像頭標定的方法與右攝像頭相同，生成的Calib_Result.mat之後，將其改名為Calib_Result_left.mat就可以了
左右攝像頭都標定完成之後，就可以開始立體標定了。
在Matlab命令列中鍵入stereo_gui啟動立體標定面板，如下圖所示
2.11. stereo_gui面板
點選Load left and right calibration files並在命令列中選擇預設的檔名(Calib_Result_left.mat和Calib_Result_right.mat)之後就可以開始Run stereo calibration了，run之後的結果如下圖所示，左右攝像頭的引數都做了修正，並且也求出了兩個攝像頭之間的旋轉和平移關係向量(om和T)
2.12. 立體標定結果
在面板上點選Show Extrinsics of stereo rig，可以看到如下圖所示的雙攝像頭關係圖，可以看到，兩個攝像頭基本是前向平行的
2.13. 雙攝像頭與定標棋盤間的位置關係
得到了立體標定引數之後，就可以把引數放入xml檔案，然後用cvLoad讀入OpenCV了。具體的方法可以參照Learning OpenCV第11章的例子，上面就是用cvSave儲存標定結果，然後再用cvLoad把之前的標定結果讀入矩陣的
2.14. xml檔案示例
這裡需要注意的是Matlab標定結果中的om向量，這個向量是旋轉矩陣通過Rodrigues變換之後得出的結果，如果要在cvStereoRectify中使用的話，需要首先將這個向量用cvRodrigues轉換成旋轉矩陣。關於Rodrigues變換，Learning OpenCV的第11章也有說明。
2.15. 旋轉矩陣的Rodrigues形式表示
3. 立體校準和匹配
有了標定引數，校準的過程就很簡單了。
我使用的是OpenCV中的cvStereoRectify，得出校準引數之後用cvRemap來校準輸入的左右影象。這部分的程式碼參考的是Learning OpenCV 十二章的例子。
校準之後，就可以立體匹配了。立體匹配OpenCV裡面有兩種方法，一種是Block Matching，一種是Graph Cut。Block Matching用的是SAD方法，速度比較快，但效果一般。Graph Cut可以參考Kolmogrov03的那篇博士論文，效果不錯，但是執行速度實在是慢到不能忍。所以還是選擇BM。
以下是我用BM進行立體匹配的引數設定
- BMState = cvCreateStereoBMState(CV_STEREO_BM_BASIC,0);
- assert(BMState != 0);
- BMState->preFilterSize=13;
- BMState->preFilterCap=13;
- BMState->SADWindowSize=19;
- BMState->minDisparity=0;
- BMState->numberOfDisparities=unitDisparity*16;
- BMState->textureThreshold=10;
- BMState->uniquenessRatio=20;
- BMState->speckleWindowSize=13;
其中minDisparity這個引數我設定為0是由於我的兩個攝像頭是前向平行放置，相同的物體在左圖中一定比在右圖中偏右，如下圖3.1所示。所以沒有必要設定回搜的引數。
如果為了追求更大的雙目重合區域而將兩個攝像頭向內偏轉的話，這個引數是需要考慮的。
3.1. 校正後的左右檢視
另外需要提的引數是uniquenessRatio，實驗下來，我感覺這個引數對於最後的匹配結果是有很大的影響。uniquenessRatio主要可以防止誤匹配，其主要作用從下面三幅圖的disparity效果比對就可以看出。在立體匹配中，我們寧願區域無法匹配，也不要誤匹配。如果有誤匹配的話，碰到障礙檢測這種應用，就會很麻煩。
3.2. UniquenessRatio為0時的匹配圖，可以看到大片的誤匹配區域
3.3. UniquenessRatio為10時的disparity map, 可以看到誤匹配被大量減少了, 但還是有噪點
3.4. UniquenessRatio為20時的disparity map, 可以看到誤匹配基本被去除了, 點雲乾淨了很多
關於cvFindStereoCorrespondenceBM這個函式的原始碼，曾經做過比較詳細的研究，過一段時間也會把之前寫的程式碼註釋整理一下，發篇博文。
4. 實際距離的測量
在用cvFindStereoCorrespondenceBM得出disparity map之後，還需要通過cvReprojectImageTo3D這個函式將單通道Disparity Map轉換成三通道的實際座標矩陣。
4.1 距離轉換公式
但是在實際操作過程中，用cvReprojectImageTo3D得到的資料並未如實際所想，生成深度矩陣所定義的世界座標系我就一直沒弄清楚。這在下面的例子中會詳細說明，希望這方面的專家能幫忙解答一下：
圖4.2是測量時的實際場景圖，場景中主要測量的三個物體就是最前面的利樂包裝盒、中間的紙杯、和最遠的塑料瓶。
4.2. 實際場景中三個待測物體的位置
圖4.3是校準後的左右圖和匹配出來的disparity map，disparity視窗中是實際的點雲，object視窗是給disparity map加了個閾值之後得到的二值圖，主要是為了分割前景和背景。可以看到要測的三個物體基本被正確地分割出來了
4.3. 雙目攝像頭得到的disparity map
圖4.4是在disparity視窗中選取一個點後然後在實際座標矩陣中得到的對應三維資訊，在這裡，我在三個物體的點雲上各選一個點來代表一個物體實際的座標資訊。(這裡通過滑鼠獲取一點座標資訊的方法參考的是opencv sample裡的watershed.cpp)
4.4. 對應點的三維座標
在這裡可以看到，(265, 156)也就是利樂包裝盒的座標是(13, 12, -157)，(137, 142)紙杯的座標是(77, 30, -312)，(95, 115)塑料瓶的座標是(144, 63, -482)。
補充一下：為了方便顯示，所以視差圖出來之後進行了一個0-255的normalize，所以value值的前一個是normalize之後點的灰度值，後一個是normalize之前點的實際視差圖。
由cvFindStereoCorrespondenceBM演算法的原始碼：
dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4); 其中 ndisp是ndisp = state->numberOfDisparities; mindisp是mindisp = state->minDisparity; mind就是sad得出的視差 實際視差大約是(64-mind-1)*256=1163, 基本是對的, 後面一項修正值在通常情況下可以忽略目前我還是不是很清楚立體座標系原點和尺度，但是從這三個點的z座標可以大致看出這三個物體的距離差大概是1:2:3，基本與實際場景中物體的位置一致。因此，可以通過這種方法確定出物體的大致距離資訊。
但是，如果就從攝像頭引數本身來測量距離的話，就不是很明白了，還求這方面的大牛解答。
5. 一些問題
5.1 關於StereoCalibrate
OpenCV自帶的cvStereoCalibrate感覺不怎麼好用，用這個函式求出的內參外參和旋轉平移矩陣進行校準，往往無法達到行對準，有時甚至會出現比較可怕的畸變。在看了piao的http://www.opencv.org.cn/forum/viewtopic.php?f=1&t=4603帖子之後，也曾經嘗試過現用cvCalibrateCamera2單獨標定(左右各20幅圖)，得出的結果基本和Matlab單獨標定的相同，然後再在cvStereoCalibrate中將引數設成CV_CALIB_USE_INTRINSIC_GUESS，用來細化內參數和畸變引數，結果得出的標定結果就又走樣了。
不知道有誰在這方面有過成功經驗的，可以出來分享一下。畢竟用Matlab工具箱還是麻煩了些。
5.2 Translation向量以及立體匹配得出的世界座標系
Learning OpenCV中對於Translation和Rotation的圖示是這樣的
5.1. Learning OpenCV中的圖示
可是在實驗過程中發現，如果將Translation向量按尺度縮放，對於StereoRectify之後的左右檢視不會有變化，比如將T = [ -226.73817 -0.62302 8.93984 ] ，變成T = [ -22.673817 -0.062302 0.893984 ]，在OpenCV中顯示的結果不會有任何變化。而且我如果修改其中的一個參量的話，左右檢視發生的變化也不是圖5.1中所示的那種變化(比如把x縮小，那麼檢視發生的變化不是往x軸方向的平移)。
因此又回到了老問題，這裡這些座標的尺度究竟是什麼？通過ReprojectTo3D那個函式得到的三維座標又是以哪個點為原點，那三個方向為x,y,z軸的？
補充: 對這個問題的解答來自於和maxwellsdemon的討論
他的解釋如下:rotation是兩者的旋轉角度的關係，但是你要把它矯正平行，也是需要translation matrix的。你可以設想，兩個看似已經平行了的攝像頭，但是深度上放置的有差距，那麼在矯正的時候會議translation matrix所對應的角度或者直線為基準，二者旋轉一個小角度，使得完全平行。
評論：
1、在Matlab Calibration時，LZ講到“在提取第一幅圖的時候命令列視窗可能會提示你輸入方格大小，這個對於標定結果沒有太大的影響，選擇預設100mm就行。” 這是不正確的，應該正確輸入標定格的大小，否則會影響T的大小的。T在cvStereoRectify中用到，並輸出Q，這個Q在cvReprojectImageTo3D中被用到，會明顯改變輸出結果。因此會產生，LZ說的，比例正確，但結果不對的問題。 回覆 wobject：恩 當時對這部分沒有徹底理解 後來在跟maxwellsdemon的討論中也發現這個尺寸對於實際標定得出的距離是有影響的，如果這裡的邊長是實際尺寸的話那麼得出的Tx就是兩個攝像頭之間的中心距了。。。但是現在還是有一個問題，就是即使是這個中心距是對的，在用ReprojectTo3D這個函式生成的三維座標矩陣中得到的物體距離還是不對的，貌似用Q矩陣計算的時候d和Tx以及f的量綱都不一樣d的量綱是畫素點乘以256，Tx的量綱是毫米，f的量綱也就不知道什麼東西了。。。。所以到最後還是無法直接得出深度資訊
回覆 scyscyao：
關於 f 的綱量，我在 chenyusiyuan的部落格中，回覆過：
怎麼把焦距數值換算為實際的物理量？假設畫素點的大小為k x l，單位為mm， fx = f / k， fy = f / (l * sinA)， A 一般假設為 90° ， 是指攝像機座標系的偏斜度（就是鏡頭座標和CCD是否垂直）。
攝像機矩陣（內參）的目的是把影象的點從影象座標轉換成實際物理的三維座標。因此其中的fx, fy, cx, cy 都是使用類似上面的綱量。同樣，Q 中的變數 f，cx, cy 也應該是一樣的。 Tx的量綱是毫米，無錯。但我對d的量綱，理解為一個比例係數。你的說法，是畫素點乘以256，我仍然不太能理解。網上有一個說法是該值要除以16，才能得到真正的值（http://altruisticrobot.tistory.com/219）。按照這個說法，cvReprojectImageTo3D之後得出的數值乘以16後基本能得出接近實際值，當然這是disparity影象是以16S為格式的影象， 在OpenCV2.0以前，都只能用這個格式。而在OpenCV2.1中，disparity影象新增一個影象格式32F。該格式的值，直接cvReprojectImageTo3D之後得出的數值，就是實際值，所以建議你們直接使用2.1版好了。
還有就是呼叫cvStereoRectify函式時，必需使用CV_CALIB_ZERO_DISPARITY，才能用cvReprojectImageTo3D函式算值，否則不準確。而這個就對兩個攝像機的佈置有限制了，有可能引起cvStereoRectify函數出錯。
我至今還沒搞明白，Q這個矩陣是如何搞出來的，不知你是否有這方面的瞭解？
回覆 wobject：恩 那個我看到了 但是對於畫素的說法 我不是很理解 你這裡的意思是不是在像平面上 一個畫素點的大小為k x l 呢？ 的確我也覺得，焦距和偏移也是以畫素為單位的，因為它不會隨著我們定義棋盤尺度的變化而變化，但是這個k和l，是怎麼求出來的？
關於畫素點是乘以256這個說法 我是看原始碼推測的 你可以看下我博文上新補充的程式碼dptr[y*dstep] = (short)(((ndisp - mind - 1 + mindisp)*256 + (d != 0 ? (p-n)*128/d : 0) + 15) >> 4); 我不知道那個韓國人乘以16是怎麼得出的。。。不過reprojectTo3D那個函數出來的值應該不是除以16，我實驗測過，稍後我會把視訊附在博文後。
關於Q的那個問題，我不是很理解，Q這個矩陣應該就是把一些disparity轉depth所要計算的量提取出來，根據式子造的一個矩陣吧
回覆 scyscyao：那個韓國人乘以16是資料表達方式的意思，並不是說那個值錯了，或者說估計被乘以了16。在DSP運算裡面，為了減少運算量和儲存空間，常把浮點小數表示成整數，至於小數點的位置，就人為記住。在cvFindStereoCorrespondenceBM函式中，其輸入被定義成兩種方式，如果是16S即16位整形的話，就預設為輸入了一個16位的整數，但是它表示的disparity其實是這個整數的值除以16。在應用時，因為cvReprojectImageTo3D同樣默認了這個設定，所以不用除以16。你看那個韓國人也沒有除以16。
但是我做了下來發現自己輸出的三維座標也有問題，感覺單位不是毫米，而是米。。。。jiakun(at)jiakunliu.com