集成光度在線標定的邊緣對準SLAM方法

徐曉蘇，游穎輝

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京 210096；2. 東南大學儀器科學與工程學院，南京 210096）

移動機器人在服務、工業自動化等領域具有廣泛應用[1]。視覺同步定位與建圖（Simultaneous Localization and Mapping, SLAM）是一種基于圖像的相機運動估計方法，在移動機器人的導航與控制中起到至關重要的作用，尤其在無外部信號作為參考時。在近二十年里，國內外在視覺SLAM領域有了較多研究，包括基于穩定特征的特征點法[2-4]、基于像素灰度的直接法[5,6]、同時參照灰度和特征的半直接法[7]、通過迭代最近點法（Iterative Closest Point, ICP）跟蹤相機的方法[8]等。特征點法對于光照變化較為魯棒，但存在計算描述子耗時較長、無法充分利用環境信息等問題。直接法可以充分利用環境信息且不依賴環境紋理，但無法有效應對光照變化的環境[9]。半直接法可以避免灰度變化的影響，但仍然受制于圖像信息的準確度。基于邊緣對準的視覺SLAM方法類似于半直接法，提取圖像中的邊緣特征，通過邊緣對準實現運動估計，能夠充分利用環境信息，并對光照變化有一定穩定性[10]，可以克服目前主流方法的缺點。

基于邊緣對準的視覺定位方法可以追溯到Eade E等人的研究[11]，該方法首次實現了基于粒子濾波的利用邊緣特征的視覺里程計。基于優化的方法則發端于Tarrio J J等人于2015年公開的一種基于邊緣特征的單目視覺里程計，該研究首次說明了邊緣對準是一個高效的可并行的操作[12]。Zhou Y等人于2019年提出的基于Canny檢測子的視覺里程計Canny-VO[13]，該方法提出了邊緣配準中常用的歐式域距離變換的兩種替代方法：近似最近鄰域和定向最近鄰域，并將邊緣的旋轉加入到優化中。文獻[14]提出的實時魯棒的邊緣對準SLAM（RESLAM）利用隨機蕨實現了回環檢測。目前的多數基于邊緣對準的方法都未考慮圖像光度變化對于邊緣提取的影響，更未在設計算法時設計離線或在線光度標定的環節。在光度標定的相關研究中，文獻[6]通過對數據集的圖像曝光時間、相機的Gamma響應函數以及詳細的鏡頭畸變因子進行了離線光度標定來提高視覺里程計性能，Paul Bergmann等人在2018年提出了一種對自動曝光的視頻流進行光度校正的方法[15]，并和文獻[6]提出的算法進行組合，有效提升了定位精度和建圖效果。

針對圖像光度變化導致不同圖像提取的邊緣特征分布差異較大的問題。本文對文獻[14]所提出的基于RGB-D相機的算法進行了優化改進，集成了在線光度標定模塊，提升了進行邊緣提取時的圖像質量，在數據集上驗證了在算法中集成在線光度標定對于定位精度有一定提升；在此基礎上，對殘差項的殘差權重函數進行設計，提升了系統多次運行的穩定性。

1 算法整體框架

本文基于機器人軟件平臺（Robot Operating System, ROS）實現集成在線光度標定的邊緣對準視覺里程計，其整體流程如圖1所示。該方法分為光度標定節點以及包含前端跟蹤線程、局部建圖線程、全局建圖線程的里程計節點。通過訂閱傳感器節點或者數據集發布的話題獲取圖像，獲取的圖像首先被讀入光度標定節點。在光度標定節點，對圖像進行提取角點、角點跟蹤、估計參數等一系列操作，再通過發布話題的方式將校正后的圖像發布。里程計節點訂閱經過光度標定后的圖像消息，在跟蹤線程對圖像幀先進行提取圖像邊緣的操作，然后通過距離變換構建殘差方程，迭代求解殘差方程實現邊緣對準，進而實現對相機運動的跟蹤，然后進行關鍵幀選擇。在局部線程內則對跟蹤線程選擇的關鍵幀進行回環編碼的計算，構建非線性優化滑動窗口，對滑動窗口中的變量進行局部優化，對關鍵幀的位姿進行優化，優化完成后根據設置的條件執行邊緣化策略以維持滑窗中的變量維數在一定數量；編碼后的關鍵幀被存入關鍵幀數據庫，用于進行回環檢測、回環驗證等環節，最后利用位姿圖構建全局優化對位姿做進一步優化。

