蔣育豪　陳蕾

摘要：將曼哈頓世界假說（Manhattan World assumption，MW）引入室內(nèi)定位問題，提出了一種改進(jìn)的基于RGB-D視覺與平面特征的室內(nèi)定位方案，不僅能有效提高場景匹配的成功率，還可簡化未知場景下的定位問題，提高定位效率和實(shí)時(shí)性，可用于對同步定位與建圖SLAM（Simultaneous Localization and Mapping）系統(tǒng)的擴(kuò)展.創(chuàng)新點(diǎn)主要體現(xiàn)在：針對解釋樹匹配的時(shí)間開銷隨特征數(shù)指數(shù)級上升的問題，設(shè)計(jì)了根據(jù)曼哈頓幀的主方向進(jìn)行分解的匹配方法;針對單條行進(jìn)路徑搜索效率有待提高的問題，提出了在初始位姿確定后采用4自由度的簡化定位方案;針對單幀中遍歷執(zhí)行子圖匹配耗時(shí)較長的問題，將小范圍子圖合并為大范圍子圖后進(jìn)行匹配.實(shí)驗(yàn)結(jié)果表明，該方案相較已有的平面特征定位方法，能縮短成功定位所需的行進(jìn)距離，并顯著降低單條行進(jìn)路徑上的平均搜索耗時(shí).

關(guān)鍵詞：定位; 同步定位與建圖; 曼哈頓世界假說

中圖分類號：TP399

文獻(xiàn)標(biāo)志碼：A

DOI： 10.3969/j.issn.1000-5641.2019.06.010

收稿日期：2019-01-11

第一作者：蔣育豪，男，碩士研究生，研究方向?yàn)橐曈X定位.E-mail： krovkov@163.com.

通信作者：陳 蕾，女，副教授，研究方向?yàn)槭覂?nèi)定位.E-mail： lchen@cs.ecnu.edu.cn.

0 引言

室內(nèi)定位（Indoor Localization）問題作為基于位置服務(wù)（Location-Based Service，LBS）、增強(qiáng)現(xiàn)實(shí)（Augmented Reality， AR）、同步定位與建圖（SLAM）等應(yīng)用的關(guān)鍵問題，是近年來領(lǐng)域內(nèi)普遍關(guān)注的熱點(diǎn)，現(xiàn)有的解決方案包括基于WiFi信號強(qiáng)度RSSI（Received SignalStrength Indication）、激光雷達(dá)（Lidar）和機(jī)器視覺等，其中機(jī)器視覺方案由于更接近人類感知方式、定位精度較高、室內(nèi)和外場景兼容度高等特點(diǎn)在探索未知環(huán)境的應(yīng)用中格外受到重視;而RGB-D采集設(shè)備的出現(xiàn)使得捕捉到的畫面帶有環(huán)境景深信息，令立體視覺的獲取更為方便快捷，

根據(jù)Glocker的定義¨視覺定位的解決方案主要可分為基于路標(biāo)方法LbAs（Landmark-based Approach）與基于圖像方法IbAs（lmage-based Approach）兩類，這兩類方案在實(shí)用化道路上存在各自的障礙，例如，以關(guān)鍵幀圖像整體或關(guān)鍵幀圖像內(nèi)特征作為匹配依據(jù)的IbAs雖具有較高定位精度，但圖像的引入導(dǎo)致其受視角變化的制約嚴(yán)重，無法利用不同視角下的相同特征路標(biāo)實(shí)現(xiàn)定位，而特征地圖構(gòu)建過程中引入的構(gòu)建誤差會給LbAs帶來負(fù)面影響;在LbAs中，以特征點(diǎn)描述子（Point Descriptor）作為路標(biāo)的解決方案可能因采集設(shè)備移動造成動態(tài)模糊（Motion Blur）而導(dǎo)致無法準(zhǔn)確提取特征，由于點(diǎn)特征無法全面呈現(xiàn)環(huán)境結(jié)構(gòu)，遮擋所造成的特征消失也將顯著影響匹配與定位效果，

雖然早期視覺SLAM及其定位方案受計(jì)算資源限制將像素點(diǎn)作為特征首選[2-3]，但高維特征由于在提取時(shí)較少受到動態(tài)模糊與遮擋問題影響而受到持續(xù)關(guān)注.通過匹配室內(nèi)場景中的已知物品來得到地圖特征的SLAM++頗具代表性[4]，但這一方案受限于物品種類，無法識別其預(yù)存立體模型庫中不存在的物品.相比之下，平面既是室內(nèi)場景中最常見的構(gòu)成要素，又能通過其組合呈現(xiàn)不同結(jié)構(gòu)，被認(rèn)為是優(yōu)良的全局特征來源.基于平面特征的定位方案有著廣闊的發(fā)展空間與應(yīng)用前景，

