陳財(cái)富， 汪 雙， 陳 波， 張 華， 王 姮

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽(yáng) 621000; 2.西南科技大學(xué) 特殊環(huán)境機(jī)器人四川省重點(diǎn)實(shí)驗(yàn)室，四川 綿陽(yáng) 621010)

0 引 言

港科大開(kāi)源的VINS-Mono[1]是視覺(jué)慣性里程計(jì)(visual inertial odometer,VIO)[2]系統(tǒng)，它的前端主要包括均值化預(yù)處理[3]限制對(duì)比度的自適應(yīng)性直方圖均衡化(CLAHE)、Shi-Tomasi角點(diǎn)特征提取和光流跟蹤[4]。雖然VIO系統(tǒng)前端適用性強(qiáng)以及魯棒性高，但它計(jì)算成本較高且計(jì)算耗時(shí)不穩(wěn)定，在要求十分苛刻的虛擬現(xiàn)實(shí)(VR)/增強(qiáng)現(xiàn)實(shí)(AR)和無(wú)人機(jī)機(jī)器人平臺(tái)上實(shí)時(shí)運(yùn)行困難。針對(duì)上述系統(tǒng)實(shí)時(shí)性的問(wèn)題，考慮尺寸重量大小滿足應(yīng)用要求，本文選用較小的尺寸和重量中央處理器(CPU)-圖像處理器(GPU)組合的NVIDIA TX2處理器，TX2不僅在成本上有一定的經(jīng)濟(jì)>效益，而且在GPU加速處理非常出色[5,6]。同時(shí)本文改進(jìn)VINS-Mono前端，提出了一種基于GPU并行加速VIO方法，采用FAST特征提取和特征點(diǎn)光流跟蹤進(jìn)行并行加速，以獲得更高實(shí)時(shí)且耗時(shí)穩(wěn)定的VIO前端；并且對(duì)文獻(xiàn)[7]特征提取查表方式進(jìn)行了改進(jìn)，占用更少的內(nèi)存空間。本文方法還在特征提取之前通過(guò)GPU對(duì)CLAHE加速，在特征提取后采用非極大值抑制[8]，消除周圍非極大值元素，使特征點(diǎn)分布相對(duì)均勻。最后,采用多層金字塔平均強(qiáng)度反向光流法進(jìn)行角點(diǎn)跟蹤，求解特征點(diǎn)匹配。實(shí)驗(yàn)部分,本文將改善后的前端系統(tǒng)與VINS-Mono后端結(jié)合，選用EuROC數(shù)據(jù)集[9]，在嵌入式CPU-GPU異構(gòu)平臺(tái)TX2上進(jìn)行了全方面的評(píng)估。

1 總體系統(tǒng)設(shè)計(jì)

本文提出的GPU并行加速VIO前端方法包括圖像灰度化、CLAHE、FAST特征點(diǎn)提取并進(jìn)行非極大值抑制、FAST特征點(diǎn)光流跟蹤以及特征點(diǎn)跟蹤誤差剔除，再結(jié)合VINS-Mono中IMU處理部分和后端部分構(gòu)成一個(gè)完整的即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)系統(tǒng)。整個(gè)系統(tǒng)框圖如圖1所示。其中，自適應(yīng)直方圖均值化、特征點(diǎn)提取、特征點(diǎn)跟蹤都通過(guò)GPU進(jìn)行并行加速運(yùn)算，其他都將在CPU下進(jìn)行處理。

圖1 總體系統(tǒng)框圖

2 方法設(shè)計(jì)

2.1 GPU并行處理

英偉達(dá)(NVIDIA)公司的GPU架構(gòu)是由可擴(kuò)展的多線程多處理陣列構(gòu)建[10]。每一個(gè)多處理陣列又被劃分為若干個(gè)流處理器，每一個(gè)流處理器又包含若干個(gè)處理器。為了更容易進(jìn)行GPU并行處理開(kāi)發(fā)，采用NVIDIA公司提供的CUDA并行計(jì)算框架，它包含重要的指令集架構(gòu)、層次化線性模型以及并行的計(jì)算引擎。CUDA編程模型包含以下層次結(jié)構(gòu)：線程、線程塊、塊網(wǎng)格以及線程Kernel。一個(gè)并行計(jì)算有多個(gè)Kernel，一個(gè)Kernel處理一個(gè)塊網(wǎng)格，一個(gè)塊網(wǎng)格由若干個(gè)線程塊組成，一個(gè)線程塊又由若干個(gè)線程構(gòu)成，線程與線程之間并行運(yùn)行[11]。整個(gè)系統(tǒng)的運(yùn)行采用CPU與GPU串并結(jié)合協(xié)同工作、各司其職的工作方式，如圖2所示。

