基于非平行雙目結構的三維坐標解算算法

劉 俍，黃振寧，馬曉鋒，劉天立，魏傳虎

（國網智能科技股份有限公司，濟南250401）

視覺是人類獲取外界信息的重要途徑，近年來，計算機視覺得到了前所未有的快速發展[1]。基于雙目視覺的三維重建技術就是利用雙目相機模仿人類的雙眼來捕獲外界信息，再將捕獲的信息輸入到計算機中[2]。利用計算機軟件實現人類大腦的模擬和仿真，可以使計算機像人類大腦一樣對信息進行篩選和處理。再根據三角幾何原理得到目標物體的深度信息，從而實現三維重建，實現對目標場景和物體的三維恢復。

基于雙目視覺的重建技術已經被廣泛應用于醫學技術研究、精準零件的制造、虛擬現實技術、目標定位、特征識別及軍事研究等方面[3]。目前，基于計算機視覺的三維重建方法大致分為2 類：立體視覺方法和運動恢復結構方法[4-5]。立體視覺方法采用2臺或多臺相機，通過立體匹配求得視差，根據三角測量原理來恢復空間點的三維信息，其精度較高。

雙目立體視覺系統中，非平行雙目結構下的三維重建模型如圖1 所示。對每臺相機來說，如果光心和像點已知，就可確定兩者組成的直線上，在針孔成像模型下，物點必然會在此直線上，2 臺相機進行交匯，就會產生2條這樣的射線，且2條直線都經過像點，兩直線相交點就是物點。圖1中P 點即空間物點，它的三維位置是唯一確定的。對于這種相機交匯問題的研究，傳統的方法是使用最小二乘法來解一個由重投影矩陣所構成的超定方程組，它們都是基于一個前提：4個平面的交點等價于2條直線的交點[6]。而實際情況存在很多噪聲，并不能簡單地理解為等價[7]，本文從幾何角度出發，將相機交匯問題轉換成求解異面直線公垂線中點問題，并給出理論推導和實驗驗證。

1 方法

假設已經經過了雙目相機的標定，內外參數也已經求得。令ML與MR分別為2臺攝像機的投影矩陣，點P 在左右成像平面上的坐標分別為(uL,vL) 和(uR,vR)，在內外參數已經求得的情況下，ML與MR為已知的，可得：

聯立式（2）子公式（Ⅰ）（Ⅱ）（Ⅲ）（Ⅳ）可以求出點P 在世界坐標系下坐標( X,Y,Z)T。

式（2）為包含X ，Y ，Z 的4 個線性方程，若直線o1p1與直線o2p2一定相交，則這4 個方程必定有唯一解。隨意聯立3個式子組都能得到唯一解[10-11]。

1.1 最小二乘法

實際應用中，投影矩陣存在一定誤差。根據立體匹配算法得到的左右圖像的匹配點也存在誤差，這些誤差就會導致左右相機、左右同名像點在世界坐標系下不能共面。基于這種情況，就不存在三維空間坐標點P 同時滿足式（2）中的（Ⅰ）（Ⅱ）（Ⅲ）（Ⅳ），它們構成的線性方程組無解。通常情況下，會用最小二乘法求取一個近似解作為物點P 在世界坐標系下的坐標的一個估計( X,Y,Z)T。

最小二乘法是求取線性方程近似解最普遍的方法，用該方法求出來的近似解將會使式（2）的離差平方和最小[12-13]。離差是指將近似解代入方程的左邊所得到的值與代入方程右邊所得的值的差值，在理想情況下，近似解應該使得式（2）離差為0，離差越小說明精度越高。

將式（2）所構成的方程記為：

式（3）中：

則式（3）的最小二乘法解為：

滿足式（4）的X3×1可以使得方程式（2）中4個方程的離差平方和最小[14-15]。

1.2 歸一化最小二乘法

將式（2）看成4個平面方程，通過尋找一個三維空間點，這個三維空間點到4 個平面的距離的平方和達到最小作為方程組的近似解。基于這種原理，引出了下面的歸一化最小二乘法[16]。將α4×3的所有行向量進行歸一化處理，在式（3）的兩邊同時左乘一個矩陣

式（5）中：

由式（5）可得歸一化最小二乘法解：

1.3 基于異面直線公垂線重點的三維坐標解算算法

1.3.1 算法幾何背景分析

