基于非平行雙目結(jié)構(gòu)的三維坐標(biāo)解算算法

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

（國網(wǎng)智能科技股份有限公司，濟(jì)南250401）

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

基于雙目視覺的重建技術(shù)已經(jīng)被廣泛應(yīng)用于醫(yī)學(xué)技術(shù)研究、精準(zhǔn)零件的制造、虛擬現(xiàn)實(shí)技術(shù)、目標(biāo)定位、特征識別及軍事研究等方面[3]。目前，基于計(jì)算機(jī)視覺的三維重建方法大致分為2 類：立體視覺方法和運(yùn)動恢復(fù)結(jié)構(gòu)方法[4-5]。立體視覺方法采用2臺或多臺相機(jī)，通過立體匹配求得視差，根據(jù)三角測量原理來恢復(fù)空間點(diǎn)的三維信息，其精度較高。

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

1 方法

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

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

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

1.1 最小二乘法

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

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

將式（2）所構(gòu)成的方程記為：

式（3）中：

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

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

1.2 歸一化最小二乘法

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

式（5）中：

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

1.3 基于異面直線公垂線重點(diǎn)的三維坐標(biāo)解算算法

1.3.1 算法幾何背景分析

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

1.3.2 算法推導(dǎo)

圖2 坐標(biāo)解算圖Fig.2 Coordinate solution diagram

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

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

根據(jù)點(diǎn)到直線的距離可知：

式中，I 為三階矩陣。

同理：

故，點(diǎn)P 到直線L1和L2的距離和為：

令對P 的偏導(dǎo)為0可得：

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

PP1=PP10-P1P10；

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

P1-P10=P10P1=‖ ‖P1P10L1；

將點(diǎn)P 代入上述公式：

同理可得：

則：

此時說明，點(diǎn)P 就是2條直線的公垂線的中點(diǎn)。

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

出現(xiàn)了為0 的特征值，所以矩陣必不可逆。此種情況因?yàn)?條直線平行，將無窮遠(yuǎn)點(diǎn)視為物點(diǎn)P 在世界坐標(biāo)系下的估計(jì)。

2 實(shí)驗(yàn)

2.1 實(shí)驗(yàn)平臺

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

圖3 四旋翼無人機(jī)平臺Fig.3 Quadrotor UAV platform

圖像采集系統(tǒng)使用的相機(jī)是小覓雙目攝像頭，如圖4所示。

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

使用圖3中的四旋翼無人機(jī)搭載圖4中的小覓攝像頭，分別在室內(nèi)和室外放置不同距離的目標(biāo)物體，對其進(jìn)行深度信息恢復(fù)。通過對使用算法所得到的距離與真實(shí)距離的對比，來驗(yàn)證算法的可行性。

2.2 相機(jī)標(biāo)定

本文采用張正友標(biāo)定法對相機(jī)進(jìn)行標(biāo)定，標(biāo)定結(jié)果如表1所示。

2.3 三維坐標(biāo)解算

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

表1 相機(jī)內(nèi)外參數(shù)Tab.1 Internal and external parameters of the camera

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

使用最小二乘法求解的部分三維坐標(biāo)點(diǎn)數(shù)據(jù)如表2所示。

表2 最小二乘法求解的部分三維坐標(biāo)點(diǎn)數(shù)據(jù)Tab.2 Partial three-dimensional coordinate points solved by least square method mm

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

Xmax-Xmin=73.73。

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

Ymax-Ymin=73.11。

測量值與真實(shí)值對比如表3所示。

表3 計(jì)算值與測量值Tab.3 Calculated and measured values mm

使用歸一化最小二乘法求解的部分三維坐標(biāo)點(diǎn)數(shù)據(jù)如表4所示。

表4 歸一化最小二乘法求解的部分三維坐標(biāo)點(diǎn)數(shù)據(jù)Tab.4 Partial three-dimensional coordinate points solved by normalized least square method mm

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

Xmax-Xmin=71.55。

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

Ymax-Ymin=71.85。

測量值與真實(shí)值對比如表5所示。

表5 計(jì)算值與測量值Tab.5 Calculated and measured values mm

使用本文設(shè)計(jì)的算法求解的部分三維坐標(biāo)點(diǎn)數(shù)據(jù)如表6所示。

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

Xmax-Xmin=68.44。

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

Ymax-Ymin=68.81。

表6 異面直線公垂線中點(diǎn)法求解部分三維坐標(biāo)點(diǎn)數(shù)據(jù)Tab.6 Partial three-dimensional coordinate points solved by midpoint method of common perpendicular line of different planes mm

測量值與真實(shí)值對比如表7所示。

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

2.4 深度信息恢復(fù)

采用的小覓相機(jī)標(biāo)準(zhǔn)版的基線為70 mm，其測量范圍為0.26～3 m，所以不適合大范圍測距。本文實(shí)驗(yàn)的深度恢復(fù)對象分別為50 mm 、70 mm 、1 500 mm 、2 000 mm。需說明的是，實(shí)際距離值來自于高精度的激光雷達(dá)所測量的數(shù)據(jù)。

實(shí)驗(yàn)1：采用張正友標(biāo)定算法得到的相機(jī)內(nèi)外參數(shù)，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點(diǎn)法來進(jìn)行深度信息恢復(fù)。目標(biāo)物體實(shí)際距離為500 mm。實(shí)驗(yàn)結(jié)果如表8所示。

實(shí)驗(yàn)2：采用張正友標(biāo)定算法得到的相機(jī)的內(nèi)外參數(shù)，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點(diǎn)法來進(jìn)行深度信息恢復(fù)。目標(biāo)物體實(shí)際距離為700 mm。實(shí)驗(yàn)結(jié)果如表9所示。

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

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

實(shí)驗(yàn)3：采用張正友標(biāo)定算法得到的相機(jī)內(nèi)外參數(shù)，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點(diǎn)法來進(jìn)行深度信息恢復(fù)。目標(biāo)物體實(shí)際距離為1 500 mm。實(shí)驗(yàn)結(jié)果如表10所示。

實(shí)驗(yàn)4：采用張正友標(biāo)定算法得到的相機(jī)內(nèi)外參數(shù)，然后用最小二乘法、歸一化最小二乘法、基于異面直線公垂線中點(diǎn)法來進(jìn)行深度信息恢復(fù)。目標(biāo)物體實(shí)際距離為2 000 mm。實(shí)驗(yàn)結(jié)果如表11所示。

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

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

3 結(jié)論

實(shí)驗(yàn)結(jié)果表明，相較于常用的最小二乘法解算和歸一化最小二乘法解算方法，基于非平行雙目結(jié)構(gòu)的三維坐標(biāo)解算算法在采用張正友標(biāo)定算法得到內(nèi)外參數(shù)、SGBM 立體匹配算法得到視差信息的基礎(chǔ)上，無論在室內(nèi)環(huán)境還是室外環(huán)境，解算得到的深度信息精度均為最高，具有良好的應(yīng)用前景。