高 翀，黃影平，趙柏淦，胡 興

（上海理工大學(xué)光電信息與計(jì)算機(jī)工程學(xué)院，上海 200093）

引 言

視覺里程計(jì)（visual odometer, VO）利用單個(gè)或多個(gè)相機(jī)獲取的圖像流來估計(jì)相機(jī)的位姿參數(shù)[1]，進(jìn)而推算相機(jī)的運(yùn)動(dòng)軌跡。目前，VO廣泛應(yīng)用于智能汽車、移動(dòng)機(jī)器人、無人機(jī)等智能體，是智能體實(shí)現(xiàn)自主定位導(dǎo)航的一個(gè)重要方法。

根據(jù)利用的圖像信息不同，可分為直接法和特征法視覺里程計(jì)。直接法[2-3]利用圖像或某個(gè)子區(qū)域中所有像素的灰度信息計(jì)算相機(jī)的運(yùn)動(dòng)，基于幀間像素灰度不變假設(shè)，這一假設(shè)在光照改變時(shí)容易遭到破壞。特征法匹配跟蹤連續(xù)圖像序列中的一些顯著特征，依據(jù)特征信息在幀間的位置變化來估計(jì)位姿。相對(duì)而言，使用特征法的VO系統(tǒng)運(yùn)行穩(wěn)定，計(jì)算成本低，對(duì)光照、圖像噪聲等不敏感。特征法VO[4-6]大多選取點(diǎn)作為特征進(jìn)行位姿估計(jì)，典型的算法有ORB-SLAM2[5]和SOFT2[6]。ORB-SLAM2是著名的開源系統(tǒng)，提取ORB（oriented FAST and rotated BRIEF）點(diǎn)特征進(jìn)行幀間匹配，由跟蹤、建圖及回環(huán)檢測(cè)三個(gè)并行線程組成；SOFT2使用類角點(diǎn)作為點(diǎn)特征，分開求解相機(jī)旋轉(zhuǎn)和平移參數(shù)，在KITTI算法排行榜中靠前。

然而，在一些弱紋理場(chǎng)景中，如圖1所示，圖像中的點(diǎn)特征并不豐富，存在無法提取到足夠多的點(diǎn)特征，或者點(diǎn)特征分布不均的情況，這就會(huì)導(dǎo)致系統(tǒng)性能的下降乃至失效。另外，線是點(diǎn)的集合，相對(duì)于離散的點(diǎn)，幀間線特征的位置變化更具有顯著性，有利于提高特征檢測(cè)跟蹤的魯棒性。因此，一些基于線特征的VO系統(tǒng)被提出[7-9]。文獻(xiàn)[7]率先提出在擴(kuò)展卡爾曼濾波視覺定位與地圖構(gòu)建系統(tǒng)中使用線特征，他們采用了一種假設(shè)-檢驗(yàn)的方法檢測(cè)直線。文獻(xiàn)[9]中將迭代最近點(diǎn)（ICP）算法調(diào)整應(yīng)用于線特征中，在非線性優(yōu)化過程中考慮一對(duì)多的線段匹配，提出迭代最近多條線（ICML）視覺里程計(jì)方法。該方法針對(duì)高速運(yùn)動(dòng)的視頻序列，在相機(jī)旋轉(zhuǎn)度較小時(shí)可以得到精度良好的位姿估計(jì)值。但是單純使用線特征有其局限性，尤其在紋理度較高的場(chǎng)景中，檢測(cè)到的線段數(shù)量減少或質(zhì)量減弱時(shí)，單純使用線特征的算法精度下降。文獻(xiàn)[10]提出采用點(diǎn)線組合特征可以給算法提供更好的幾何約束，有效提高系統(tǒng)魯棒性，他們從RGB-D圖像中提取3D點(diǎn)和線段，使用最大似然估計(jì)計(jì)算相機(jī)位姿。文獻(xiàn)[11]同樣將點(diǎn)線特征結(jié)合，通過計(jì)算線段端點(diǎn)的亞像素視差進(jìn)行線特征匹配。文獻(xiàn)[12]采用點(diǎn)線組合特征建立重投影誤差模型，并且根據(jù)點(diǎn)線特征的檢測(cè)誤差協(xié)方差矩陣對(duì)點(diǎn)線特征的重投影誤差加權(quán)，通過非線性最小化此重投影誤差來估計(jì)相機(jī)位姿。文獻(xiàn)[13]在ORB-SLAM2的基礎(chǔ)上增加線特征，提出基于線的地圖初始化方法，在幀間旋轉(zhuǎn)幅度較小的假設(shè)條件下，根據(jù)連續(xù)3幀圖像中的5條線間的對(duì)應(yīng)關(guān)系估計(jì)相機(jī)位姿。