圖1 算法整體流程圖Fig.1 The overall flow chart of the algorithm

2 光度標定

光度標定節點通過提取角點建立圖像之間的數據關系實現跟蹤，進而完成對曝光時間、漸暈等參數的估計，再對圖像做校正并發布圖像消息話題。

2.1 圖像光度建模

場景點被光源照亮時，將光線反射到空間中，反射的總光量稱為該三維點的輻射L。如果移動觀察者接收到該點的輻射與觀察者的觀察角度無關，則稱該場景點的輻射表現為蘭伯特反射。場景點的輻射被相機的感光元件接收，記感光元件上位置x接收到的輻射照度為I(x)。理想情況下，不同空間位置具有相同輻射的場景點會產生相同的傳感器輻射照度。然而，對于大多數相機而言，采集到的圖像像素強度從圖像中心向圖像邊界下降。這種現象被稱為漸暈效應，一般是由于光線被透鏡部分阻塞或者由于透鏡的幾何形狀引起的。則感光元件上的輻射照度I(x)可以通過I(x) =V(x)L表示，V: Ω→ [ 0,1]為漸暈因子，與圖像感光元件中的位置x?Ω相關。由于感光元件的輻射照度在曝光時間窗口內隨時間累積，假設在時間窗口內的傳感器感光元件的輻射照度恒定，對于曝光時間e內累積的輻射照度為Iacc(x) =eI(x) 。f: ?→ [ 0 ,255]為相機響應函數（Camera Response Function, CRF），從而場景點輻射L與圖像輸出的像素強度O之間的對應關系可以表示為：

使用經驗響應模型（Empiric model of Response,EMoR）對CRF進行建模，并利用基函數hk(x)組合得到如下的全局響應函數：

其中ck?R，f0(x)為通過原理成分分析（Principle Component Analysis, PCA）獲得的平均響應。本文假設漸暈的分布符合徑向漸暈模型，即衰減因子在圖像中心是對稱的，進一步地假設漸暈的中心和圖像的中心一致，則漸暈因子可以用一個六階多項式進行擬合。

其中R(x)是圖像點x相對于圖像中心的歸一化半徑。

2.2 參數估計

對于給定的圖像F，式(1)中的各項參數僅輸出的像素強度O已知，因此進行光度標定還需選擇一定數量的場景點通過重投影建立圖像幀之間的聯系。通過提取Shi-Thomais角點，并采用具有自適應增益的Kanade-Lucas-Tomasi（KLT）光流跟蹤算法來獲取點對之間的聯系。同時為了更好地校正漸暈，需要提取的角點均勻分布在圖像上，因此將圖像分成一系列小塊再提取角點。同時為了增加觀測量，在跟蹤的角點周圍的像素塊也被用于計算重投影殘差。

記輸入的圖像幀集合為Fp，跟蹤的場景點集合為P，對于在Fp中可視的場景點p?P，通過跟蹤點對可以建立如式(4)所示的殘差方程[15]。

通常需要較多的圖像來求解優化式(4)中的所有參數，曝光時間可以通過圖像相鄰幀之間的時差來估計，而漸暈模型和響應模型則需要多個圖像幀來估計。因此在本文算法中，將需要估計的參數進行分離估計。每新進一幀圖像，先用當前估計的漸暈和響應函數校正圖像，再估計曝光時間。因此式(4)可改寫為[15]：