以此為基礎(chǔ)，Shi等利用深度學(xué)習(xí)為SLAM提供平面特征描述子[5];Forstner等利用平面匹配來計(jì)算路徑中位姿（Pose）的持續(xù)變化[6];Taguchi等同時(shí)將點(diǎn)與平面作為特征，并采用RANSAC算法進(jìn)行匹配與定位[7];Fernandez-Moral等的PbMap定位[8]則使用解釋樹算法匹配兩個(gè)由平面組合而成的結(jié)構(gòu)，實(shí)現(xiàn)識別及定位.PbMap定位方法本質(zhì)為充分利用平面所組成結(jié)構(gòu)而非平面特征本身進(jìn)行匹配，對環(huán)境結(jié)構(gòu)的理解與基本特征的利用有著獨(dú)到之處;另一方面它卻受限于運(yùn)行耗時(shí)的限制，一般僅利用較小視野范圍的子圖結(jié)構(gòu)進(jìn)行匹配，往往需要犧牲識別定位效果以支持實(shí)時(shí)運(yùn)行，值得關(guān)注的是，仍有一些基于平面的SLAM系統(tǒng)放棄對平面特征進(jìn)行復(fù)用，而是選擇采用基于關(guān)鍵幀的IbA方式實(shí)現(xiàn)定位[9-12].

本文致力于改進(jìn)PbMap定位方法，實(shí)現(xiàn)基于平面特征的LbA定位方案，并將曼哈頓世界假說（MW）引入至室內(nèi)場景的結(jié)構(gòu)匹配來擴(kuò)大視野范圍并提高運(yùn)算效率，旨在為平面SLAM系統(tǒng)實(shí)時(shí)提供可靠的定位結(jié)果，本文后續(xù)內(nèi)容組織如下：第1節(jié)介紹平面定位的相關(guān)概念并分析已有方法;第2節(jié)提出本文定位方案;第3節(jié)給出實(shí)驗(yàn)結(jié)果并進(jìn)行分析;第4節(jié)總結(jié)全文.

1 相關(guān)問題說明

1.1 曼哈頓世界假說

MW模型[13]揭示了人造建筑結(jié)構(gòu)的特性，以三維立體網(wǎng)格的形式更準(zhǔn)確地描述了城市及室內(nèi)結(jié)構(gòu)，之后提出的一系列結(jié)構(gòu)假說模型，如亞特蘭大世界假說（Atlanta World assump-tion，AW） [14]、混合曼哈頓世界假說（Mixed Manhattan World assumption，MMW） [15]等分別擴(kuò)展了其對建筑結(jié)構(gòu)特點(diǎn)的描述，總體上，人們對于建筑環(huán)境的描述方式可借助如圖1所示的一系列模型來表示，如圖l（b），點(diǎn)云作為表征物體的最基本元素可普適地展現(xiàn)外部環(huán)境[16]，而平面作為建筑的主要組成要素，被一系列平面SLAM及定位系統(tǒng)用于描述環(huán)境的基本特征，如圖l（c）.在平面的基礎(chǔ)上，曼哈頓世界系列假說模型進(jìn)一步探索了建筑的結(jié)構(gòu)特點(diǎn)：在MW下，建筑中存在一個(gè)正交的立體坐標(biāo)系，建筑內(nèi)平面的法線與一個(gè)坐標(biāo)軸保持平行，如圖l（f）;而在AW下，建筑中存在一個(gè)豎直主方向與多個(gè)水平的主方向，建筑內(nèi)平面的法線與其中一個(gè)主方向保持平行，如圖l（e）;MMW則作了進(jìn)一步推廣，假定建筑中存在一個(gè)主要的正交立體坐標(biāo)系與其他副坐標(biāo)系組，建筑內(nèi)平面的法線平行于某一坐標(biāo)系的坐標(biāo)軸，如圖l（d）.曼哈頓世界系列假說、平面及點(diǎn)云分別以不同方式對建筑結(jié)構(gòu)進(jìn)行了解讀，其相互關(guān)系如圖l（a）所示，選擇某一假說模型雖然犧牲了部分普遍特征（如平面模型犧牲了點(diǎn)云中的曲面信息、曼哈頓世界系列模型不同程度上犧牲了平面位姿的統(tǒng)計(jì)隨機(jī)性），但恰當(dāng)?shù)募僬f選擇也可能為應(yīng)用帶來更多先驗(yàn)信息.