圖2 CPU-GPU異構(gòu)模型

2.2 CLAHE并行化設(shè)計(jì)

CLAHE的核心思想是先對(duì)圖像均勻分成等份矩形，再對(duì)每部分矩形區(qū)域求解累計(jì)分布直方圖并進(jìn)行對(duì)比度限幅，最后求解每個(gè)區(qū)域塊的變換函數(shù)對(duì)每一個(gè)像素點(diǎn)進(jìn)行像素值轉(zhuǎn)換，對(duì)于相鄰區(qū)域的像素值需進(jìn)行插值操作，以消除塊狀效應(yīng)。并行化設(shè)計(jì)具體過(guò)程如下：

1)圖像分塊：將圖像劃分成等大小的矩形區(qū)域塊，本文劃分為8×8的區(qū)域塊。

2)計(jì)算每一個(gè)區(qū)域塊的分布直方圖

(1)

3)對(duì)每一個(gè)區(qū)域塊分布直方圖進(jìn)行對(duì)比度限幅，并調(diào)整像素點(diǎn)

(2)

4)計(jì)算第i,j區(qū)域塊下直方圖累計(jì)變換函數(shù)

(3)

式中Wij，Hij為第i,j區(qū)域塊的寬度和長(zhǎng)度。

5)根據(jù)整個(gè)圖像每一個(gè)像素點(diǎn)在不同區(qū)域的分布情況，執(zhí)行不同的像素點(diǎn)映射(直接映射，線性插值映射，雙線性插值映射)，本文直接采用插值的方式，有效防止if else指令的發(fā)散，計(jì)算如下：

a.確定某個(gè)像素點(diǎn)(m,n)相鄰的4個(gè)映射函數(shù)si0j0,si0j1，si1j0，si1j1,如果在邊界不滿足映射函數(shù)則按照取最近的映射函數(shù)操作

(4)

b.確定不同映射函數(shù)的占比

(5)

c.對(duì)像素點(diǎn)進(jìn)行映射

si1j0(Imn)×(1-α)×β+si1j1(Imn)×(1-α)×(1-β)

(6)

6)GPU加速設(shè)計(jì)：GPU加速過(guò)程采用兩個(gè)核Kernel進(jìn)行，第一個(gè)Kernel_1處理上述過(guò)程(1)～(4)，分為64個(gè)線程分別求解每一個(gè)區(qū)域塊的映射函數(shù)；第二個(gè)Kernel_2處理上述過(guò)程(5)，分別對(duì)圖像中每一個(gè)像素點(diǎn)進(jìn)行映射插值操作，最后得到映射后的圖像。

2.3 FAST特征檢測(cè)并行化設(shè)計(jì)

FAST算法是一種檢測(cè)局部像素灰度變化明顯的特征檢測(cè)方法。本文在CPU-GPU異構(gòu)平臺(tái)上采用GPU并行設(shè)計(jì)實(shí)現(xiàn)FAST角點(diǎn)快速檢測(cè)，最后再通過(guò)GPU對(duì)檢出的角點(diǎn)進(jìn)行非極大值抑制加速處理。FAST特征檢測(cè)核心思想是一個(gè)像素與領(lǐng)域像素的光度強(qiáng)度差別較多或較大時(shí)，該像素點(diǎn)即為FAST角點(diǎn)。

本文特征檢測(cè)并行化設(shè)計(jì)過(guò)程如下：

1)對(duì)圖像遍歷取像素點(diǎn)(不包括邊界為3的像素點(diǎn))，設(shè)第i行第j列的像素點(diǎn)灰度值為Iij。

3)設(shè)置閾值T,并將像素點(diǎn)與其周圍16個(gè)像素點(diǎn)進(jìn)行比較，區(qū)分出更亮、更暗或者相似的像素點(diǎn)，并進(jìn)行標(biāo)記

(7)