其中，M為當前圖像幀集合的大小，Pi記為在第i幀可視的場景點，f-1是逆響應函數。對于固定的場景點的輻射照度pL，式(5)僅曝光時間是未知量，求解時間隨變量維度線性增長，可以快速求解。全部參數的優化估計通過在后端優化過程中對式(4)求解得到。在本文中對圖像進行光度標定的效果如圖2所示，圖像質量得到了明顯改善，提取的邊緣特征分布更加均勻。

圖2 光度標定前后的圖像Fig.2 Images before and after photometric calibration

3 基于邊緣對準的視覺SLAM

視覺里程計通過提取經過光度標定的圖像中的邊緣，并利用距離變換進行邊緣對準，實現對相機運動的估計，并在后端構建滑動窗口對位姿再進行優化。

3.1 運動估計

對于在t時刻接收到的圖像It和深度圖像Zt，假設已完成了對準和時間同步，也即對于圖像It上的像素u=(x,y)，在Zt中的對應深度z=Zt(u)。

定義像素點u在三維空間對應的場景點為P,則從像素點u計算三維點P的逆投影函數如下：

其中，fx、fy、cx、cy為相機的內參，通過對相機標定獲得。對應地，將三維點P投影至像素坐標系的投影函數具有如下的形式：

式中，ξji為圖像幀Fi到Fj的相對變換，對應的位姿變換矩陣為Tji。

在每幀圖像中，通過對齊當前幀Fj與關鍵幀Fi對應的邊緣檢測結果來估計兩幀之間的相對運動ξji，在對齊過程中，將關鍵幀Fi中的具有有效深度的所有像素點εi重投影至當前幀Fj，并與當前幀檢測到的最近的邊緣像素對齊。在本文中通過在歐氏距離域上計算距離變換來預先計算圖像中每個像素到有效邊緣像素的距離，從而得到重投影的邊緣像素距離變換殘差：

其中，Dj為當前幀Fj的距離變換，ui為關鍵幀Fi中邊緣像素的位置。聯合參考關鍵幀中的所有具有有效深度的像素點的距離變換殘差，構建優化方程：

Huber權重系數的計算公式如下：

T-分布的計算公式如下：

式中，k、vσ、 均為根據傳感器誤差特性擬合得到的系數。

3.2 后端優化

為了提高算法的精度，在局部建圖線程構建滑動窗口對邊緣像素的深度、相機位姿和相機內參進行聯合優化。記滑動窗口中的圖像幀集合為W，將滑動窗口中的每一幀激活的邊緣像素點分別投影到其他幀上，類似于式(11)，得到：

式中，Ai為第i個關鍵幀中激活的殘差項集合，δr為誤差權重系數，r是包含優化后的相機內參C和逆深度參數ρi的殘差項：

其中，為利用優化后的關鍵幀相對世界的變換矩陣ξi,ξj重投影至關鍵幀Fj。通過Gauss-Newton優化方法最小化式(11)，從而有：

式中，W??n×n為對角權重矩陣，r為殘差序列，J為r的Jacobian矩陣，對式(14)迭代求解得到優化后的參數。

3.3 關鍵幀選擇及邊緣化

為了減少后端非線性優化的計算量，在前端跟蹤結束后，對于當前幀分別計算以下4個關鍵幀指標。

1）平均平方光流Mfov用于衡量視場內邊緣像素的整體光度變化。

式中u為邊緣像素坐標，u′為重投影后的像素坐標，n為邊緣像素的數量。

2）無旋轉的平均平方光流Mtran用于衡量因平移引起的光度變化。

式中ut為僅做平移變換后的像素坐標。

3）無位移的平均平方光流Mrota用于衡量因旋轉引起的光度變化。

式中ur為僅做旋轉變換后的像素坐標。