1.2 解釋樹結(jié)構(gòu)匹配

解釋樹算法作為一類經(jīng)典匹配算法，通過遞歸計(jì)算組件與組件間的關(guān)系來確定整體的匹配情況，常被用于計(jì)算二維或三維對象的匹配問題[17].在三維匹配問題中，三維實(shí)體被拆解為多個(gè)分塊即組件來執(zhí)行解釋樹算法匹配，其算法流程見算法1.算法l中，一元約束判斷源實(shí)體中組件PC與目標(biāo)實(shí)體中組件PT之間是否匹配，二元約束判斷對于待加入的匹配對{PC，PT），Pc與Lbr中源實(shí)體的其他組件PC的相對關(guān)系，PT與Lbr中目標(biāo)實(shí)體的其他組件PT的相對關(guān)系是否匹配，算法最終得出兩實(shí)體間數(shù)量最多的組件匹配對集合，

在基于平面特征的定位應(yīng)用中，在目標(biāo)地圖中識別出當(dāng)前平面集所組成的結(jié)構(gòu)，就可以看作立體匹配的問題使用解釋樹算法進(jìn)行計(jì)算，得到當(dāng)前地圖與目標(biāo)地圖間對應(yīng)平面的配對關(guān)系并實(shí)現(xiàn)定位.

1.3 PbMap定位

平面定位模塊利用由視覺里程計(jì)（Visual Odometry，VO）連續(xù)配準(zhǔn)的平面特征地圖與目標(biāo)地圖進(jìn)行匹配實(shí)現(xiàn)定位，同時(shí)定位得到的位姿也進(jìn)一步地為VO修正誤差提供參考.Fernandez-Moral采用解釋樹匹配算法實(shí)現(xiàn)了利用平面特征的識別與定位[8]，以下從其建圖、識別與定位環(huán)節(jié)簡要介紹該方案.

1.3.1 PbMap建圖

由于解釋樹算法匹配是由平面所組成的結(jié)構(gòu)，故界定結(jié)構(gòu)的范圍是必要的.在人類認(rèn)知中，結(jié)構(gòu)表現(xiàn)為相鄰平面之間的局部關(guān)系，所以在解釋樹算法匹配兩個(gè)結(jié)構(gòu)之前，PbMap定位方案在當(dāng)前地圖與目標(biāo)地圖中分別劃分兩個(gè)具有一定量平面的鄰域以供匹配.PbMap連通圖的構(gòu)建將特征地圖中彼此鄰近、聯(lián)系緊密的平面特征相互聯(lián)接，形成的連通圖含有不同度的結(jié)點(diǎn).結(jié)點(diǎn)度值高，即聯(lián)系緊密的平面特征集合作為連通圖的樞紐結(jié)點(diǎn)，能與其相鄰結(jié)點(diǎn)平面組成更為復(fù)雜的結(jié)構(gòu)，更富有辨識度.于是，在特征地圖中構(gòu)建PbMap地圖并劃分子圖的目的在于，在特征地圖中構(gòu)建連通圖，找出富含鄰居結(jié)點(diǎn)即結(jié)構(gòu)特征豐富的平面組，并將其構(gòu)建成鄰域子圖進(jìn)行解釋樹匹配.

設(shè)備在探索環(huán)境的過程中將當(dāng)前幀內(nèi)所檢測到的每一平面置入地圖，并同步檢測該平面與其他所有平面的鄰近關(guān)系.如圖2（a）所示，若當(dāng)前幀提取到A、B兩平面并置入當(dāng)前地圖后，與平面A之間最短距離小于鄰近閾值th的平面都被視為平面A的鄰居平面（如圖2（b））.當(dāng)平面P與平面P互為對方的鄰居平面，則可以視為P與P滿足鄰近關(guān)系，即在二者之間連接邊.于是，把平面特征視為圖中的結(jié)點(diǎn)，而把平面特征之間的鄰近關(guān)系視為圖中的邊，就可以把平面特征地圖中的所有平面串聯(lián)形成了連通圖的形式.平面P及其所有鄰居平面結(jié)點(diǎn)就表示了一定范圍的鄰域內(nèi)，相鄰平面所組建的結(jié)構(gòu).于是，對結(jié)構(gòu)之間的匹配即可轉(zhuǎn)化為由平面與相連平面所形成的子圖之間的匹配，平面P的子圖即被定義為平面P及其所有鄰居平面的集合.平面A的子圖的結(jié)構(gòu)形式如圖2（b），連通圖形式如圖2（c）中的淺灰底色部分.子圖識別過程就將當(dāng)前地圖中A的子圖遍歷地與目標(biāo)地圖中每一平面所聯(lián)結(jié)的子圖結(jié)構(gòu)執(zhí)行解釋樹匹配.

1.3.2 子圖匹配識別

在劃分得到當(dāng)前地圖與目標(biāo)地圖中的子圖結(jié)構(gòu)后，就可以采用解釋樹匹配對當(dāng)前子圖結(jié)構(gòu)在目標(biāo)地圖中進(jìn)行識別.當(dāng)前子圖與目標(biāo)子圖中的平面集合即為待匹配組件集合Lc與LT，平面特征即為整體子圖匹配過程中基本組件.平面對Pu與Pu間的一元約束定義為

帶有一元約束與二元約束的判斷過程比較了以平面特征為結(jié)點(diǎn)的兩個(gè)子圖的匹配關(guān)系，其中一元約束比較在Lc與LT中作為結(jié)點(diǎn)的平面特征是否具有相同屬性;而二元約束則判斷LG與LT中作為邊的平面間相對位置關(guān)系是否一致，該識別匹配過程最終得到當(dāng)前子圖與目標(biāo)子圖的最多平面匹配對集合，并經(jīng)由后續(xù)定位過程將這些平面匹配對進(jìn)行對齊，通過最小化平面配準(zhǔn)誤差計(jì)算定位坐標(biāo).

2 結(jié)合MW改進(jìn)PbMap定位

2.1 行進(jìn)路徑的初始定位

在將MW引入至平面定位問題之后，曼哈頓主方向可以從多個(gè)方面為PbMap定位服務(wù)，提升其運(yùn)算效率與識別效果，下面以PbMap定位的一般過程為序，對傳統(tǒng)方法的不足進(jìn)行分析并給出本文的改進(jìn)方案.

2.1.1 PbMap建圖

第1.3.1介紹了將平面地圖聯(lián)結(jié)成子圖進(jìn)行匹配的一般方法，然而在實(shí)際應(yīng)用中，每一幀的定位過程通常含有大量的重復(fù)計(jì)算.例如在如圖2（a）所示的一幀畫面中，平面A、B被提取后依次將A、B所在的子圖與目標(biāo)地圖中的所有子圖進(jìn)行匹配.若A的子圖與目標(biāo)地圖中的所有子圖進(jìn)行一次遍歷匹配后未能成功識別，則繼續(xù)將平面B的子圖與目標(biāo)地圖中的所有子圖進(jìn)行遍歷匹配.由此可見，平均識別耗時(shí)會隨著單幀檢測到子圖數(shù)的增加而增長，這一點(diǎn)在實(shí)驗(yàn)中也得到了驗(yàn)證，通常每幀都涉及多次子圖搜索過程.同時(shí)，由于子圖匹配的耗時(shí)隨著子圖范圍的擴(kuò)張而呈指數(shù)級增長，PbMap定位[8]將鄰近關(guān)系閾值th推薦設(shè)置為1m以滿足實(shí)時(shí)應(yīng)用.這意味著，為了能在單幀執(zhí)行多次子圖搜索過程，PbMap在每次子圖搜索中只利用視野內(nèi)的部分特征，選取每一平面的小范圍子圖結(jié)構(gòu)進(jìn)行匹配搜索，減少每次子圖搜索的耗時(shí)以支持實(shí)時(shí)運(yùn)行;而在采用較小子圖結(jié)構(gòu)后，匹配識別成功率會隨著可用特征數(shù)的減少而降低，并為定位模塊提供更少的平面匹配對作為配準(zhǔn)依據(jù).

為在保證識別定位效果的同時(shí)提高搜索效率，在建圖過程中，本文將一幀中檢測到的所有平面及其鄰居面都置入同一個(gè)子圖，如圖2（c）中的深灰底色部分.令平面A、B都同處一個(gè)更大范圍的子圖之中進(jìn)行匹配;而相應(yīng)地在目標(biāo)地圖中也選取較大的鄰近關(guān)系閾值th來增大目標(biāo)子圖的范圍.在這里，由th所界定的子圖范圍可以看作為探測設(shè)備的視野范圍大小.在以探測設(shè)備單幀的視野范圍作為子圖范圍以消除重復(fù)搜索后，子圖匹配過程將進(jìn)一步利用MW信息削減其耗時(shí).

2.1.2 子圖匹配識別

在將單幀檢測到的平面置入同一子圖以排除多次搜索過程后，PbMap定位會受限于指數(shù)級增長的子圖匹配耗時(shí)，因大范圍子圖內(nèi)平面特征數(shù)量的增多而無法實(shí)現(xiàn)實(shí)時(shí)運(yùn)行，由于室內(nèi)場景中曼哈頓主方向信息的存在，在子圖匹配過程中本文將不同的主方向作為豎直方向，分為3種情況進(jìn)行解釋樹搜索，并在每組搜索中利用平面在曼哈頓主方向下的屬性信息濾除誤匹配對，子圖范圍內(nèi)的平面所呈現(xiàn)的MW坐標(biāo)系主方向之一被選取為子圖匹配的豎直方向，并進(jìn)而確定每個(gè)平面在當(dāng)前MW坐標(biāo)系下的附加位置信息：平面類型與沿MW坐標(biāo)軸尺寸.平面類型包括：①水平平面;②滿足MW的豎直平面，即與非豎直方向的某主方向保持垂直的平面;⑧僅滿足AW的豎直平面，即僅與假定豎直方向保持平行的平面;④隨機(jī)位姿平面，即不與任一主方向保持垂直或平行的平面，

由于平面基本屬性與子圖豎直方向的選取無關(guān)，所以3組搜索中的基本屬性約束只需執(zhí)行一次，在實(shí)際實(shí)現(xiàn)中，當(dāng)前地圖與目標(biāo)地圖平面匹配對間的一元約束與二元約束結(jié)果都用索引表進(jìn)行記錄，僅出現(xiàn)在當(dāng)前幀中的平面與其他平面的約束判別結(jié)果會進(jìn)行計(jì)算并在索引表中更新.

另外，由于目標(biāo)地圖已知正確的豎直方向，所以在選取錯(cuò)誤主方向?yàn)楫?dāng)前子圖豎直方向時(shí)，水平平面的匹配對總是誤匹配對.所以在實(shí)際運(yùn)行搜索樹匹配時(shí)，本文要求每組匹配對結(jié)果都將一對水平平面匹配對作為根節(jié)點(diǎn)，從而使錯(cuò)誤豎直方向下的匹配從誤匹配對開始執(zhí)行，令匹配對數(shù)量更少，更快得到結(jié)果并確定豎直方向的選取是否正確.