4)對(duì)上述計(jì)算出的16個(gè)像素點(diǎn)得出的標(biāo)簽進(jìn)行分類，如果有連續(xù)的N個(gè)像素點(diǎn)的Ln=darker或者Ln=brighter，則像素點(diǎn)為Iij特征點(diǎn)。對(duì)于上述判斷連續(xù)N個(gè)點(diǎn)是更亮或者更暗的點(diǎn)，在CUDA編程處理的過(guò)程中if else指令將會(huì)頻繁調(diào)用，并且不同線程之間if else活動(dòng)范圍不一致，會(huì)降低GPU并行處理吞吐量，效率會(huì)下降。文獻(xiàn)[7]先通過(guò)計(jì)算16個(gè)像素點(diǎn)更亮和更暗值，再通過(guò)01二進(jìn)制的方式保存，第0個(gè)像素點(diǎn)對(duì)應(yīng)低位，第1個(gè)像素點(diǎn)對(duì)應(yīng)位加一，依此類推，將更亮和更暗的數(shù)據(jù)分別保存在216位并作為地址，最后，通過(guò)預(yù)先設(shè)計(jì)8 K字節(jié)查詢表，直接查詢是否是角點(diǎn)。而本文則是設(shè)計(jì)一個(gè)快速查詢角點(diǎn)的函數(shù)，節(jié)約空間，并且這個(gè)函數(shù)具有少量運(yùn)算指令、少量參數(shù)和無(wú)if else指令，通過(guò)輸入更亮和更暗的地址直接快速計(jì)算出是否為角點(diǎn)。計(jì)算更亮和更暗查詢地址(addr_brighter和addr_darker)

(8)

快速角點(diǎn)查詢函數(shù)設(shè)計(jì)

(9)

式中 Ξ&為連續(xù)求與符號(hào)，CN為連續(xù)N個(gè)角點(diǎn)查詢數(shù)據(jù)庫(kù)，共16個(gè)字節(jié)數(shù)據(jù)大小。如果F(Addr)=1，則為非角點(diǎn)；反之,F(Addr)=0,則為角點(diǎn)。最后,對(duì)每個(gè)像素點(diǎn)計(jì)算出角點(diǎn)得分，非角點(diǎn)為0，角點(diǎn)則根據(jù)像素差進(jìn)行計(jì)算。

5)進(jìn)行非極大值抑制，對(duì)圖像柵格化劃分為32×32等分進(jìn)行非極大值抑制篩選。

6)GPU加速設(shè)計(jì)：特征檢測(cè)過(guò)程GPU設(shè)計(jì)上需要分為兩個(gè)Kernel。第一個(gè)Kernel處理上述過(guò)程(1)～(4) ，通過(guò)快速角點(diǎn)提取函數(shù)對(duì)角點(diǎn)判斷，并執(zhí)行計(jì)算得分角點(diǎn)響應(yīng)得分，整個(gè)過(guò)程分為128個(gè)線程并行運(yùn)行，每一個(gè)線程平均處理圖像像素點(diǎn)。第二個(gè)Kernel處理上述過(guò)程(5)非線性抑制過(guò)程，先對(duì)圖片劃分32×32的單元格區(qū)域，線程分為32個(gè)，每一個(gè)線程處理一行單元格像素點(diǎn)，線程中并行執(zhí)行像素點(diǎn)抑制，并在全局共享內(nèi)存中保存在該單元格中響應(yīng)得分最大值。

2.4 光流跟蹤并行化設(shè)計(jì)

本文采用多層金字塔平均強(qiáng)度反向光流法作為特征跟蹤。多層光流有效克服局部最小值，同時(shí)平均強(qiáng)度能有效提高跟蹤效果，而反向光流則可以節(jié)省計(jì)算時(shí)間。本文光流跟蹤并行化設(shè)計(jì)過(guò)程如下：

1)設(shè)置金子塔層數(shù)，并設(shè)定跟蹤規(guī)則：在計(jì)算光流時(shí)，先從頂層的圖像跟蹤計(jì)算，再把跟蹤結(jié)果作為下一層光流跟蹤的初始值，直到最底層。

2)建立光流跟蹤：光流跟蹤其實(shí)是解決以下最小二乘問(wèn)題

(10)

(11)

式中I1(x),I2(x)分別為上幀圖片和當(dāng)前幀圖片某像素的光度強(qiáng)度。1,2分別為上一幀圖片和當(dāng)前幀圖片某像素在指定范圍內(nèi)像素點(diǎn)的平均光度強(qiáng)度

(12)

求解最小二乘問(wèn)題，得雅可比

(13)

求解像素位移增量

(14)

其中,H為海森矩陣

(15)

最后,更新位移量。