4）參考關鍵幀投影到當前幀的像素點數量Nin和未投影到當前幀的像素點數量Nout。

當以上指標符合式(20)(21)所示的條件之一時，將當前幀作為新的關鍵幀。

滑動窗口內的關鍵幀需要維持在一定的數量，因此需要采取邊緣化部分關鍵幀的策略，關鍵幀的邊緣化策略有兩種選擇方式。

1）該關鍵幀中的邊緣像素點在優化過程中激活的數量少于設置的閾值θvis，則該關鍵幀被邊緣化。

2）若不存在符合上述條件的關鍵幀，則保留最新的兩幀關鍵幀，對其余的關鍵幀計算幀間的歐氏距離得分si，邊緣化部分關鍵幀使其在空間上分布均勻。

式中,di,j為兩個關鍵幀之間的歐氏距離。

3.4 回環檢測

回環檢測分為發現回環候選幀，利用回環候選幀的信息修正位姿估計，再驗證回環檢測是否正確。本文的回環檢測是基于隨機蕨詞袋設計的，其流程如下。

2）利用回環信息修正位姿估計。當相似度得分大于設置的閾值θHam，則將當前幀視為回環候選幀加入到回環處理過程，以關鍵幀相對世界坐標系的位姿作為節點，滑動窗口關鍵幀之間的相對位姿作為邊，構建如下的位姿圖優化問題。圖3(a)展示了這一過程，其中箭頭方向表示將該幀投影到另一幀。

3）回環驗證。將由候選回環關鍵幀及其鄰近關鍵幀構成的回環驗證關鍵幀集合V投影到當前幀，計算邊緣投影重疊的數量，圖3(b)展示了這一過程。為了避免重復計算，對回環驗證關鍵幀集合中的關鍵幀先統計圖像中邊緣像素的出現次數。對于關鍵幀iF計算如下的計數地圖：

圖3 回環檢測示意圖Fig.3 Schematic diagram of loop detection

式中ξci為關鍵幀Fi到當前幀Fcur的位姿變換，N為回環驗證關鍵幀V中關鍵幀數量，對于整個回環驗證關鍵幀集合V的計數地圖有：

將候選回環關鍵幀中具有有效深度的邊緣像素點投影至計數地圖，對于集合V中每一幀，生成大小為N+1的直方圖來計算邊緣重疊數量。如果結果符合式(28)，則通過驗證。

式中，wi為權重系數。回環驗證通過后更新位姿。

4 實驗結果與分析

采用TUM RGB-D數據集驗證本文所提出的算法。針對光度標定、誤差權重系數、回環驗證的改進效果，并與ORB-SLAM3[4]進行對比。該數據集包含39個室內環境的圖像序列，使用Kinect相機以30 Hz的頻率錄制的，包含格式為BGR8的RGB圖像、格式為32FC的深度圖像，軌跡真值是通過具有16個高速攝像頭的動作捕捉系統以100 Hz獲得的，是驗證單目或基于RGB-D相機的視覺里程計性能常用的數據集。

實驗平臺為具有16 GB運行內存，AMD Ryzen7-4800U計算處理單元的個人電腦，操作系統為Ubuntu20.04.2LTS，ROS版本為noetic。將本文提出的兩種改進算法與RESLAM[14]、ORB-SLAM3[4]進行對比，驗證算法性能。兩種改進算法分別是集成光度在線標定利用Huber魯棒核函數構造殘差項系數的RESLAM（下文稱RESLAM_PMH）、集成光度標定并且利用T分布構造殘差項系數的RESLAM（下文稱RESLAM_PMT）。

本文采用的評價指標為絕對軌跡誤差（Absolute Trajectory Error,ATE）平移部分的均方根誤差（Root Mean Squared Error,RMSE）。時間點i的絕對軌跡誤差計算公式如下：

式中，S是估計位姿Pi到位姿真值Qi的剛體運動變換矩陣。本文中主要通過對比各算法的絕對軌跡誤差的大小說明算法的精度高低。