每一組假想豎直方向下的匹配過程見算法2.匹配過程采取當(dāng)前地圖中子圖的平面集合法Lc、目標(biāo)地圖中子圖的平面集合LT需包含的水平匹配對L include作為輸入，對子圖中平面特征間是否滿足一元約束與二元約束進(jìn)行比對.3組解釋樹匹配中，具有最多匹配對，且其數(shù)量超過th的匹配將被選定為識別匹配結(jié)果，其豎直方向也將作為候選豎直方向用于后續(xù)定位.

2.1.3 子圖配準(zhǔn)與定位

在得到候選匹配對與候選豎直方向后，當(dāng)前子圖的候選豎直方向與目標(biāo)子圖的豎直方向就可以進(jìn)行對齊.根據(jù)水平平面匹配對的法向信息，可以確定豎直軸的指向，即如圖3所示的橫滾角與俯仰角可以確定.于是匹配就可以從3軸位置坐標(biāo)、橫滾轉(zhuǎn)向、偏航轉(zhuǎn)向及俯仰轉(zhuǎn)向未知的6自由度問題降為4自由度問題，其計(jì)算流程如圖4所示，其中對子圖匹配程度的評分S為

計(jì)算流程，首先將解釋樹搜索中得到的平面匹配對作為初始時(shí)的平面匹配對，計(jì)算相應(yīng)的位姿變換矩陣與匹配得分，進(jìn)而在目標(biāo)子圖中查找與當(dāng)前子圖中每一平面P：最接近的匹配平面Pv.目標(biāo)子圖中符合以下情況的平面會被選定為Pv的匹配對，成為子圖新增的平面匹配對：①平面法線方向與當(dāng)前子圖中P相同;②平面輪廓與當(dāng)前子圖中P在法向投影平面上有重疊;③平面與當(dāng)前子圖中puc法向距離最近.

