摘 要：光束平差法（bundle adjustment，BA）是同步定位和地圖構(gòu)建（simultaneous localization and mapping，SLAM）后端優(yōu)化的關(guān)鍵技術(shù)。在線使用光束平差時能否滿足實(shí)時性要求，是將其應(yīng)用于自動駕駛車端等實(shí)時系統(tǒng)的關(guān)鍵因素。首先分析特定場景中SLAM數(shù)據(jù)特點(diǎn)，提出滑動窗口機(jī)制降低計(jì)算規(guī)模；分析局部BA計(jì)算中稀疏矩陣性質(zhì)提升算法的可并行性；最后基于嵌入式GPU對算法進(jìn)行并行加速。將其應(yīng)用于車載SLAM系統(tǒng)并在真實(shí)場景下測試，實(shí)驗(yàn)結(jié)果表明，在AGX Xavier嵌入式GPU上，針對720P道路場景，該方法比同平臺CPU上處理性能平均提升4.8倍，可以處理15 fps的相機(jī)位姿地圖數(shù)據(jù)，滿足了30 fps的視頻處理需求，達(dá)到了車載系統(tǒng)的實(shí)時性要求。

關(guān)鍵詞：光束平差法； 嵌入式GPU； SLAM； CUDA

中圖分類號：TP302 文獻(xiàn)標(biāo)志碼：A

文章編號：1001-3695（2022）07-008-1973-06

doi：10.19734/j.issn.1001-3695.2021.12.0674

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目（61972180）

作者簡介：謝雙鐿（1995-），男，湖南醴陵人，碩士研究生，主要研究方向?yàn)橛?jì)算機(jī)體系結(jié)構(gòu)；孫瑞鑫（1997-），男，河南信陽人，碩士研究生，主要研究方向?yàn)橛?jì)算機(jī)體系結(jié)構(gòu)；郭雪亮（1993-），男，河北邢臺人，碩士，主要研究方向?yàn)?D視覺；柴志雷（1975-），男（通信作者），山西人，教授，博導(dǎo)，博士，主要研究方向?yàn)橛?jì)算機(jī)體系結(jié)構(gòu)（zlchai@jiangnan.edu.cn）.

Research on high speed calculation method of online bundle adjustment

Xie Shuangyi¹， Sun Ruixin¹， Guo Xueliang²， Chai Zhilei^1?

（1.School of Artificial Intelligence amp; Computer Science， Jiangnan University， Wuxi Jiangsu 214122， China; 2. UISEE（Shanghai） Automotive Technologies Ltd.， Shanghai 201807， China）

Abstract：The bundle adjustment（BA） is a key technology for the back-end optimization of simultaneous positioning and map construction（SLAM） . Using BA online can meet real-time requirements， which is a key factor in applying it to real-time systems such as autonomous driving vehicles. This paper firstly analyzed the characteristics of SLAM data in real scene and proposed a method using sliding window to reduce the calculation scale. Then it featured the parallelism of the algorithm by analyzing the properties of sparse matrices in local BA. Finally， it implemented and accelerated the algorithm based on the embedded GPU. Applied and tested into a vehicle-mounted SLAM system in a real scene， the experimental results show that it has 4.8 times higher processing performance than the CPU on the same platform on average on the AGX Xavier with embedded GPU for 720P images from road scenes. It can also handle position， pose and map data at 15 fps， which meets the real-time requirements of the vehicle-mounted system.

Key words：bundle adjustment（BA）; embedded GPU; SLAM; CUDA

0 引言

光束平差法^［1］是一種基于相機(jī)成像模型等約束條件來求解物體的三維坐標(biāo)、相機(jī)的外方位參數(shù)以及相機(jī)的內(nèi)方位參數(shù)的算法，被廣泛應(yīng)用于運(yùn)動推斷結(jié)構(gòu)（structure from motion，SFM）^［2］、同步定位和地圖構(gòu)建^［3］、運(yùn)動機(jī)器人和遙感遙測等領(lǐng)域。在SLAM應(yīng)用中，BA作為優(yōu)化器對前端視覺里程計(jì)（visual odometry，VO）^［4］輸出地圖的參數(shù)精度進(jìn)行優(yōu)化，是地圖優(yōu)化的關(guān)鍵算法。

BA是對地圖數(shù)據(jù)進(jìn)行最優(yōu)化估計(jì)的計(jì)算密集型算法，但高延時的計(jì)算限制了BA的應(yīng)用場景，因此如何實(shí)現(xiàn)高效計(jì)算一直是研究的重點(diǎn)。2009年SBA（sparse bundle adjustment）^［5］利用重投影計(jì)算中相機(jī)矩陣的稀疏性質(zhì)，使用Cholesky分解法求解BA，極大提高了BA的求解速度和精度，成為后續(xù)SLAM后端^{［6，7］}的主流算法。2010年Google公司開發(fā)了C++數(shù)學(xué)庫Ceres Solver^［8］，可以方便地建模求解BA并具有優(yōu)秀的性能。2011年Grisetti等人^［9］提出圖優(yōu)化的計(jì)算框架g²o，該框架基于因子圖模型構(gòu)建BA，針對因子圖的邊緣化結(jié)構(gòu)優(yōu)化計(jì)算效率，并且可以使用流形優(yōu)化解決旋轉(zhuǎn)矩陣優(yōu)化中出現(xiàn)奇異解的問題。2019年天津大學(xué)針對BA算法中的舒爾消元計(jì)算部分設(shè)計(jì)了FPGA加速器^［10］，對BA專用加速器開始了探索。2020年Ortiz等人^［11］針對多核IPU上設(shè)計(jì)了BA算法，在大規(guī)模數(shù)據(jù)上相較于Ceres Solver快了24倍。但上述優(yōu)化大多是針對整體數(shù)據(jù)離線計(jì)算的方式，而離線算法對實(shí)時性一般不作要求，并且依賴大型計(jì)算機(jī)來完成計(jì)算。

