基于圖優(yōu)化的Kinect三維視覺里程計(jì)設(shè)計(jì)

張兆博， 伍新華， 劉 剛

(武漢理工大學(xué) 計(jì)算機(jī)學(xué)院，湖北 武漢430000)

0 引 言

里程計(jì)作為同步定位和地圖構(gòu)建(simultaneous localization and mapping,SLAM)[1,2]的關(guān)鍵部分，近年來逐漸成為機(jī)器人領(lǐng)域的熱點(diǎn)研究問題，而SLAM更被認(rèn)為是實(shí)現(xiàn)移動(dòng)機(jī)器人自主化的核心技術(shù)。視覺里程計(jì)(visual odometer)一詞源于Nister D等人[3]在2004年的一篇里程碑式的文獻(xiàn)，旨在通過分析視覺信息，來獲取運(yùn)動(dòng)的軌跡信息，是通過視覺傳感器作為機(jī)器人“雙眼”的基礎(chǔ)。

視覺里程計(jì)根據(jù)計(jì)算運(yùn)動(dòng)軌跡所使用的數(shù)據(jù)傳感器類型，可以分為單目里程計(jì)、雙目里程計(jì)、以及RGB—D里程計(jì)。2010年下半年，微軟推出了可以直接獲取景深信息的RGB—D相機(jī)Kinect，再加上其大眾化的價(jià)格，深受眾學(xué)者的喜愛。

早期Bailey T等人[4]使用擴(kuò)展卡爾曼濾波算法來求視覺里程計(jì)。文獻(xiàn)[5～7]介紹了利用濾波求解視覺里程計(jì)的典型方法。這類算法對于運(yùn)動(dòng)時(shí)間較短的小場景可以取得較好的結(jié)果，但大規(guī)模場景的里程計(jì)會(huì)隨著誤差的累積產(chǎn)生較大的偏差。

Martinez Henry P等人[8]在2012年最先提出使用RGB—D相機(jī)求解視覺里程計(jì)，并提出一種TORO算法用來對相機(jī)位姿進(jìn)行優(yōu)化。該類算法雖起步較晚，但卻在近年來取得了不俗的成果[9～11]。Nikolas Engel Hard等人[12]在2012年實(shí)現(xiàn)了一種手持RGB-D里程計(jì)系統(tǒng)并進(jìn)行了改進(jìn)。Engel J等人[13]在2014年提出的LSD—SLAM中首次引入“關(guān)鍵幀”的概念，即通過選取有代表性的幀來求解視覺里程計(jì)。Mur-Artal R等人[14]在2015首次將優(yōu)化的過程運(yùn)行在線程上，提高了系統(tǒng)的性能。

本文使用Kinect作為視覺傳感器，進(jìn)行視覺里程計(jì)的計(jì)算。本文在Henry算法的基礎(chǔ)上，充分結(jié)合Kinect相機(jī)易于獲取深度圖像這一特點(diǎn)，提出一種新的基于深度圖像匹配的關(guān)鍵幀選取算法，通過在關(guān)鍵幀上構(gòu)建帶環(huán)圖模型，對視覺里程計(jì)結(jié)果進(jìn)行局部和全局優(yōu)化。實(shí)驗(yàn)結(jié)果表明，相比較Henry P等人的方法，本文方法在減小里程計(jì)相對誤差上效果顯著，這對以視覺里程計(jì)為基礎(chǔ)，重建三維場景的應(yīng)用中具有重要的作用[15，16]。

1 基于深度圖像的關(guān)鍵幀選取

Kinect攝像機(jī)的幀率一般在30 Hz左右，計(jì)算每一幀的視覺里程計(jì)是不可取的。因?yàn)楫?dāng)幀與幀之間距離過近時(shí)，相當(dāng)于沒有移動(dòng)；過遠(yuǎn)時(shí)，則無法進(jìn)行計(jì)算。因此，只用當(dāng)攝像機(jī)的運(yùn)動(dòng)在一定范圍內(nèi)時(shí)，計(jì)算才有意義，這樣的幀定義為“關(guān)鍵幀”。本文根據(jù)Kinect攝像機(jī)易于獲取深度信息這一特點(diǎn)，利用光流法匹配幀間位姿，利用閾值來篩選關(guān)鍵幀。

