精準估算目標物距離　雙目相機自動校正不走鐘

3D雙目相機(Binocular Camera)的校正作業完成之後，能產生個別相機的內部和外部參數(Intrinsic And Extrinsic Parameters)、水平矯正參數(Rectification Parameter)，以獲得更準確的視差圖(Disparity Map)。一般而言，3D雙目相機在出廠之前，都有經過人工校正。不過，一旦將它安裝至自駕車、無人機、人形機器人之後，無法再執行人工校正的情況下，為適應各種不同的環境變化，並能精準地估算出目標物的距離，就必須仰賴3D雙目相機的自動校正(Auto-Calibration)或自我校正(Self-Calibration)功能。

針孔相機模型

立體視覺大都是應用針孔相機模型(Pinhole Camera Model)的原理來實現的。這主要是因為針孔相機模型的構造簡單、數學公式容易處理，以及能夠模擬在現實世界中已經過校正之後的相機的行為。圖1是針孔相機模型的示意圖。圖2是按照針孔相機模型設計的3D雙目相機產生立體影像或點雲(Point Cloud)的作業流程。其中，階段①可獨立作業，或稱之為離線(Offline)作業。完成階段①之後，可以獲得每台相機的內部參數和外部參數。一台相機的完整內部參數包含有內部矩陣(Intrinsic Matrix)，或稱作K矩陣，以及失真係數(Distortion Coefficients)或失真參數(k1、k2、p1、p2、k3等)，可用來模擬鏡頭失真(Lens Distortion)。

式子(1)是內部矩陣的表示式，其中，fx和fy分別是在x軸和y軸上的焦距(Focal Length)。cx和cy分別是在x軸和y軸上的主點(Principal Point)。s是傾斜係數(Skew Coefficient)，表示光學感測器在x軸和y軸之間的任何傾斜或非正交(Non-Orthogonality)。在大多數的數位相機中，傾斜可以忽略不計，通常被預設為零。

圖3是3D雙目相機進行人工校正時，所使用的9×7個正方形棋盤格圖案(Chessboard Pattern)。自動校正能透過匹配的特徵點(Matching Feature Point)，推算出每台相機的內部、外部參數。人工校正則必須依靠棋盤格座標中的內角(Interior Corner)位置。所謂內角是指在棋盤格圖案中，能組成一個完整正方形的角，但位於棋盤格四邊的角必須忽略不計，如圖3中的點才是內角。這是因為人工校正所使用的演算法是專門尋找棋盤格圖案中的這些正方形的角，例如：OpenCV的findChessboardCorners()函數。圖3的棋盤格圖案雖具有10×7個正方形，但它實際上只有9×6個內角。

外部參數包含旋轉矩陣(Rotation Matrix)R和位移向量(Translation Vector)t或T。利用針孔相機模型的內部、外部參數可模擬從3D座標轉換至2D座標，如圖4。針孔相機模型的內部參數、外部參數與世界座標、相機座標、像素座標之間的轉換關係，如圖5。內部參數和外部參數可構成相機矩陣P，如圖6，利用P可將3D世界座標上的點轉換成2D圖像座標上的點。從3D座標轉換至2D座標，稱作前向投影(Forward Projection, FP)，使用一般相機拍照就是源自這種原理，圖像是光學感測器所測得的數據。從2D座標轉換至3D座標，則稱為反向投影(Backprojection, BP)，3D雙目相機就是屬於這種原理，點雲是最終預估的數據。

此外，還有重投影(Reprojection)，通常它是為了評估點雲的精確度，將點雲從3D座標轉換至2D座標，然後比較由點雲轉換產生的2D圖和原本輸入的2D圖之間的誤差，若誤差越小，就表示點雲的精確度越高。如圖5所示，世界座標和相機座標都是屬於3D座標系，式子(2)是它們之間的轉換公式，其中，Pc是相機座標值，Pw是世界座標值。從相機座標轉換至世界座標時，必須使用旋轉矩陣R的轉置(Transpose)。

圖7、圖8是使用棋盤格圖案校正3D雙目相機的Python程式碼。圖7是前段程式碼，主要是為了產生能繼續使用的Python序列物件wide_dist_pickle.p，省去重複尋找棋盤格座標中的內角。在此Python序列物件內，包含有兩個主要的資料集：目標點(Object Point)、影像點(Image Point)。目標點是指可供校正使用的3D棋盤格座標的點。影像點是指在影像平面中所尋找到的內角位置。稍後經過後段程式碼運算後，每一個影像點會對應到一個3D目標點。這些目標點對於所有校正影像而言，都是相同的，因為它們代表真實的棋盤格座標。

圖8是後段程式碼，它首先載入wide_dist_pickle.p，再呼叫自訂的cal_undistort函式，根據載入的目標點和影像點計算出內部矩陣和失真係數，最後將輸入的圖像檔test_image.png去失真(Undistort)。

到此即完成以棋盤格圖案校正3D雙目相機的步驟。不過，若去失真的效果不如預期(圖9)，或calibrateCamera()傳回的誤差值(ret)太大(理想值是小於1個像素)，就必須重頭檢查每一個校正步驟和提高輸入圖像的畫質。