近年來隨著自動駕駛等領(lǐng)域的發(fā)展，SLAM系統(tǒng)希望能更多應(yīng)用于嵌入式實(shí)時環(huán)境下，但使用傳統(tǒng)BA作為離線處理VO數(shù)據(jù)的后端^［12］，計(jì)算復(fù)雜度高^{［13，14］}、計(jì)算數(shù)據(jù)量大，難以達(dá)到駕駛場景下實(shí)時SLAM的性能和精度的要求。因此如何設(shè)計(jì)實(shí)現(xiàn)實(shí)時響應(yīng)的在線輕量化BA算法成為解決車載平臺上SLAM實(shí)際應(yīng)用的關(guān)鍵。

本文提出一種在線BA算法，算法中設(shè)計(jì)了維護(hù)VO數(shù)據(jù)的滑動窗口機(jī)制實(shí)現(xiàn)局部BA在線計(jì)算，并根據(jù)滑動窗口中數(shù)據(jù)稀疏矩陣的特點(diǎn)設(shè)計(jì)并行優(yōu)化算法，最終將優(yōu)化算法在嵌入式GPU平臺上實(shí)現(xiàn)并應(yīng)用到完整的SLAM系統(tǒng)中，對比g²o后端性能達(dá)到了4.8倍的加速比，實(shí)現(xiàn)了SLAM場景下的實(shí)時優(yōu)化。

1 光束平差法基本原理和計(jì)算方法

1.1 最小化重投影誤差優(yōu)化法

光束法平差的本質(zhì)是通過優(yōu)化相機(jī)位姿和路標(biāo)點(diǎn)空間坐標(biāo)等參數(shù)以求解觀測方程下的最小觀測誤差^［15］。最小觀測誤差是指空間三維點(diǎn)重新透視投影變換后的投影坐標(biāo)與實(shí)際觀測圖像上的像素坐標(biāo)的誤差，也稱為重投影誤差。透視投影變換是根據(jù)相機(jī)投影模型中的參數(shù)將三維空間坐標(biāo)變換到相機(jī)拍攝圖像的二維像素坐標(biāo)的過程，透視投影變換可以用式（1）描述，其中各個參數(shù)的描述如表1所示。

為方便表述，將空間三維坐標(biāo)記為L，投影點(diǎn)像素坐標(biāo)記為O，相機(jī)參數(shù)統(tǒng)一記做P，投影函數(shù)用函數(shù)f表示，則透視投影變換可重新寫做

BA計(jì)算中需要聯(lián)合多組成像點(diǎn)來完成優(yōu)化，當(dāng)空間三維點(diǎn)Li在相機(jī)Pj中成像，透視投影變換后有投影點(diǎn)像素坐標(biāo)Oij，而對應(yīng)的觀測點(diǎn)像素坐標(biāo)為Oij，此時對應(yīng)的重投影誤差計(jì)算公式如下：

BA通過求解全局最小重投影誤差得到各個參數(shù)的最優(yōu)估計(jì)。根據(jù)重投影誤差聯(lián)立為方程組可以得到如式（4）所示的全局最小二乘優(yōu)化問題，其中F（x）為所有g(shù)ij的聯(lián)立方程組，表示全局重投影的誤差，方程的輸入是由所有待優(yōu)化的參數(shù)組成的向量，記為x=［L1，L2，…，P1，P2，…］。

1.2 Levenberg-Marquardt優(yōu)化算法

BA中通過Levenberg-Marquardt（LM）優(yōu)化算法^［16］求解最小重投影誤差。LM優(yōu)化算法是一種非線性最小二乘問題的求解算法，也是SLAM中光束平差問題的標(biāo)準(zhǔn)算法^［17］。LM算法的特點(diǎn)在于基于高斯—牛頓法加入了阻尼系數(shù)，阻尼可以根據(jù)迭代結(jié)果自動調(diào)節(jié)，使得算法自身同時具備高斯—牛頓法和最速下降法的優(yōu)點(diǎn)。

LM算法的核心在于利用泰勒展開近似判斷當(dāng)前迭代搜索的質(zhì)量。對于F（x），其二階泰勒展開近似函數(shù)T定義為

對于最小二乘問題的優(yōu)化，LM算法中定義迭代步長Δx求解方程如式（6）所示。

其中：F=F（x）為全局重投影誤差成本函數(shù);J=J（x）為重投影誤差函數(shù)g對應(yīng)的雅可比矩陣;μ為阻尼參數(shù);I為單位矩陣。求解步長后還需要根據(jù)效果評價指標(biāo)ρ判斷步長是否有效。評價指標(biāo)計(jì)算公式如下：