假設(shè)3維空間中一點(diǎn)X=[x,y,z,1]T，在時(shí)刻t與時(shí)刻t+1之間的剛體運(yùn)動(dòng)為

(1)

式中R3×3為3個(gè)自由度的旋轉(zhuǎn)矩陣，t3×3為3維平移向量。變換矩陣T可以表示為

(2)

式中T為圖像幀之間的位置變換，T又稱為幀間位姿；[ωx,ωy,ωz]為相對于x,y,z軸的旋轉(zhuǎn)；[tx,ty,tz]為相對于x,y,z軸的平移。連續(xù)的變換矩陣T即視覺里程計(jì)。根據(jù)透視投影，x(t+1)于t+1時(shí)刻在深度圖像上的投影點(diǎn)為

(3)

式中fL為攝像機(jī)的焦距。求得投影點(diǎn)x(t+1)相對于μ=[ωx,ωy,ωz,tx,ty,tz]的導(dǎo)數(shù)矩陣Fμ,即

(4)

以上導(dǎo)數(shù)可反映像素點(diǎn)根據(jù)Fμ在深度圖像上的位置變化。點(diǎn)X在時(shí)刻t的深度值為d，根據(jù)LK方法，存在

d(F(x,μ),t+1)=d(x,t)

(5)

即在一定的范圍內(nèi)對于三維空間點(diǎn)X在相鄰時(shí)刻的深度值恒定。求解μ，即是找到一個(gè)μ使得最小二乘誤差最小，即

(6)

式中Ω為t時(shí)刻的模板區(qū)域，只有在Ω內(nèi)部的像素點(diǎn)才考慮運(yùn)動(dòng)估計(jì)。因?yàn)樯疃葓D像保存了場景的景深信息，可以通過簡單的二值化分離出背景，非背景區(qū)域即為Ω模板區(qū)域。

假設(shè)運(yùn)動(dòng)為μ+Δμ，則誤差為

t+1)-d(x,t))

(7)

根據(jù)泰勒展開式

(8)

計(jì)算μ是一個(gè)迭代過程。在每次迭代中計(jì)算運(yùn)動(dòng)增量Δμ。然后，模板根據(jù)運(yùn)動(dòng)增量Δμ進(jìn)行變換，變換結(jié)果用于下一次迭代。當(dāng)Δμ收斂后迭代過程結(jié)束，μ通過一系列Δμ變換組合得到。μ經(jīng)過矩陣變換得到幀間位姿T。

對于運(yùn)動(dòng)矩陣μ=[ωx,ωy,ωz,tx,ty,tz]，數(shù)值Lμ表示運(yùn)動(dòng)的大小，Lμ如下

(9)

利用閾值法對Lμ進(jìn)行定義

(10)

式中 過小表示兩圖像幀間距很近，沒有必要都進(jìn)行保留；過大則表示兩幀間距很遠(yuǎn)，缺乏關(guān)聯(lián)性；適中則能保證兩幀的間隔性與關(guān)聯(lián)性，并且新的圖像幀會(huì)被加入到關(guān)鍵幀序列中去。其中k1和k2的取值通過實(shí)驗(yàn)獲得，閾值法的好處在于可以靈活地調(diào)整閾值來保證結(jié)果的正確性，針對一些復(fù)雜多變的環(huán)境是非常有效的。

2 基于圖的里程計(jì)優(yōu)化

計(jì)算視覺里程計(jì)實(shí)質(zhì)是計(jì)算幀序列的幀間位姿序列，Tij表示第i幀與第j幀的位姿變換。引入圖G={V,E}構(gòu)建優(yōu)化模型，E為頂點(diǎn)表示相機(jī)的位姿