在每次迭代的匹配度評定中，不僅解釋樹算法中得到的平面匹配對被納為考量目標(biāo)，迭代中新增的平面匹配對也同步地加入至匹配對集合中.兩子圖的匹配程度計(jì)算從平面重疊面積占比、色度相似程度、距離等3個(gè)角度進(jìn)行評定.公式（8）中，a intet（puc，Pu）代表Pu與Pu在法向投影平面上的重疊面積，Du，v表示兩平面的法向距離，而a為控制距離影響因素的參數(shù)，在實(shí)驗(yàn)中取為6.公式（8）的計(jì)算最終得到一個(gè)歸一化的得分，作為匹配評估得分.

2.2 行進(jìn)路徑的后續(xù)定位

在傳統(tǒng)的基于點(diǎn)特征的定位方法中，通常將位姿計(jì)算統(tǒng)一看作為6自由度的定位問題.由于點(diǎn)特征無法揭示環(huán)境結(jié)構(gòu)信息，即使前一時(shí)刻的成功定位可以確定建筑結(jié)構(gòu)坐標(biāo)軸主方向，但VO（Value Object）的誤差同樣會造成當(dāng)前幀建筑結(jié)構(gòu)主方向與前一時(shí)刻存在偏差，隨著時(shí)間推移，誤差將持續(xù)累加甚至引起地圖構(gòu)建錯(cuò)誤.然而，由于從平面特征中能夠解讀環(huán)境的結(jié)構(gòu)與法向信息，在行進(jìn)路徑中的每一時(shí)刻都能從小范圍鄰域內(nèi)的平面特征中提取出MW環(huán)境結(jié)構(gòu)的主方向.