當(dāng)ρgt;0時，說明此次搜索的迭代步長是有效的，將阻尼系數(shù)根據(jù)ρ進(jìn)行重置；當(dāng)ρ≤0時，說明此次搜索的迭代步長無效，需要擴(kuò)大阻尼系數(shù)重新搜索步長。參數(shù)x為經(jīng)過多次迭代優(yōu)化后收斂的結(jié)果，也就是經(jīng)過BA算法優(yōu)化后的參數(shù)估計(jì)值。

2 基于滑動窗口的在線BA算法

2.1 滑動窗口機(jī)制

SLAM系統(tǒng)中為了提高整體計(jì)算性能，將整體任務(wù)分為前端的VO模塊和后端的地圖優(yōu)化模塊。前端模塊負(fù)責(zé)處理傳感器的輸入信息并計(jì)算相機(jī)位姿和地圖數(shù)據(jù)，對實(shí)時性有較高的要求；后端則是使用BA算法對當(dāng)前地圖數(shù)據(jù)離線計(jì)算優(yōu)化。BA優(yōu)化算法中，對數(shù)據(jù)中所有相機(jī)位姿和三維點(diǎn)進(jìn)行離線修正，稱之為全局BA（global BA，GBA）。GBA由于處理全局?jǐn)?shù)據(jù)計(jì)算復(fù)雜，通常將其分離為后端模塊進(jìn)行離線計(jì)算，無法對前端輸出實(shí)時反饋優(yōu)化結(jié)果，導(dǎo)致前端模塊會有累積漂移誤差的問題。局部BA（local BA，LBA）則不同于GBA，優(yōu)化時將地圖中的待優(yōu)化數(shù)據(jù)限定在一定的時間或空間窗口內(nèi)進(jìn)行優(yōu)化，配合滑動窗口機(jī)制可以在單次窗口計(jì)算代價大幅度減小的同時獲得不遜于GBA的優(yōu)化精度^［18］。對于SLAM場景下實(shí)時輸入的連續(xù)圖像序列，VO在相鄰時間空間內(nèi)輸出的相機(jī)位姿和特征點(diǎn)明顯具有強(qiáng)相關(guān)性，不同圖像間擁有大量共視特征點(diǎn)；而對于間隔較遠(yuǎn)的相機(jī)和特征點(diǎn)則表現(xiàn)出邊緣化特征，圖像間的共視特征點(diǎn)極少。因此本文提出一種基于滑動窗口機(jī)制的LBA算法，將VO數(shù)據(jù)建立數(shù)據(jù)結(jié)構(gòu)關(guān)系，利用連續(xù)圖像之間的相關(guān)特性劃分滑動窗口計(jì)算LBA。

如圖1所示，將VO輸出的相機(jī)位姿P逐行遞增排列，三維空間點(diǎn)L逐列遞增排列，矩陣中的黑色方塊表示三維點(diǎn)在對應(yīng)的相機(jī)位姿下產(chǎn)生的重投影誤差。隨著VO輸出增加，矩陣規(guī)模擴(kuò)大，對應(yīng)GBA處理的數(shù)據(jù)規(guī)模也隨之?dāng)U大。在新輸出的相機(jī)位姿中，即對應(yīng)矩陣新的一行中，舊的三維空間點(diǎn)可能會在此相機(jī)中共視產(chǎn)生新的投影點(diǎn)，也可能離開相機(jī)視角導(dǎo)致投影點(diǎn)不再出現(xiàn)，而也可能VO計(jì)算得出新的三維空間點(diǎn)，即對應(yīng)矩陣產(chǎn)生新的一列。根據(jù)此特點(diǎn)，LBA將窗口限制為連續(xù)的數(shù)個相機(jī)位姿，并選取在這些相機(jī)位姿下產(chǎn)生投影的三維點(diǎn)參與計(jì)算。窗口隨時間順序在圖像序列上進(jìn)行滑動如圖中W1和W2所示，隨著窗口的滑動，會有新相機(jī)位姿及其相關(guān)路標(biāo)點(diǎn)進(jìn)入窗口中參與計(jì)算，同時也會有相機(jī)位姿退出窗口，與之相關(guān)路標(biāo)點(diǎn)也隨之不再參與計(jì)算?；瑒哟翱趧澐殖鼍植繑?shù)據(jù)的方式避免了GBA方式的大規(guī)模計(jì)算場景，也確保LBA計(jì)算中數(shù)據(jù)規(guī)模保持穩(wěn)定，同時這也符合SLAM系統(tǒng)實(shí)際工作時VO數(shù)據(jù)隨圖像輸入變化的流動過程。窗口隨著VO輸出滑動進(jìn)行LBA計(jì)算，達(dá)到在線計(jì)算的效果，實(shí)時反饋更新窗口中的最優(yōu)值，從而保證計(jì)算結(jié)果的整體最優(yōu)。