3)GPU加速設(shè)計(jì)：在這一過(guò)程中GPU并行加速運(yùn)行存在兩個(gè)主要的問(wèn)題：內(nèi)存合并和運(yùn)行發(fā)散。為了解決這些問(wèn)題，本文采用一個(gè)FAST角點(diǎn)跟蹤占用一個(gè)線程，并設(shè)定迭代相同的迭代次數(shù)克服運(yùn)行發(fā)散。整個(gè)并行過(guò)程只采用一個(gè)Kernel，并創(chuàng)建128的線程同時(shí)處理這些FAST角點(diǎn)，處理完成后將結(jié)果合并在一個(gè)內(nèi)存塊中，最后交給CPU處理。

3 實(shí)驗(yàn)驗(yàn)證分析

本文實(shí)驗(yàn)硬件平臺(tái)為NVIDIA嵌入式處理器TX2。TX2的GPU采用NVIDIA Pascal架構(gòu)，配有256個(gè)計(jì)算核心，算力高到6.2，它的CPU采用雙核Denver2 64位和4核ARM A57，內(nèi)存為8 GB 128位LPDDR4，TX2較小的尺寸和重量非常適用于無(wú)人機(jī)等輕負(fù)載機(jī)器人。本文實(shí)驗(yàn)分為兩部分：1)系統(tǒng)耗時(shí)對(duì)比；2)系統(tǒng)精度對(duì)比。本文評(píng)估的數(shù)據(jù)選用數(shù)據(jù)集EuROC Machine Hall(MH)序列集，該數(shù)據(jù)序列集包括2 273幀圖片。

3.1 系統(tǒng)耗時(shí)實(shí)驗(yàn)

圖3進(jìn)行4組耗時(shí)統(tǒng)計(jì)實(shí)驗(yàn)，選用MH—05數(shù)據(jù)集測(cè)試。圖3(a)～(c)分別表示CLAHE,FAST和光流跟蹤耗時(shí)對(duì)比曲線，每一個(gè)曲線分別進(jìn)行OpenCV-CPU,OpenCV-GPU和本文GPU耗時(shí)對(duì)比實(shí)驗(yàn)；圖(d)為本文前端與VINS-Mono前端耗時(shí)對(duì)比曲線。分析圖(a)～(c)可知，本文Clahe-GPU耗時(shí)與OpenCV-GPU耗時(shí)基本一致但更平穩(wěn)，同時(shí)比CPU耗時(shí)更低；而本文FAST和光流跟蹤耗時(shí)明顯低于OpenCV-GPU和CPU；兩者都具有較高的運(yùn)算速度。分析曲線圖(d)可知，VINS-Mono前端平均耗時(shí)為0.017 65 s，而本文前端平均耗時(shí)為0.005 62 s，具有更快的計(jì)算速度；曲線中VINS-Mono前端多幀耗時(shí)明顯大于0.035 s，而本文前端耗時(shí)比較平穩(wěn)。

3.2 系統(tǒng)精度實(shí)驗(yàn)

本文以EuROC中MH_01,MH_02,MH_03,MH_04和MH_05作為實(shí)驗(yàn)數(shù)據(jù)集，采用均方根誤差來(lái)評(píng)估本文前端結(jié)合VINS-Mono后端的里程計(jì)軌跡精度，并與VINS-Mono系統(tǒng)進(jìn)行對(duì)比。誤差計(jì)算采用EVO評(píng)估工具[12]，計(jì)算結(jié)果如表1所示。

表1 軌跡均方根誤差 m

根據(jù)表1所示，在MH—04和MH—05運(yùn)動(dòng)比較劇烈的數(shù)據(jù)集下本文精度略高于VINS-Mono，在其他數(shù)據(jù)集本文精度與VINS-Mono相近，平均均方根相差0.008 963 m。本文保持了較好的定位精度，與VINS-Mono相差很小。

4 結(jié) 論

本文設(shè)計(jì)了一種基于GPU并行加速視覺(jué)里程計(jì)前端方法，對(duì)系統(tǒng)中CLAHE、FAST角點(diǎn)提取和光流跟蹤進(jìn)行GPU并行化設(shè)計(jì)。實(shí)驗(yàn)結(jié)果表明:本文系統(tǒng)在不降低整體定位精度的情況下，明顯提高算法的實(shí)時(shí)性效率，能保證定位導(dǎo)航的實(shí)時(shí)性要求。雖然實(shí)現(xiàn)性能得到了提升，但精度卻保持了原來(lái)的精度。下一步工作將在本文的基礎(chǔ)上對(duì)VIO前端算法改進(jìn)，提高系統(tǒng)的精度，最后在實(shí)際情況下進(jìn)行定位測(cè)試。