若行進(jìn)路徑中兩相鄰幀如圖5所示，前一幀經(jīng)過成功定位后已知豎直方向?yàn)樽鴺?biāo)系軸X軸，而后一幀子圖鄰域內(nèi)計(jì)算得到的曼哈頓主方向坐標(biāo)系如圖.經(jīng)過VO將兩幀畫面配準(zhǔn)過后，根據(jù)位置關(guān)系能夠確定后一幀的主方向Z軸為當(dāng)前時(shí)刻的豎直方向.即使軌跡中存在的誤差使得前一幀X軸與后一幀Z軸存在偏差，但每一時(shí)刻的豎直方向都是由當(dāng)時(shí)鄰域范圍的特征計(jì)算得到的，因此豎直方向免受累積誤差影響，只要VO相鄰兩幀的位姿計(jì)算成功，偏差在置信范圍以內(nèi)，就能從前時(shí)刻的豎直方向判斷出當(dāng)前時(shí)刻的豎直方向所在;而VO失效時(shí)則重新運(yùn)行6自由度初始定位.

在獲悉軌跡中后續(xù)每一時(shí)刻的豎直方向指向后，定位過程就擁有更多條件信息，從6自由度定位簡化為4自由度定位問題.值得注意的是，問題的簡化并不受限于定位算法的選擇，在平面、線條特征地圖構(gòu)建的過程中加入曼哈頓主方向計(jì)算即可實(shí)現(xiàn)簡化.具體表現(xiàn)為第2.1.2所介紹的識別過程，僅需一組解釋樹匹配搜索過程，根據(jù)豎直方向參考排除的平面匹配對也降低了子圖識別耗時(shí).當(dāng)VO連續(xù)正常運(yùn)作時(shí)，一條行進(jìn)路徑上的定位平均耗時(shí)將接近4自由度定位的耗時(shí)水平.

3 實(shí)驗(yàn)評估

為了評估本文定位方法在實(shí)際應(yīng)用中的效果，這一節(jié)給出了場景實(shí)驗(yàn)與結(jié)果分析.

實(shí)驗(yàn)以微軟Kinect 1攝像頭為輸入設(shè)備，并采用搭載2.5 GHz Intel i5的計(jì)算機(jī)作為實(shí)驗(yàn)運(yùn)行設(shè)備.

由于現(xiàn)有的公開SLAM數(shù)據(jù)集場景軌跡與平面要素較少，本文采用現(xiàn)實(shí)生活中的10個(gè)房間作為實(shí)驗(yàn)場所.對每一房間，都采集20條不同的運(yùn)動軌跡所捕捉到的RGB-D圖像，以測試基于特征定位方式從不同位姿識別場景的能力.