若當(dāng)前窗口中相機(jī)的數(shù)量為m，三維點(diǎn)數(shù)量為n，參與計(jì)算的重投影點(diǎn)的數(shù)量為l，顯然llt;m×n，則重投影過程算法復(fù)雜度為O（l）。待優(yōu)化的相機(jī)參數(shù)和三維點(diǎn)坐標(biāo)參數(shù)的數(shù)量為6m+3n，因此可以由待優(yōu)化參數(shù)數(shù)量得知LM算法單步迭代的復(fù)雜度為O16（6m+3n）³。若算法中迭代次數(shù)為k次，則整體BA算法復(fù)雜度為Okl+16（6m+3n）³。

2.2 LM算法優(yōu)化設(shè)計(jì)

雖然滑動窗口減少了LM算法輸入數(shù)據(jù)的計(jì)算規(guī)模，但是在線計(jì)算的方式對LBA算法有更高的性能要求，嵌入式設(shè)備CPU難以達(dá)到實(shí)時性要求。因此還需要對LM算法進(jìn)行優(yōu)化，設(shè)計(jì)并行計(jì)算方法，通過異構(gòu)設(shè)備實(shí)現(xiàn)算法加速效果。求解BA問題時，LM優(yōu)化算法中主要有以下計(jì)算任務(wù)：a）計(jì)算重投影誤差g；b）計(jì)算雅可比矩陣J；c）求解式（6）計(jì)算迭代步長Δx；d）更新相機(jī)位姿和路標(biāo)點(diǎn)坐標(biāo)。其中耗時最長的部分是計(jì)算雅可比矩陣J和求解迭代步長，占據(jù)總耗時的80%^［10］，而且以矩陣計(jì)算為主。通過滑動窗口機(jī)制建立的投影點(diǎn)與相機(jī)位姿和三維空間坐標(biāo)之間的關(guān)系，可以為LM算法各步驟設(shè)計(jì)并行計(jì)算方法。

2.2.1 稀疏矩陣重映射

通過滑動窗口對VO數(shù)據(jù)的管理能夠快速得到LBA輸入數(shù)據(jù)之間的關(guān)系。由于實(shí)際中三維空間點(diǎn)僅可能被有限的數(shù)個相機(jī)聯(lián)合觀測，也就是同一三維空間點(diǎn)在連續(xù)多個相機(jī)位姿下產(chǎn)生重投影誤差的長度有限，所以BA的輸入數(shù)據(jù)矩陣整體呈現(xiàn)為近似沿對角線分布的稀疏矩陣?；瑒哟翱陔m然限制輸入數(shù)據(jù)矩陣大小，但仍然具備稀疏的性質(zhì)，導(dǎo)致設(shè)計(jì)并行任務(wù)時，對輸入數(shù)據(jù)無論是利用相機(jī)位姿按行劃分，或利用空間三維點(diǎn)按列劃分都會出現(xiàn)計(jì)算任務(wù)不均衡的問題，所以此處設(shè)計(jì)一種將稀疏矩陣一維化展開的方法，實(shí)現(xiàn)對窗口輸入數(shù)據(jù)的細(xì)粒度并行劃分。

如圖2所示，輸入數(shù)據(jù)保存在稀疏矩陣中，通過行號和列號對數(shù)據(jù)進(jìn)行索引。在遍歷所有數(shù)據(jù)時需要枚舉所有的行列號，而矩陣中的有效元素?cái)?shù)量卻遠(yuǎn)低于矩陣大小，因此為了重新編碼矩陣中有效元素，使用映射數(shù)組記錄有效元素的行列編號。映射數(shù)組記錄了元素的行列號，對應(yīng)到滑動窗口對應(yīng)的相機(jī)位姿編號和三維空間點(diǎn)編號，因此映射數(shù)組實(shí)際上記錄了輸出的重投影誤差編號對應(yīng)的輸入數(shù)據(jù)編號的映射關(guān)系。由于映射數(shù)組記錄的是索引編號，所以相機(jī)位姿、空間三維點(diǎn)坐標(biāo)和重投影誤差都可以使用線性存儲結(jié)構(gòu)，方便按照編號索引。

對于線性存儲中相機(jī)位姿P和三維點(diǎn)空間點(diǎn)L，觀測點(diǎn)坐標(biāo)，存儲索引關(guān)系的映射數(shù)組為map，使用.at（·）函數(shù)表示線性存儲中對應(yīng)輸入索引的數(shù)據(jù)，則重投影計(jì)算中編號為k的數(shù)據(jù)有如下的計(jì)算關(guān)系，如式（8）所示。

從重投影誤差的計(jì)算式中可以發(fā)現(xiàn)，各編號對應(yīng)的誤差計(jì)算均相互獨(dú)立，并且輸出數(shù)據(jù)與為一維的線性存儲編號一一對應(yīng)，因此可以設(shè)計(jì)并行算法，將各編號數(shù)據(jù)并行完成計(jì)算。

2.2.2 數(shù)據(jù)并行計(jì)算設(shè)計(jì)

LM優(yōu)化算法求解BA過程中，待優(yōu)化的參數(shù)只有相機(jī)位姿和三維空間坐標(biāo)，雅可比矩陣根據(jù)參數(shù)類型分為兩部分，如式（9）所示。

顯然雅可比矩陣的規(guī)模由重投影誤差數(shù)量決定，它可以與重投影誤差使用相同的編號索引。由于投影關(guān)系中三維空間點(diǎn)對應(yīng)相機(jī)位姿只產(chǎn)生一個投影點(diǎn)，計(jì)算透視投影函數(shù)的雅可比矩陣可以表述為式（9）的形式，結(jié)合式（8）的映射關(guān)系可以得到矩陣J對應(yīng)投影點(diǎn)計(jì)算出的單獨(dú)一行具有式（10）的形式。