圖 1 弱紋理場(chǎng)景Fig. 1 The low-textured scene

本文采用點(diǎn)線結(jié)合作為特征，提出了一種基于點(diǎn)線特征融合的立體視覺里程計(jì)算法，在算法設(shè)計(jì)上有如下創(chuàng)新。1）構(gòu)建新穎的點(diǎn)線重投影誤差模型作為目標(biāo)函數(shù)，模型中，使用Huber核函數(shù)減小誤匹配特征對(duì)優(yōu)化過程的影響。使用列文伯格-馬夸爾特法求解所構(gòu)建的點(diǎn)線誤差模型，得到旋轉(zhuǎn)矩陣和平移向量，再利用球形線性插值技術(shù)對(duì)旋轉(zhuǎn)參數(shù)進(jìn)行進(jìn)一步優(yōu)化。2）在特征的提取與匹配階段，對(duì)點(diǎn)特征施加環(huán)形匹配和Bucketing技術(shù)，對(duì)線特征施加外觀幾何約束及環(huán)形匹配，有效提高特征匹配的精度。進(jìn)行匹配的時(shí)候施加恒速約束縮小搜索范圍，匹配質(zhì)量的提高增加了內(nèi)點(diǎn)的比例，從而減少位姿估計(jì)時(shí)的迭代次數(shù)，有效提高了運(yùn)行速度，實(shí)現(xiàn)了魯棒性與實(shí)時(shí)性間的平衡。3）在公共數(shù)據(jù)集KITTI和EuRoC中進(jìn)行實(shí)驗(yàn)驗(yàn)證，證明了本文算法在多種場(chǎng)景中的魯棒性能，相較于其他點(diǎn)線特征的VO以及具有代表性的點(diǎn)特征VO，本文算法在精度方面有所提升。

1 方法總體框架

圖2為本文所提基于點(diǎn)線特征融合的立體視覺里程計(jì)算法框圖，分為特征提取和運(yùn)動(dòng)估計(jì)兩個(gè)部分。系統(tǒng)輸入為 k ?1 幀和 k幀圖像， l 和 r 分別代表左右?guī)瑘D像，輸出為 k ?1 幀與 k 幀間的旋轉(zhuǎn)矩陣和平移向量

圖 2 方法總體框圖Fig. 2 The overall framework of the method

首先對(duì)獲取的第 k 幀左右兩幀圖像同時(shí)提取ORB和LSD（line segment detection）特征并進(jìn)行匹配，在此階段，對(duì)ORB施加恒速、點(diǎn)環(huán)形匹配及Bucketing約束，對(duì)LSD進(jìn)行外觀幾何約束及線環(huán)形匹配約束以提高特征匹配的精度及速度。然后在連續(xù)圖像序列中分別跟蹤點(diǎn)線兩種特征，計(jì)算點(diǎn)特征及線特征端點(diǎn)的空間位置，將所得到的空間位置投影到下一幀圖像中，構(gòu)建點(diǎn)線重投影誤差模型。最終由列文伯格-馬夸爾特法求解所構(gòu)建的點(diǎn)線誤差模型得到旋轉(zhuǎn)及平移參數(shù)，同時(shí)結(jié)合球形線性插值技術(shù)對(duì)旋轉(zhuǎn)參數(shù)進(jìn)行優(yōu)化。

2 點(diǎn)特征及其處理

選用ORB算子[14]檢測(cè)點(diǎn)特征。對(duì)檢測(cè)到的點(diǎn)作環(huán)形匹配，匹配的時(shí)候采用BRIEF描述子在一定的搜索范圍內(nèi)進(jìn)行，過程如圖3所示。在左右?guī)c前后幀間根據(jù)的順序進(jìn)行環(huán)形匹配，匹配的終點(diǎn)與起點(diǎn)相同時(shí)匹配成功，這樣的處理極大地提高了幀間匹配的精度。