表1展示了采用不同鄰近閾值的PbMap定位方法與本文定位方法在陌生場景下的初始時(shí)刻6自由度的定位表現(xiàn).由表1可知，單一子圖搜索的平均耗時(shí)與每幀子圖搜索的平均耗時(shí)（表1中左起第5列、第6列），PbMap定位平均每幀執(zhí)行約2輪子圖搜索.這代表著在實(shí)驗(yàn)中，平均每幀對第二個(gè)捕捉到的平面執(zhí)行子圖搜索后才能定位成功，這也造成了重復(fù)的計(jì)算開銷.隨著采用更大范圍的鄰近閾值以構(gòu)成更大視野范圍的子圖，定位所需路徑長度逐漸縮短，而搜索耗時(shí)與總耗時(shí)也在相應(yīng)地以指數(shù)級增長.子圖匹配耗時(shí)與子圖內(nèi)特征匹配對數(shù)量的關(guān)系如圖6所示.在解釋樹子圖匹配過程中得到匹配對數(shù)量相同的情況下，匹配耗時(shí)隨子圖內(nèi)特征匹配對數(shù)量呈指數(shù)級增長;而在子圖內(nèi)特征匹配對數(shù)相當(dāng)?shù)那闆r下，得到匹配對更多的解釋樹匹配過程相對耗時(shí)更高.這使得平面定位無法完全利用完整視野范圍內(nèi)的所有特征構(gòu)造子圖進(jìn)行匹配，只能選擇較小的子圖結(jié)構(gòu)，在犧牲定位表現(xiàn)的前提下達(dá)到實(shí)時(shí)運(yùn)行.

本文所采用的平面定位策略由于將每幀捕捉到的所有平面置入一個(gè)大的子圖，鄰近閾值僅作為連接平面特征之用，識別過程所匹配的子圖大小規(guī)模主要由視覺探測設(shè)備的視野范圍所決定.所以，這即等效于PbMap的鄰近閾值取值為略小于探測設(shè)備視距的浮動值.在本次實(shí)驗(yàn)中，子圖的大小范圍平均約為3.71 m，并與鄰近閾值分別為1m、2m、4m時(shí)的PbMap解釋樹匹配定位進(jìn)行比較.在與4m級鄰近閡值的PbMap定位過程具有相當(dāng)?shù)淖訄D內(nèi)特征數(shù)量時(shí)，其將解釋樹搜索過程分拆為3組不同豎直方向選取下的解釋樹搜索，匹配耗時(shí)隨特征數(shù)量呈指數(shù)級增長，含少量特征的小組匹配方案可有效提升搜索速度，在3組搜索總特征匹配對數(shù)增多的情況下具有更短的搜索耗時(shí).另外，由于每幀不再需要迭代搜索不同平面的子圖，不同子圖結(jié)構(gòu)的多次匹配搜索也可節(jié)省至每幀一次，從而使得子圖結(jié)構(gòu)的匹配識別利用視野內(nèi)更廣范圍的同時(shí)提高了運(yùn)算效率.同時(shí)在后續(xù)的識別定位中，由于初始位姿確定后的豎直方向參考信息，識別定位的時(shí)間開銷又進(jìn)一步得到削減.定位軌跡所有時(shí)刻的平均定位數(shù)據(jù)如表2所示.本文提出方法中的最大定位耗時(shí)即為初始定位時(shí)的最大耗時(shí).在每幀的子圖匹配中引入更多特征后，定位的精度與成功率都會隨之提高，而隨著行進(jìn)距離的持續(xù)增加，定位耗時(shí)趨近于后續(xù)4自由度定位耗時(shí)，降低了整條路徑上的平均定位耗時(shí).

實(shí)驗(yàn)中，劃定子圖的鄰近閾值th與th取值為Im與4m，子圖匹配中的th sixc-uppcr和th area-upper、th size-lower和th area-lower分別設(shè)定為3和1/3，th dist為O.lm，而th angle為7度.平面色度差閡值th hue為0.3，而識別匹配對閾值thn取為4.需要注意的是，結(jié)構(gòu)匹配過程中約束的閾值設(shè)定并不嚴(yán)格受限，可根據(jù)采集設(shè)備精度、地圖構(gòu)建精度、環(huán)境結(jié)構(gòu)約束的符合程度而變化.

4 結(jié)論

隨著基于高層次特征SLAM的發(fā)展，本文旨在為SLAM系統(tǒng)提供基于平面特征的定位系統(tǒng)模塊.將曼哈頓世界假說引入平面定位過程，并以PbMap定位方法為基礎(chǔ)提出了改進(jìn)方案：采用大范圍子圖進(jìn)行特征匹配以排除單幀內(nèi)反復(fù)執(zhí)行的子圖識別;利用曼哈頓世界假說模型分解初始位姿計(jì)算的解釋樹匹配，以降低大范圍子圖的搜索時(shí)間開銷，并將行進(jìn)路徑上的后續(xù)定位簡化為4自由度的定位問題.本文方案更充分地利用視野內(nèi)的更多特征，能更早地得到定位結(jié)果并削減搜索耗時(shí)，在定位效果與運(yùn)行效率上都實(shí)現(xiàn)了一定程度的提升.實(shí)驗(yàn)顯示，大范圍子圖結(jié)構(gòu)使定位所需軌跡距離更短，而曼哈頓世界假說的引入有效地提高了運(yùn)算效率，能完整地利用現(xiàn)有RGB-D采集設(shè)備的視野范圍，并更好地支持實(shí)時(shí)應(yīng)用.