自動校正

使用3D雙目相機連續拍攝，取得一連串的2D雙圖像或圖像對(Image Pairs)。在每一對2D圖像之間，必須具有足夠的運動(Motion)和視差(Parallax)資訊，這對於自動校正所需的約束條件至關重要。被拍攝的目標物還必須具有足夠的紋理(Texture)，以獲得可靠的特徵偵測(Feature Detection)和特徵匹配(Feature Matching)的結果。此外，須確保當3D雙目相機在靜態場景中移動時，或當場景發生變動時，也能正常拍攝。

採集數據

使用3D雙目相機連續拍攝，取得一連串的2D雙圖像或圖像對(Image Pairs)。在每一對2D圖像之間，必須具有足夠的運動(Motion)和視差(Parallax)資訊，這對於自動校正所需的約束條件至關重要。被拍攝的目標物還必須具有足夠的紋理(Texture)，以獲得可靠的特徵偵測(Feature Detection)和特徵匹配(Feature Matching)的結果。此外，須確保當3D雙目相機在靜態場景中移動時，或當場景發生變動時，也能正常拍攝。

特徵偵測和匹配

• 特徵偵測：在每張圖像中，偵測出顯著的特徵。常見的特徵偵測演算法包括：SIFT、SURF、ORB、FAST、Harris Corner Detector。
• 特徵匹配：在左右兩圖像中偵測到的特徵之間，產生對應關係。並在圖像對序列中，建立不同圖像對之間的對應關係。通常是使用特徵描述符和匹配演算法來完成，例如：窮舉式匹配(Brute-Force Matching)、FLANN、比率測試(Ratio Test)。
• 異常值移除(Outlier Removal)：譬如使用隨機樣本共識(RANSAC)來刪除不符合對極幾何(Epipolar Geometry)的異常匹配。此步驟對於提高自動校正的精確度非常重要。

初始參數的估算 得出下列初始參數

• 基礎矩陣(Fundamental Matrix, F)：使用圖像對之間的特徵點的對應關係，估算出基礎矩陣。基礎矩陣是將立體視覺中的圖像對之間的幾何關係進行編碼，使左右兩圖像中的對應點產生關聯。式子(3)是基礎矩陣的數學式，其中x和x'是分別位於左右兩個圖像中的對應點。
• 本質矩陣(Essential Matrix, E)：如果內部參數已有粗估值，例如：來自先前的校正或相機規格，則使用基礎矩陣(F)和初始的內部參數，可算出本質矩陣(E)，如式子(4)。其中K'和K分別是左、右相機的內部矩陣。本質矩陣是對兩台相機位置之間的相對旋轉(右手法則)和相對平移資訊進行編碼。所以，也可以使用相機的外部參數來表示本質矩陣，如式子(5)，其中[t]x是平移向量t的斜對稱矩陣(Skew-Symmetric Matrix)，如式子(6)。平移向量t描述了兩台相機之間的相對運動。雖然，從幾何的角度來說，E和外積(Cross Product)相關，所以E=t×R似乎也成立，但外積畢竟是向量運算，其結果仍是向量，可是E是矩陣，所以應盡量避免使用外積符號x，而改用t的斜對稱矩陣(S)和矩陣乘法。

• 初始的內部與外部參數：使用奇異值分解(SVD)可從本質矩陣E中，求出3D雙目相機的旋轉矩陣R的初始估計值。平移向量(t)的初始估計值則可從E的零空間(NullSpace)導出。所謂零空間是指t滿足Et=0，E的轉換不會影響到t，因為t和E的零空間對齊。t代表運動方向，不是座標上的點。座標轉換之後，左右相機的相對運動方向應維持不變。通常t等於U正交矩陣的第三列u3。此外，為了估算3D雙目相機的內部參數，可利用最佳化方法，譬如：萊文伯格-馬夸特(Levenberg–Marquardt)演算法，求出內部參數的初始預估值。

捆集調整BA非線性微調

• 制訂最佳化問題：定義一個重投影誤差最小的最佳化問題。
• 重投影誤差最小化：上述的萊文伯格-馬夸特演算法是屬於局部的最佳化，而BA是屬於全域的最佳化。而且BA極大化地減少多個圖像的重投影誤差，能提高3D座標點和相機校正參數的準確性。
• 稀疏集(Sparse Bundle)調整：對於大型資料集，BA能充份利用問題的稀疏性，以降低運算成本。

解析度的縮放因數(選項)

若3D座標需要絕對的比例值或深度值，就必須結合其它可靠的數據來源，以消除含糊不清的情況，這包含：

• 利用場景中已知的物件距離。
• 結合來自其他感測器(例如：IMU、GPS)的數據。
• 若是安裝在SLAM系統中，可應用閉環技術(Loop Closure)。

輸出

自動校正的輸出為一組完整的校正參數，包括：

• 每台相機的內部參數。
• 外部參數。
• 場景的3D重建(選項)。