不難發(fā)現(xiàn)，此時雅可比矩陣具有以下性質(zhì)：重投影誤差g.at（k）只參與雅可比行分塊矩陣Jk的計(jì)算;JP和JL在行分塊矩陣中每行塊只分別對應(yīng)一個分塊矩陣，其余均為零矩陣。因此雅可比矩陣只需要一維線性結(jié)構(gòu)進(jìn)行壓縮存儲，且有與重投影誤差形式相似的結(jié)構(gòu)，索引為k的行分塊矩陣存儲結(jié)構(gòu)如式（11）所示。映射數(shù)組同時還記錄了JP和JL的行列索引信息，輸入數(shù)據(jù)中相機(jī)位姿數(shù)量m、三維空間點(diǎn)數(shù)量n、行塊的編號k滿足0≤klt;m+n，JP.at（k）對應(yīng)的列塊編號為i，JL.at（k）對應(yīng)的列塊編號為i+m+j。

將輸入數(shù)據(jù)進(jìn)行展開并重新映射索引后，雅可比矩陣中每個行塊矩陣均可以并行計(jì)算，且計(jì)算時只需要讀取相關(guān)的P.at（j）和L.at（i）作為輸入計(jì)算對應(yīng)的分塊矩陣即可。數(shù)組map中已經(jīng)記錄了對應(yīng)的數(shù)據(jù)映射關(guān)系，可以快速讀取數(shù)據(jù)。

在上述分組存儲后，雅可比矩陣J以行塊進(jìn)行分組計(jì)算，并將每行塊分為獨(dú)立兩個分塊矩陣存儲，基于此分塊形式可以將步長的求解方程先計(jì)算出部分結(jié)果再進(jìn)行求和來得到最終結(jié)果，將式（6）的計(jì)算轉(zhuǎn)換為如式（12）的表示。

根據(jù)映射數(shù)組的索引k可以得到式（12）中各式的對應(yīng)計(jì)算，如式（13）所示。

步驟中所有的矩陣乘法并行計(jì)算可以有效提高計(jì)算效率。形如式（12）的方程組使用舒爾消元能夠?qū)⒎匠剔D(zhuǎn)換為形如HscΔxP=bsc的形式，先行計(jì)算出部分解，從而回避大矩陣求逆的計(jì)算，給出消元方程計(jì)算如式（14）所示。

求解ΔxP時，由于Hsc為實(shí)正定對稱矩陣，使用數(shù)學(xué)庫中的Cholesky分解方法求解方程更加高效。解出ΔxP后回代方程即可求出ΔxL，如式（15）所示。

舒爾消元過程充分運(yùn)用BA問題中矩陣分塊的性質(zhì)，能夠?qū)⒕仃囘\(yùn)算并行化。對于HPP和HLL具有對角分塊形式的矩陣，對角的分塊矩陣相互獨(dú)立完成計(jì)算。對于bsc和ΔxL劃分為行塊的形式運(yùn)算，也滿足并行計(jì)算的條件。對于計(jì)算HPLH^-1LLHTPL，則使用類似圖2的方法建立映射表，將分塊矩陣的二維行列索引重映射為一維數(shù)組，再根據(jù)索引分組并行計(jì)算。

3 基于嵌入式GPU的并行加速

本文基于NVIDIA Jetson AGX Xavier嵌入式GPU進(jìn)行并行加速，該設(shè)備配備有8核ARM v8.2 64位CPU和64個Tensor Core的512核Volta架構(gòu)GPU，并且配有32 GB內(nèi)存并支持統(tǒng)一內(nèi)存訪問，GPU使用NVIDIA公司的CUDA編程框架進(jìn)行開發(fā)管理。Xavier上使用統(tǒng)一內(nèi)存，配合CUDA異構(gòu)編程框架，CPU和GPU間可以快速傳遞內(nèi)存數(shù)據(jù)。根據(jù)優(yōu)化設(shè)計(jì)后的BA算法框架如圖3所示，CPU上實(shí)現(xiàn)滑動窗口機(jī)制和LM算法迭代控制判斷，GPU上實(shí)現(xiàn)雅可比矩陣計(jì)算、舒爾消元、線性方程求解等并行計(jì)算任務(wù)。

3.1 滑動窗口數(shù)據(jù)組織實(shí)現(xiàn)