[參考文獻(xiàn)]

[1] GLOCKER B，SHOTTON J， CRIMINISI A，et al_Real-time RGB-D camera relocalization via randomized fernsfor keyframe encoding [J]. IEEE transactions on visualization and computer graphics， 2015， 21（5）： 571-583

[2]SE s，LOWE D G，LITTLE J J.Vision-based global localization and mapping for mobile robots [J]. IEEETransactions on robotics， 2005， 21（3）： 364-375.

[3] MUR-ARTAL R，TARDOS J D.Orb-slam2： An open-source slam system for monocular， stereo， and rgb-deras[J].IEEE Transactions on Robotics， 2017， 33（5）： 1255-1262.

[4]SALAS-MORENO R F. NEWCOMBE R A， STRASDAT H， et al. Slam++： Simultaneous localisation and map-ping at the level of objects [C]//Proceedings of the IEEE conference on computer vision and pattern recognition.2013： 1352-1359.

[5] SHI Y， XU K， NIE13NER M， et al. Planematch： Patch coplanarity prediction for robust rgb-d reconstruction[C]//Proceedings of the European Conference on Computer Vision （ECCV）. 2018： 750-766.

[6]FORSTNER W， KHOSHELHAM K. Efficient and accurate registration of point clouds with plane to planecorrespondences [C]//Proceedings of the IEEE International Conference on Computer Vision. 2017： 2165-2173.

[7]TAGUCHI Y. JIAN Y D， RAMALINGAM S， et al. Point-plane SLAM for hand-held 3D sensors [C]//2013 IEEEInternational Conference on Robotics and Automation. IEEE， 2013： 5182-5189.

[8]FERNANDEZ-MORAL E， RIVES P， AREVALO V， et al. Scene structure registration for localization andmapping [J]. Robotics and Autonomous Systems， 2016， 75： 649-660.

[9]SALAS-MORENO R F， GLOCKEN B， KELLY P H J， et al. Dense planar SLAM [C]//2014 IEEE InternationalSymposium on Mixed and Augmented Reality （ISMAR）. IEEE： 2014： 157-164.

[10] HSIAO M， WESTMAN E， ZHANG G， ET AL. Keyframe-based dense planar SLAM [C]//2017 IEEE Interna-tional Conference on Robotics and Automation （ICRA）. IEEE， 2017： 5110-5117.

[11]MA L， KERL C. STUCKLER J， et al. CPA-SLAM： Consistent plane-model alignment for direct RGB-D SLAM[C]//2016 IEEE International Conference on Robotics and Automation （ICRA）. IEEE， 2016： 1285-1291.

[12]CHO H G， YEON S， CHOI H， et al. Detection and compensation of degeneracy cases for IMU-kinect integrated continuous SLAM with plane features [J]. Sensors， 2018， 18（4）： 935（9pages）.

[13]COUGHLAN J M： YUILLE A L. Manhattan world： Compass direction from a single image by bayesian inference[C]//Proceedings of the Seventh IEEE International Conference on Computer Vision. IEEE， 1999， 2： 941-947.

[14]SCHINDLER G， DELLAERT F. Atlanta world： An expectation maximization framework for simultaneous low-level edge grouping and camera calibration in complex man-made environments [C]//Proceedings of the 2004IEEE Computer Society Conference on Computer Vision and Pattern Recognition （CVPR'04）. IEEE： 2004， 1：I-L.

[15] STRAUB J， ROSMAN G， FREIFELD 0， et al. A mixture of manhattan frames： Beyond the manhattan world[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2014： 3770-3777.

[16]RUSU R B， Cousins S. 3D is here： Point Cloud Library （PCL） [C]// IEEE International Conference on Robotics & Automation. 2011： 1-4.

[17] GRIMSON W E L， LOZANO-PEREZ T. Localizing overlapping parts by searching the interpretation tree [J].IEEE Transactions on Pattern Analysis and Machine Intelligence： 1987， 9（4）： 469-482.