圖 3 環(huán)形匹配與恒速約束Fig. 3 Correspondence matching between left-right and consecutive images with a loop process(yellow part indicates the search area)

為了縮小特征匹配時(shí)的搜索范圍以提高匹配速度，提出恒速約束模型。在相機(jī)幀率較高時(shí)其幀間運(yùn)動(dòng)變化較小，相機(jī)運(yùn)動(dòng)具有平滑性。假設(shè)相機(jī)在幀間勻速運(yùn)動(dòng)，通過重投影可以預(yù)測(cè)匹配點(diǎn)的粗略位置，進(jìn)而以預(yù)測(cè)位置為中心進(jìn)行匹配搜索。記空間點(diǎn) P 在第 k 幀上的投影點(diǎn)為 pk，它的預(yù)測(cè)位置 pk?pre可以由下式計(jì)算：

式中： pk?1為點(diǎn) P 在 k ?1 幀時(shí)的世界坐標(biāo)； K(·)為相機(jī)投影矩陣；為 k ?1 幀的位姿變換矩陣。對(duì)于左右?guī)瑘D像，在搜索區(qū)域?yàn)?δu×δv的矩形框內(nèi)進(jìn)行搜索匹配；對(duì)于前后兩幀圖像，搜索區(qū)域?yàn)榘霃綖?δr的圓，圓心為特征點(diǎn)的預(yù)測(cè)位置。如圖3所示，黃點(diǎn)為待匹配的點(diǎn)特征，黃色圓表示半徑為 δr的前后幀間搜索區(qū)域，黃色矩形框表示 δu×δv大小的左右?guī)g搜索區(qū)域。

此外，采用Bucketing技術(shù)[6]將圖像劃分為50pixel×50pixel 大小的矩形框，并確保在每個(gè)矩形框中提取一定數(shù)目的點(diǎn)特征，使得所提取的 ORB特征可以均勻分布在圖像中。

3 線特征及其處理

選用LSD算子[15]提取線特征，可在線性時(shí)間內(nèi)得到亞像素級(jí)精度的檢測(cè)結(jié)果。對(duì)提取到的LSD線特征計(jì)算LBD描述子[16]并進(jìn)行匹配，同時(shí)舍棄具有不同長(zhǎng)度及方向的線特征對(duì)，確保得到最佳匹配對(duì)，這可以有效提高線特征的匹配精度。

4 點(diǎn)線重投影誤差模型

4.1 空間直線的幾何表示

式中： Kl表示線段的投影矩陣； f 為相機(jī)焦距； cx， cy是像素坐標(biāo)相對(duì)于圖像坐標(biāo)的偏移量； Q 表示線段的變換矩陣，包含旋轉(zhuǎn)矩陣R和平移向量 t ； [·]1:3表示取前三維向量； [·]×表 示向量的反對(duì)稱矩陣。

圖 4 線特征重投影誤差模型Fig. 4 The re-projection error model of line features

4.2 線特征的重投影誤差

4.3 點(diǎn)線重投影誤差模型

構(gòu)建點(diǎn)線誤差模型時(shí)，通常直接將重投影誤差的二范數(shù)平方和作為求解的目標(biāo)函數(shù)。但由于匹配過程存在誤匹配，這會(huì)導(dǎo)致在目標(biāo)函數(shù)中二范數(shù)增長(zhǎng)速度過快，此時(shí)優(yōu)化求解算法會(huì)主要集中調(diào)整錯(cuò)誤的誤差項(xiàng)的值，進(jìn)而忽略正確的誤差項(xiàng)的影響。通過在目標(biāo)函數(shù)中增加Huber核函數(shù)，可以平衡每個(gè)誤差項(xiàng)對(duì)優(yōu)化過程的影響。因此，構(gòu)建點(diǎn)線重投影誤差模型如下：

5 實(shí)驗(yàn)與分析

選取公共數(shù)據(jù)集KITTI和EuRoC對(duì)本文所提算法進(jìn)行精度、魯棒性的實(shí)驗(yàn)論證。采用業(yè)內(nèi)普遍使用的兩種評(píng)判標(biāo)準(zhǔn)來評(píng)估算法的準(zhǔn)確性：1）平均旋轉(zhuǎn)誤差（average relative rotational Error, ARE）和平均平移誤差（Average Relative Translation Error, ATE），如文獻(xiàn)[18]所定義；2）絕對(duì)軌跡誤差（Absolute Trajectory Root-Mean-Square Error, AT-RMSE），如文獻(xiàn)[19]所定義。