VO會輸出當(dāng)前幀間的相機(jī)位姿和匹配特征點(diǎn)，但需要緩存多幀間的三維點(diǎn)和相機(jī)位姿信息完成BA。因此需要設(shè)計(jì)數(shù)據(jù)結(jié)構(gòu)用于保存相鄰幀間特征點(diǎn)的匹配關(guān)系以及三維點(diǎn)與特征點(diǎn)的投影對應(yīng)關(guān)系，用于實(shí)現(xiàn)滑動窗口機(jī)制?；瑒哟翱谥械臄?shù)據(jù)結(jié)構(gòu)如圖4所示。VO輸出的特征點(diǎn)有兩種情況：a）該特征點(diǎn)為新提取的特征點(diǎn)，此時會對應(yīng)計(jì)算出新的三維點(diǎn)；b）該特征點(diǎn)在上一幀有匹配的特征點(diǎn)，此時需要將該特征點(diǎn)與已有的三維點(diǎn)建立關(guān)聯(lián)。因此使用鄰接表結(jié)構(gòu)來分別存儲相機(jī)位姿、三維點(diǎn)和二維投影點(diǎn)。各個鄰接表中，每個特征點(diǎn)保存匹配的上一幀中與之匹配的特征點(diǎn)的指針，方便查找匹配關(guān)系；每個三維點(diǎn)保存與之建立投影關(guān)系的特征點(diǎn)指針；相機(jī)位姿則記錄與此相機(jī)相關(guān)的所有特征點(diǎn)指針及三維點(diǎn)指針，便于窗口發(fā)生滑動時及時釋放資源。

3.2 基于CUDA實(shí)現(xiàn)LM并行優(yōu)化

Xavier平臺上GPU并行計(jì)算模型是一種稱為SIMT（單指令多線程）的眾核架構(gòu)，包含多種并行結(jié)構(gòu)。在CUDA編程模型下，GPU作為一個協(xié)處理器，適合處理高度并行化的大量輕量級線程，以最大限度地提高吞吐量。CUDA架構(gòu)中線程層次結(jié)構(gòu)由網(wǎng)格（grid）維度和塊（block）維度決定，第一個參數(shù)決定啟動的塊的數(shù)量，第二個參數(shù)決定每個塊中線程的數(shù)目。

滑動窗口中將輸入數(shù)據(jù)存儲在全局內(nèi)存中后，根據(jù)建立的映射數(shù)組可以將投影點(diǎn)的旋轉(zhuǎn)投影變換和雅可比矩陣的第k個分塊矩陣計(jì)算分別對應(yīng)到第k號線程中進(jìn)行計(jì)算，計(jì)算流程如圖5所示。各個線程計(jì)算先從全局內(nèi)存中取出輸入數(shù)據(jù)緩存到寄存器內(nèi)存中，計(jì)算出中間結(jié)果后再將結(jié)果存儲到對應(yīng)結(jié)果的全局內(nèi)存中，即可構(gòu)造出線性方程組的初步形式。

得到迭代方程系數(shù)矩陣后進(jìn)一步進(jìn)行舒爾消元，消元過程中HPLH^－1LLHTPL的計(jì)算需要根據(jù)有效行塊進(jìn)行重新映射索引，而且以k編號的分塊矩陣的乘積為最終結(jié)果的部分和。因此求和過程可以結(jié)合并行分組進(jìn)行歸約求和以取得更好的效率，具體計(jì)算流程如圖6所示。

舒爾消元后的方程HscΔxP=bsc的系數(shù)矩陣大小只和相機(jī)參數(shù)數(shù)量有關(guān)，即與滑動窗口大小有關(guān)。由于SLAM場景中連續(xù)相關(guān)相機(jī)位姿數(shù)目數(shù)量，所以使用CUSOLVER數(shù)學(xué)庫中的稠密喬洛斯基分解方法（cusolverDnDpotrf）及其配套的求解函數(shù)（cusolverDnDpotrs）完成線性方程求解。求解出ΔxP后利用式（15）解出ΔxL，將解出的全部再次計(jì)算重投影誤差，將誤差的值傳回CPU端，由CPU控制此次迭代是否可以接受以及下一輪迭代的進(jìn)行。迭代完成后將優(yōu)化后的相機(jī)位姿和三維點(diǎn)信息傳遞回CPU并輸出，完成地圖優(yōu)化構(gòu)建。

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

本文所實(shí)現(xiàn)的優(yōu)化算法在完整的自動駕駛實(shí)驗(yàn)平臺上部署并在真實(shí)場景下進(jìn)行了測試。搭建的測試環(huán)境如圖7所示，其中包含NVIDIA Jetson AGX Xavier計(jì)算平臺，其CPU為64位8核心Carmel Arm v8.2、GPU為512核心Volta架構(gòu)、內(nèi)存為32 GB 256位LPDDR4X內(nèi)存、操作系統(tǒng)為Ubuntu 18.04 LTS、CUDA版本為10.2；同時搭載了攝像頭、高精度GPS等傳感器設(shè)備。

SLAM系統(tǒng)用于驗(yàn)證在線光束平差算法的性能，該系統(tǒng)將攝像頭采集的圖片序列分析處理，實(shí)時計(jì)算稀疏光流并輸出優(yōu)化后的三維點(diǎn)云和行駛路徑。SLAM系統(tǒng)主要由圖像矯正、特征提取、特征追蹤、位姿恢復(fù)、點(diǎn)云重建和BA模塊組成，均采用在線計(jì)算的方式。通過測試平臺搭載720P@30 fps定焦相機(jī)在車場實(shí)際道路行駛采集實(shí)驗(yàn)測試數(shù)據(jù)，并對基于滑動窗口的在線BA算法進(jìn)行測試，對比同平臺下使用g²o^［9］作為后端在CPU優(yōu)化算法的結(jié)果差異。