(11)

式中V為邊表示頂點(diǎn)間的變換或約束

(12)

未進(jìn)行優(yōu)化的視覺里程計(jì)的圖結(jié)構(gòu)如圖1所示。

圖1 不帶路標(biāo)的圖結(jié)構(gòu)

本文在上述鏈?zhǔn)浇Y(jié)構(gòu)的基礎(chǔ)上，通過上文的關(guān)鍵幀算法，在局部添加局部回環(huán)以及隨機(jī)回環(huán)，進(jìn)而構(gòu)造不含路標(biāo)頂點(diǎn)的因子圖。具體流程如下：

1)初始化關(guān)鍵幀的序列F，并將第一幀f0放入F。

2)對于新來的一幀fnew，利用關(guān)鍵幀提取算法計(jì)算F中最后一幀與fnew的幀間位姿T，并估計(jì)該T的大小Lμ。有以下幾種可能：

a.過小，說明離前一個(gè)關(guān)鍵幀很近，丟棄該幀；

b.過大，說明運(yùn)動(dòng)太大，可能是距離太遠(yuǎn)或計(jì)算錯(cuò)誤，同樣丟棄該幀；

c.適中，運(yùn)動(dòng)估計(jì)正確，同時(shí)又離上一個(gè)關(guān)鍵幀有一定距離，則把fnew作為新的關(guān)鍵幀，進(jìn)入回環(huán)檢測程序。

3)局部回環(huán)檢測：匹配fnew與F末尾n1個(gè)關(guān)鍵幀。匹配成功時(shí)，在圖里新增一條約束邊。

4)隨機(jī)回環(huán)：隨機(jī)在F里取n2個(gè)幀，與fnew進(jìn)行匹配。匹配成功時(shí)，在圖里新增一條邊。一般取n2=2n1，這樣取值的好處在于盡可能多地讓新幀與前面的幀建立約束邊，同時(shí)保證不會(huì)因?yàn)槟骋粠膩G失導(dǎo)致圖鏈的斷裂。

5)將fnew放入F末尾。若有新的數(shù)據(jù)，則回步驟(2)；若無，則圖構(gòu)造完成。取n1=3，n2=6時(shí)圖的結(jié)構(gòu)如圖2所示。

圖2 本文算法構(gòu)造的圖結(jié)構(gòu)

當(dāng)圖結(jié)構(gòu)是存在回環(huán)的非鏈狀結(jié)構(gòu)時(shí)，由于邊Tij中存在誤差，使得所有邊給出的數(shù)據(jù)不一致，此時(shí)，優(yōu)化一個(gè)不一致性誤差

(13)

(14)

(15)

式中Jij為誤差函數(shù)eij(x*)在x*附近的雅克比矩陣。將式(15)代入式(14)的某一項(xiàng)并展開

F(x*+Δx)=C+2bΔx+ΔxTHΔx

(16)

H=∑Hij為Hession矩陣的累加。為使式(16)最小，使其一階導(dǎo)數(shù)等于0，得

HΔx=-b

(17)

至此，將優(yōu)化問題進(jìn)一步轉(zhuǎn)化為線性方程組求解問題。

求解的過程通過高斯—牛頓迭代法完成。首先將關(guān)鍵幀匹配求得的每一幀的位姿向量[ωx,ωy,ωz,tx,ty,tz]作為圖中每個(gè)頂點(diǎn)x的初始值x*，求得的Δx疊加x*作為新的初始值，重復(fù)執(zhí)行該過程直到x*收斂，此時(shí)x*為優(yōu)化后的結(jié)果。

3 實(shí)驗(yàn)與結(jié)果分析

為了對算法進(jìn)行評估，本文利用Computer Vision Group提供的RGB-D數(shù)據(jù)集進(jìn)行試驗(yàn)，該數(shù)據(jù)集主要包含2個(gè)部分：Kinect視頻序列和真實(shí)的里程計(jì)。

3.1 算法性能評估