實(shí)驗(yàn)采用Intel（R）Core（TM）i5-7200U（2.5 GHz）4核處理器。算法在Ubuntu16.04系統(tǒng)中實(shí)現(xiàn)，基于C++和OpenCV庫編寫。

5.1 KITTI數(shù)據(jù)集

KITTI數(shù)據(jù)集[18]共包含22個(gè)立體圖像序列，序列00～10給定了真實(shí)運(yùn)動(dòng)位姿，包含市區(qū)、鄉(xiāng)村和高速公路等實(shí)際運(yùn)動(dòng)場(chǎng)景，行駛距離從500 m至5 000 m不等，幀率為10 frame/s，圖像大小為1 241 pixel × 376 pixel。

使用KITTI數(shù)據(jù)集的00-10序列對(duì)本文所提算法進(jìn)行測(cè)試，并與ORB-SLAM2[5]、SOFT2[6]和PLSVO[12]進(jìn)行精度及實(shí)時(shí)性的對(duì)比。ORBSLAM2是著名的開源系統(tǒng)，采用ORB點(diǎn)特征；SOFT2在KITTI排行榜上基于視覺方法的里程計(jì)中排行第一，選用類角點(diǎn)作為特征；PLSVO是Ruben等人提出的一個(gè)結(jié)合概率方法與點(diǎn)線特征的視覺里程計(jì)系統(tǒng)。實(shí)驗(yàn)分別對(duì)00-10序列進(jìn)行5次測(cè)試，取平均值作為最終結(jié)果。

圖5為KITTI下的點(diǎn)線特征提取結(jié)果。黃色小點(diǎn)表示提取到的ORB點(diǎn)特征，紅色線段為提取到的LSD線特征。圖6展示了這四種算法對(duì)00-10序列測(cè)試得到的ARE、ATE及AT-RMSE的對(duì)比結(jié)果。與PLSVO相比，本文算法在所有序列上表現(xiàn)更佳，誤差值更小。與ORB-SLAM2和SOFT2相比，本文算法總體上與它們相近，在多個(gè)序列表現(xiàn)更佳，如序列00和序列06，這些表現(xiàn)效果更佳的序列多為城鎮(zhèn)場(chǎng)景，富含結(jié)構(gòu)化特征明顯的建筑物，存在較多易于提取的線特征，此種場(chǎng)景下，更為明顯地體現(xiàn)了線特征的優(yōu)勢(shì)。

圖 5 KITTI數(shù)據(jù)集點(diǎn)線特征提取結(jié)果Fig. 5 Point and line feature extracting results of KITTI

圖 6 KITTI 00～10序列的ARE、ATE與AT-RMSEFig. 6 The ARE, ATE and AT-RMSE of KITTI 00-10

表1展示了這四種算法對(duì)KITTI 00～10序列綜合評(píng)估的ARE和ATE，本文算法的ARE為0.20(?)/100m ，ATE為0.69%。總體上，我們的算法相較于ORB-SLAM2和PLSVO在精度上有所提升，與SOFT2的精度值相近。我們的算法全面優(yōu)于PLSVO算法，證實(shí)了本系統(tǒng)所采用的點(diǎn)線誤差模型以及特征匹配機(jī)制的有效性。圖7所示為使用本文算法對(duì)KITTI 00和06序列的軌跡構(gòu)建（藍(lán)色實(shí)線）以及與真實(shí)軌跡（黑色虛線）的對(duì)比圖。

表 1 KITTI 00～10序列的綜合評(píng)估ARE和ATE對(duì)比結(jié)果Tab. 1 The comparison of ARE and ATE on KITTI 00-10