首先對車場的實(shí)際數(shù)據(jù)進(jìn)行分析，統(tǒng)計(jì)VO輸出的被連續(xù)觀測特征點(diǎn)的共視相機(jī)數(shù)量，得到的分布數(shù)據(jù)如表2所示。

通過對輸出的特征點(diǎn)數(shù)量比例進(jìn)行分析可以發(fā)現(xiàn)，特征點(diǎn)關(guān)聯(lián)的相機(jī)數(shù)量越多，則所占比例越低，這符合對道路行駛數(shù)據(jù)中特征點(diǎn)呈窗口出現(xiàn)的特點(diǎn)，也證明了SLAM場景下更適合使用LBA。統(tǒng)計(jì)中近95%特征點(diǎn)的連續(xù)相關(guān)相機(jī)數(shù)量不超過10，說明SLAM系統(tǒng)中的數(shù)據(jù)相對于整體具有局部時空連續(xù)的性質(zhì)，使用滑動窗口機(jī)制能夠利用這一性質(zhì)提高計(jì)算效率。

對于實(shí)地采集的3 419張圖片道路數(shù)據(jù)，在配置LBA不同窗口大小的情況下在平臺上進(jìn)行測試，BA模塊平均耗時對比情況如表3所示，迭代次數(shù)為15次，CPU端算法耗時平均為GPU加速算法4.8倍，而且隨著窗口大小的增加，加速比越大，單個窗口中需要計(jì)算的路標(biāo)點(diǎn)數(shù)量會隨之增加，性能提升也越大，體現(xiàn)了使用GPU加速并行計(jì)算的優(yōu)勢。

整體重投影誤差隨迭代次數(shù)下降曲線如圖8所示，兩者都是基于相同參數(shù)配置的LM優(yōu)化算法，因此迭代曲線下降速度基本一致。對比g²o和本文設(shè)計(jì)方法的優(yōu)化后結(jié)果的旋轉(zhuǎn)矩陣系數(shù)和三維點(diǎn)的均方誤差差異小于1%，證明算法精度符合要求。

將前端VO和本文設(shè)計(jì)的后端優(yōu)化BA算法共同部署到Xavier平臺上，攝像機(jī)以30 fps速率輸入圖片到VO，VO每2幀圖片輸出一次位姿數(shù)據(jù)，對應(yīng)為15 fps的輸出速率，BA算法選取窗口大小為5，實(shí)時運(yùn)行中和運(yùn)行結(jié)束后的軌跡和點(diǎn)云輸出如圖9所示。運(yùn)行中后端BA算法平均耗時為59.6 ms（gt;15 fps），滿足實(shí)時性的要求。SLAM系統(tǒng)最終輸出的軌跡與實(shí)際衛(wèi)星圖像對比如圖10所示。

本文實(shí)現(xiàn)方案與其他SLAM系統(tǒng)后端在性能方面的對比如表4所示。其中文獻(xiàn)［19］設(shè)計(jì)了完整的SLAM系統(tǒng)，此處參考其中LBA性能的平均值；文獻(xiàn)［20］只針對BA中部分計(jì)算加速，因此只分析了單步迭代的耗時。多個方案間存在性能上的差異與計(jì)算的數(shù)據(jù)量有關(guān)，基于滑動窗口的設(shè)計(jì)可以有效劃分安排計(jì)算任務(wù)，從而極大提高計(jì)算速度。同時，BA主要計(jì)算任務(wù)全部轉(zhuǎn)移到GPU上進(jìn)行計(jì)算，能夠減少CPU與GPU間的數(shù)據(jù)傳輸時間，提升整體的計(jì)算速度。

5 結(jié)束語

本文提出一種基于滑動窗口的在線BA算法，對其并行加速實(shí)現(xiàn)在線BA的高速計(jì)算。通過滑動窗口機(jī)制解決了實(shí)時輸出的位姿和點(diǎn)云無法被在線計(jì)算的問題；同時利用LBA數(shù)據(jù)的稀疏矩陣性質(zhì)，提出LM算法的并行優(yōu)化計(jì)算方法，解決LBA復(fù)雜計(jì)算的性能問題；最后在嵌入式GPU平臺上基于CUDA編程模型實(shí)現(xiàn)LBA的實(shí)時在線計(jì)算。通過將該算法應(yīng)用于實(shí)際車載場景下的SLAM系統(tǒng)中，成功使得系統(tǒng)在實(shí)時運(yùn)行的條件下達(dá)到了高精度。

駕駛場景的數(shù)據(jù)中，三維投影點(diǎn)只在局部相機(jī)窗口內(nèi)發(fā)生共視，相機(jī)矩陣整體呈現(xiàn)稀疏性質(zhì)，因此數(shù)據(jù)的特殊結(jié)構(gòu)是加速計(jì)算的突破點(diǎn)；此外相較于離線優(yōu)化算法，在線算法從局部最優(yōu)達(dá)成全局最優(yōu)的計(jì)算方式更適合實(shí)時SLAM中的持續(xù)計(jì)算場景。車載場景下計(jì)算資源有限，充分利用硬件特性進(jìn)行算法優(yōu)化，是實(shí)現(xiàn)算法高速計(jì)算的有效手段。

參考文獻(xiàn)：