為了分析集成光度標定模塊后對算法性能的影響，并且為了避免隨機的數值波動對評估結果的影響，本文以數據集中的freg1/xyz序列采用RESLAM、RESLAM_PMH、RESLAM_PMT、ORB-SLAM3算法分別進行了20次實驗并求其絕對軌跡誤差的平均值，其結果如圖4所示。

圖4 算法的20次運行結果Fig.4 The resultsof 20runsof thealgorithms

由圖4可知，基于特征點法的ORB-SLAM3的算法性能在具有較為豐富紋理的freg1_xyz的序列上的運行結果最佳，而RESLAM_PMH、RESLAM_PMT經過光度標定后相對RESLAM都有較大的提升。且RESLAM_PMT在freg1/xyz序列上相較RESLAM和RESLAM_PMH的多次運行結果精度更高，更穩定。更進一步地展示實驗組中的其中這一組軌跡及其在x、y、z各軸的對比如圖5-6所示。

圖5 展示了三種算法估計的freg1/xyz軌跡，可以看出RESLAM_PMT估計的軌跡在序列的大部分段與真實軌跡較為接近，直觀地反映了該算法的改進對定位精度有所提升。圖6展示了各算法估計的軌跡與真實軌跡的x，y，z軸運動分量對比，可知RESLAM_PMT估計的運動軌跡的各軸分量與真實軌跡更為接近，在y軸和z軸上精度提升表現得更為明顯，圖中橫軸為以相對起始時刻的時間偏移量，起始時刻以科學計數的方式標注在橫軸末尾。

圖5 freg1/xyz序列軌跡Fig.5 freg1/xyz sequence trajector y

圖6 x、y、z軸運動軌跡分量Fig.6 x, y,z axismotiontrajectory comp onents

對freg3/nostructure_texture_near_withloop序列進行半稠密重建，圖7展示了三種算法的建圖效果，序列中存在回環使得RESLAM和RESLAM_PMH的建圖中紅色方框內重建的海報圖形有較大的重疊，而RESLAM_PMT則更為清晰。

圖7 三種算法的稀疏建圖Fig.7 The sparse mapping of three algorithms

在數據集中的多個序列上運行算法，其絕對軌跡誤差的均方根誤差如表1所示，其中的“/”表示只有部分軌跡，也即視覺定位失效時間較長。

從表1中可以看出ORB-SLAM3的算法性能在具有較多紋理的序列中保持著最佳的定位精度，但在freg3/structure_notexture_far序列中存在失效的情況。而本文所設計的RESLAM_PMH和RESLAM_PMT方法，在大多數序列上都優于原有的RESLAM方法，平均軌跡均方根誤差分別由0.073 m提高到0.053 m和0.033 m，分別提高27.3%和55.3%。

表1 各算法絕對軌跡誤差均方根誤差(單位：m)Tab.1 Root mean square error of absolute trajectory error of each algorithm(Unit: m)

5 結 論

本文提出了一種集成在線光度標定的邊緣對準視覺SLAM方法。首先集成在線光度標定設計了RESLAM_PMH算法，對圖像的漸暈效應、曝光時間進行估計，并依此校正圖像像素的強度，提升圖像的質量；并對位姿估計中殘差項的權重系數進行選擇，設計了RESLAM_PMT算法，提高系統對于較大殘差的抑制效果；綜合提升了算法的定位精度和建圖效果。

在數據集中的多個序列上進行多組對比實驗，驗證了集成在線光度標定后，基于邊緣對準的視覺SLAM算法定位精度和建圖效果均有所提升。同時與目前較為先進的基于特征點法的開源算法ORB-SLAM3進行對比，反映了基于邊緣對準算法在精度上略有不足，但在較為特殊的弱紋理環境下相較特征點法具有明顯優勢，這為后續的研究提供了較好的借鑒。