對于每臺相機而言，如果光心和像點是已知的，就可以確定光心和像點組成的直線。針孔模型下，三維空間點必然會在直線上，2臺相機就產生2條這樣的直線，2直線都經過三維空間點，所以，利用2直線在三維空間點相交的原理，就可以對其進行交匯定位。但是，由于相機畸變，受圖像噪聲等各種原因影響，2 條直線不一定會相交。從幾何角度分析，可以將到2 條異面直線距離平方之和最小的點作為2條異面直線的交點，再證明此交點為異面直線公垂線的中點。這時，交匯定位問題就可以轉化成求異面直線公垂線中點坐標的問題[17-18]。

1.3.2 算法推導

圖2 坐標解算圖Fig.2 Coordinate solution diagram

G1、G2、G3、G4分別對應式（2）中（Ⅰ）（Ⅱ）（Ⅲ）（Ⅳ）4個平面方程的法向量。即：

由2個平面的交線的求解公式可知，2個平面法向量的叉乘即為交線方向向量。G1×G2是式（2）中（Ⅰ）（Ⅱ）2 個平面交線的方向向量，由單位向量求解定理可知，L1求解公式如式（11）所示。同理，G3×G4是式（2）中（Ⅲ）（Ⅳ）2 個平面交線的方向向量，L2求解公式如式（11）所示。

根據點到直線的距離可知：

式中，I 為三階矩陣。

同理：

故，點P 到直線L1和L2的距離和為：

令對P 的偏導為0可得：

此時，點P 到兩直線距離平方和達到最小。其次證明這個點就是2條異面直線公垂線的中點。根據向量的減法法則可得：

PP1=PP10-P1P10；

P-P1=(P-P10)-(P1-P10)；

P1-P10=P10P1=‖ ‖P1P10L1；

將點P 代入上述公式：

同理可得：

則：

此時說明，點P 就是2條直線的公垂線的中點。

2）當 |L1TL2|=1 時，L1L2互相平行，L1-L2必為零向量，則L1+L2必不為零向量

出現了為0 的特征值，所以矩陣必不可逆。此種情況因為2條直線平行，將無窮遠點視為物點P 在世界坐標系下的估計。

2 實驗

2.1 實驗平臺

四旋翼飛行器具備結構簡單、機動性強、可垂直起降、定點懸停、容易操作等特點，故本文采用四旋翼無人機作為實驗平臺，對目標物體進行深度信息恢復，實驗采用四旋翼無人機平臺如圖3 所示。四旋翼無人機機身是由對稱的十字形鋼體結構構成；在十字形結構的4 個端點分別安裝1 個由2 片槳葉組成的旋翼為飛行器提供飛行動力，每個旋翼均安裝在1 個電機轉子上，通過控制電機的轉動狀態控制每個旋翼的轉速，來提供不同的升力以實現各種姿態；每個電機均又與電機驅動部件、飛行控制單元相連接，通過飛行控制單元提供的控制信號來調節轉速大小。

圖3 四旋翼無人機平臺Fig.3 Quadrotor UAV platform

圖像采集系統使用的相機是小覓雙目攝像頭，如圖4所示。

圖4 小覓攝像頭Fig.4 Xiaomi camera

使用圖3中的四旋翼無人機搭載圖4中的小覓攝像頭，分別在室內和室外放置不同距離的目標物體，對其進行深度信息恢復。通過對使用算法所得到的距離與真實距離的對比，來驗證算法的可行性。

2.2 相機標定

本文采用張正友標定法對相機進行標定，標定結果如表1所示。

2.3 三維坐標解算

分別用本文設計的算法、最小二乘法和歸一化最小二乘法求解圖5中的水杯三維點坐標。在獲得三維點坐標以后，通過求出坐標中X 坐標的最大值Xmax與X 坐標的最小值Xmin的差值和Y 坐標的最大值Ymax與Y 坐標的最小值Ymin的差值，實現對杯子直徑的測量，將距離的最大值視為杯子的直徑。

表1 相機內外參數Tab.1 Internal and external parameters of the camera

圖5 雙目原圖Fig.5 Original binocular image

使用最小二乘法求解的部分三維坐標點數據如表2所示。

表2 最小二乘法求解的部分三維坐標點數據Tab.2 Partial three-dimensional coordinate points solved by least square method mm

由表2，Xmax=37.85，Xmin=-35.88，杯子直徑計算可得：

Xmax-Xmin=73.73。