[1]單杰.光束法平差簡史與概要［J］.武漢大學(xué)學(xué)報(bào)：信息科學(xué)版，2018，43（12）：1797-1810.（Shan Jie.Brief history and summary of bundle adjustment［J］.Journal of Wuhan University：Information Science，2018，43（12）：1797-1810.）

［2]Zhu Siyu，Zhang Runze，Zhou Lei，et al.Very large-scale global SfM by distributed motion averaging[C］//Proc of IEEE Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2018：4568-4577.

［3]劉浩敏，章國鋒，鮑虎軍.基于單目視覺的同時定位與地圖構(gòu)建方法綜述[J］.計(jì)算機(jī)輔助設(shè)計(jì)與圖形學(xué)學(xué)報(bào)，2016，28（6）：855-868.（Liu Haomin，Zhang Guofeng，Bao Hujun.A survey of monocular simultaneous localization and mapping[J］.Journal of Computer Aided Design amp; Computer Graphics，2016，28（6）：855-868.）

［4]Clark R，Wang Sen，Wen Hongkai，et al.VINet：visual-inertial odometry as a sequence-to-sequence learning problem[C］//Proc of AAAI Conference on Artificial Intelligence.Palo Alto，CA：AAAI Press，2017：3995-4001.

［5]Lourakis M I A，Argyros A A.SBA：a software package for generic sparse bundle adjustment[J］.ACM Trans on Mathematical Software，2009，36（1）：1-30.

［6]Bustos A P，Chin T J，Eriksson A，et al.Visual SLAM：why bundle adjust？[C］//Proc of International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2009：2385-2391.

［7]潘林豪，田福慶，應(yīng)文健，等.融合雙目視覺與慣導(dǎo)信息的高效視覺里程計(jì)算法[J］.計(jì)算機(jī)應(yīng)用研究，2021，38（6）：1739-1743，1769.（Pan Linhao，Tian Fuqing，Ying Wenjian，et al.Efficient visual odometry algorithm combining stereo vision and inertial navigation information[J］.Application Research of Computers，2021，38（6）：1739-1743，1769.）

［8]Agarwal S，Mierle K.Ceres Solver[EB/OL］.（2012）[2021-10-11］.http：//ceres-solver.org.

［9]Grisetti G，Kümmerle R，Strasdat H，et al.g²o：a general framework for （hyper） graph optimization[C］//Proc of IEEE International Confe-rence on Robotics and Automation.Piscataway，NJ：IEEE Press，2011：3607-3613.

［10]Qin Shuzhen，Liu Qiang，Yu Bo，et al.π-BA：bundle adjustment acce-leration on embedded FPGAs with co-observation optimization[C］//Proc of the 27th IEEE Annual International Symposium on Field-Programmable Custom Computing Machines. Piscataway，NJ：IEEE Press，2019：100-108.

［11]Ortiz J，Pupilli M，Leutenegger S，et al.Bundle adjustment on a graph processor[C］//Proc of IEEE/CVF Conference on Computer Vision and Pattern Recognition.Piscataway，NJ：IEEE Press，2020：2413-2422.

［12]張洪華，劉璇，陳付豪，等.基于圖優(yōu)化的 SLAM 后端優(yōu)化研究與發(fā)展[J］.計(jì)算機(jī)應(yīng)用研究，2019，36（1）：11-17.（Zhang Honghua，Liu Xuan，Chen Fuhao，et al.Research and development of SLAM back-end optimization based on graph optimization[J］.Application Research of Computers，2019，36（1）：11-17.）

［13]Agarwal S，F(xiàn)urukawa Y，Snavely N，et al.Building Rome in a day[J］.Communications of the ACM，2011，54（10）：105-112.

［14]Frahm J M，F(xiàn)ite-Georgel P，Gallup D，et al.Building Rome on a cloudless day[C］//Proc of European Conference on Computer Vision.Berlin：Springer-Verlag，2010：368-381.

［15]Triggs B，McLauchlan P F，Hartley R I，et al.Bundle adjustment—a modern synthesis[C］//Proc of International Workshop on Vision Algorithms.Berlin：Springer，1999：298-372.

［16]Madsen K，Nielsen H B，Tingleff O.Methods for non-linear least squares problems （2nd ed）[R］.Lyngby：Technical University of Denmark，2004.

［17]Strasdat H，Montiel J M M，Davison A J.Visual SLAM：why filter？[J］.Image and Vision Computing，2012，30（2）：65-77.

［18]Zhang Zhengyou，Shan Ying.Incremental motion estimation through modified bundle adjustment[C］//Proc of International Conference on Image Processing.Piscataway，NJ：IEEE Press，2003.

［19]Campos C，Elvira R，Rodríguez J J G，et al.ORB-SLAM3：an accurate open-source library for visual，visual-inertial，and multimap SLAM[J］.IEEE Trans on Robotics，2021，37（6）：1874-1890.

［20]劉強(qiáng)，秦書臻，俞波，等.一種用于SLAM的嵌入式光束平差法加速器設(shè)計(jì)[J］.天津大學(xué)學(xué)報(bào)：自然科學(xué)與工程技術(shù)版，2020，53（12）：1281-1287.（Liu Qiang，Qin Shuzhen，Yu Bo，et al.An embedded bundle adjustment accelerator design for SLAM[J］.Journal of Tianjin University：Sciences and Technology，2020，53（12）：1281-1287.）