表2對(duì)比了四個(gè)系統(tǒng)的每幀圖像的平均消耗時(shí)間。可以看到，本文算法略慢于ORB-SLAM2和SOFT2，快于PLSVO。這是因?yàn)槲覀兺瑫r(shí)提取點(diǎn)線特征，而ORB-SLAM2和SOFT2僅需提取點(diǎn)特征，所以本文算法略慢是合理的。但是，在特征提取與匹配階段，施加了恒速約束限制搜索范圍，施加外觀幾何約束和環(huán)形匹配約束提高匹配精度，有效提高了內(nèi)點(diǎn)比例，減少優(yōu)化過程中的迭代次數(shù)，因此我們的算法速度快于PLSVO。

表 2 平均每幀時(shí)耗對(duì)比Tab. 2 The comparison result of consuming time per frame

表 3 EuRoC數(shù)據(jù)集11個(gè)序列的絕對(duì)軌跡誤差（AT-RMSE）對(duì)比結(jié)果Tab. 3 Comparison of AT-RMSE on EuRoC dataset

圖 7 KITTI 00與06序列的軌跡重構(gòu)圖Fig. 7 Trajectory reconstruction of KITTI sequences 00 and 06

5.2 EuRoC數(shù)據(jù)集

EuRoC數(shù)據(jù)集[20]是由微小型飛行器（Micro Air Vehicle, MAV）在三個(gè)不同場(chǎng)景中采集得到的11個(gè)圖像序列。根據(jù)MAV的速度、場(chǎng)景光照條件及紋理信息，將這11個(gè)序列分為三個(gè)難度等級(jí)，即容易、中等和困難。表3為本文算法在EuRoC數(shù)據(jù)集所有序列上的絕對(duì)軌跡誤差值，并與ORB-SLAM2和SOFT2進(jìn)行對(duì)比。由于PLSVO文獻(xiàn)作者并未進(jìn)行EuRoC數(shù)據(jù)集下的實(shí)驗(yàn)，所以這里未與之進(jìn)行比對(duì)。從結(jié)果可以看出，本文所提點(diǎn)線誤差模型在此數(shù)據(jù)集下具有良好的表現(xiàn)，誤差控制在厘米級(jí)別。EuRoC數(shù)據(jù)集都為室內(nèi)場(chǎng)景，室內(nèi)人造場(chǎng)景下，存在更多易于提取的線特征，其中序列V1_01_easy、V1_03_difficult中線特征豐富，點(diǎn)特征數(shù)量較少且存在分布不均的情況，場(chǎng)景中紋理信息較弱，綜合點(diǎn)線特征的系統(tǒng)比ORB-SLAM2和SOFT2表現(xiàn)更佳。圖8為EuRoC數(shù)據(jù)集中圖像的點(diǎn)線特征提取結(jié)果。圖9為V1_01_easy和V1_03_difficult的軌跡構(gòu)建以及與真實(shí)軌跡的對(duì)比。

圖 8 EuRoC數(shù)據(jù)集點(diǎn)線特征提取結(jié)果Fig. 8 Point and line feature extracting results of EuRoC

圖 9 EuRoC V1_01_easy與V1_03_difficult的軌跡重構(gòu)圖Fig. 9 Trajectory reconstruction of EuRoC V1_01_easy and V1_03_difficult

6 結(jié) 論

本文提出一種可在多種場(chǎng)景下穩(wěn)定工作、基于點(diǎn)線特征融合的立體視覺里程計(jì)方法。在特征提取與匹配階段，采用ORB算子提取點(diǎn)特征，采用LSD算子提取線特征。對(duì)ORB特征匹配采用環(huán)形匹配和Bucketing技術(shù)；對(duì)LSD線特征施加外觀幾何約束及環(huán)形匹配策略提高匹配的精度；匹配的時(shí)候施加恒速約束縮小搜索范圍，提高匹配速度。在位姿估計(jì)階段，提出了一種新穎的融合點(diǎn)線特征的點(diǎn)線重投影誤差模型，使用Huber核函數(shù)減小誤匹配特征對(duì)優(yōu)化過程的影響，由此模型結(jié)合球形線性插值得到相機(jī)位姿參數(shù)。采用公共數(shù)據(jù)集KITTI及EuRoC對(duì)算法進(jìn)行實(shí)驗(yàn)驗(yàn)證，并與代表性的點(diǎn)特征VO以及點(diǎn)線特征VO進(jìn)行比較。實(shí)驗(yàn)結(jié)果證明我們的算法性能表現(xiàn)良好，可在多種場(chǎng)景下應(yīng)用。