由表2，Ymax=38.95，Ymin=-34.16，杯子直徑計算可得：

Ymax-Ymin=73.11。

測量值與真實值對比如表3所示。

表3 計算值與測量值Tab.3 Calculated and measured values mm

使用歸一化最小二乘法求解的部分三維坐標點數據如表4所示。

表4 歸一化最小二乘法求解的部分三維坐標點數據Tab.4 Partial three-dimensional coordinate points solved by normalized least square method mm

由表4，Xmax=36.67，Xmin=-34.88，杯子直徑計算可得：

Xmax-Xmin=71.55。

由表4，Ymax=39.19，Ymin=-32.66，杯子直徑計算可得：

Ymax-Ymin=71.85。

測量值與真實值對比如表5所示。

表5 計算值與測量值Tab.5 Calculated and measured values mm

使用本文設計的算法求解的部分三維坐標點數據如表6所示。

由表6，Xmax=35.43，Xmin=-33.01，杯子直徑計算可得：

Xmax-Xmin=68.44。

由表6，Ymax=40.29，Ymin=-28.52，杯子直徑計算可得：

Ymax-Ymin=68.81。

表6 異面直線公垂線中點法求解部分三維坐標點數據Tab.6 Partial three-dimensional coordinate points solved by midpoint method of common perpendicular line of different planes mm

測量值與真實值對比如表7所示。

由表3 可知，使用最小二乘法進行三維坐標點的解算，然后求取杯子的直徑，與真實值進行對比，誤差為12.4%。由表5 可知，使用歸一化最小二乘法的誤差為9%。由表7 可知，使用本文提出的基于異面直線公垂線中點的三維坐標點解算算法的誤差為4.3%。由此得出，在非平行結構雙目結構下，本文提出的算法在解算三維坐標點時精度最高。

2.4 深度信息恢復

采用的小覓相機標準版的基線為70 mm，其測量范圍為0.26～3 m，所以不適合大范圍測距。本文實驗的深度恢復對象分別為50 mm 、70 mm 、1 500 mm 、2 000 mm。需說明的是，實際距離值來自于高精度的激光雷達所測量的數據。

實驗1：采用張正友標定算法得到的相機內外參數，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點法來進行深度信息恢復。目標物體實際距離為500 mm。實驗結果如表8所示。

實驗2：采用張正友標定算法得到的相機的內外參數，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點法來進行深度信息恢復。目標物體實際距離為700 mm。實驗結果如表9所示。

表8 3種解算算法對實際距離為500 mm 的目標深度信息恢復精度比較Tab.8 Comparison of the accuracy of depth information restoration of target with an actual distance of 500 mm by three algorithms

表9 3種解算算法對實際距離為700 mm 的目標深度信息恢復精度比較Tab.9 Comparison of the accuracy of depth information restoration of target with an actual distance of 700 mm by three algorithms

實驗3：采用張正友標定算法得到的相機內外參數，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點法來進行深度信息恢復。目標物體實際距離為1 500 mm。實驗結果如表10所示。

實驗4：采用張正友標定算法得到的相機內外參數，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點法來進行深度信息恢復。目標物體實際距離為2 000 mm。實驗結果如表11所示。

綜合表8～11 可以看出，在光照充足的室內環境下，環境干擾因素小，且飛機處于靜止狀態。所以，3種算法的精度都較高，誤差值在5%以內。本文提出的方法精度最高，精度在2%以內，能夠滿足精度需求較高的場合。由于室外環境光照不均勻，容易造成低紋理和重復紋理、深度不連續、平滑鏡面反射等問題，且無人機處于飛行狀態，自身的抖動對立體匹配影響很大，這使得測距的精度受到嚴重的影響，尤其是最小二乘法和歸一化最小二乘法解算，誤差達到20%左右，本文提出的方法精度雖然有所下降，但仍然保持在8%以內，說明其魯棒性更好。

表11 3種解算算法對實際距離為2 000 mm 的目標深度信息恢復精度比較Tab.11 Comparison of the accuracy of depth information restoration of target with an actual distance of 2 000 mm by three algorithms

3 結論

實驗結果表明，相較于常用的最小二乘法解算和歸一化最小二乘法解算方法，基于非平行雙目結構的三維坐標解算算法在采用張正友標定算法得到內外參數、SGBM 立體匹配算法得到視差信息的基礎上，無論在室內環境還是室外環境，解算得到的深度信息精度均為最高，具有良好的應用前景。