算法性能主要包含2方面：算法的精度和算法的實(shí)時(shí)性。算法的精度由求得的視覺里程計(jì)與真實(shí)里程計(jì)的誤差決定，算法的實(shí)時(shí)性由單幀里程計(jì)平均用時(shí)決定。圖3給出了視覺里程計(jì)在2維空間的投影結(jié)果。

圖3 不同算法估計(jì)相機(jī)運(yùn)動(dòng)2D軌跡

圖4(a)為不同算法在具體標(biāo)準(zhǔn)誤差上的比較，絕對軌跡誤差 (absolute trajectory error,ATE)用來衡量算法估計(jì)軌跡與真實(shí)軌跡的誤差；相對姿態(tài)誤差 (relative pose error,RPE)用來衡量位姿之間存在的誤差積累程度。圖4(b)為不同算法在耗費(fèi)時(shí)間上的比較?？梢钥闯?，相比較傳統(tǒng)算法，本文算法在不影響實(shí)時(shí)性的前提下，在絕對軌跡誤差和相對姿態(tài)誤差上，取得了較為顯著的提高。

圖4 性能對比

3.2 三維點(diǎn)云圖

為進(jìn)一步驗(yàn)證算法性能，以求得的視覺里程計(jì)為基礎(chǔ)，實(shí)現(xiàn)三維場景的重構(gòu)，通過點(diǎn)云圖呈現(xiàn)。當(dāng)然由于Kinect相機(jī)拍攝距離的局限性，對于過遠(yuǎn)的場景，由于無法得到其圖像，所以沒辦法進(jìn)行拼接還原。如圖5所示。

圖5 數(shù)據(jù)集的三維點(diǎn)云模型

3.3 真實(shí)室內(nèi)環(huán)境實(shí)驗(yàn)

為了進(jìn)一步通過實(shí)驗(yàn)檢驗(yàn)算法，通過手持Kinect，在室內(nèi)緩慢繞行1周，收集室內(nèi)場景信息。Kinect的幀率為30 Hz，分辨率大小640×480，共收集1 800幀RGB-D圖像，最終選取了230個(gè)關(guān)鍵幀。由于當(dāng)物體離Kinect較遠(yuǎn)時(shí)，測量的深度信息可能不準(zhǔn)確，設(shè)定只保留7 m以內(nèi)的點(diǎn)。點(diǎn)云0.05 m，最小距離k1設(shè)為0.8，k2設(shè)為2，相鄰檢測幀數(shù)n1設(shè)為4，隨機(jī)檢測幀數(shù)n2設(shè)為8。實(shí)驗(yàn)建立的室內(nèi)三維場景如圖6所示。左圖為整體效果，右圖是對局部區(qū)域放大后的顯示效果，從該圖中可以看出，對于拐角、轉(zhuǎn)彎等易于產(chǎn)生較大誤差的地方，本文算法都實(shí)現(xiàn)了較好的優(yōu)化。

圖6 真實(shí)室內(nèi)環(huán)境的三維點(diǎn)云模型

4 結(jié)束語

Kinect相機(jī)因其能夠提供深度信息的優(yōu)勢在計(jì)算機(jī)視覺領(lǐng)域具有廣闊的應(yīng)用前景。本文以Kinect為傳感器，實(shí)現(xiàn)基于圖優(yōu)化的視覺里程計(jì)設(shè)計(jì)。該方法充分利用了Kinect易于獲得深度信息的特點(diǎn)，提出了一種新的基于深度圖像的關(guān)鍵幀選取算法，并通過在關(guān)鍵幀添加局部和隨機(jī)回環(huán)構(gòu)建圖，在圖上用非線性最小二乘方法實(shí)現(xiàn)視覺里程計(jì)的優(yōu)化。實(shí)驗(yàn)結(jié)果表明：運(yùn)用本文方法，在不影響實(shí)時(shí)性的同時(shí)，有效減小了誤差，得到了更為精確的視覺里程計(jì)結(jié)果。