論文鏈接：https://lamor./images/50036607/2021-ajuric-comparison-mipro.pdf摘要同時定位和建圖（SLAM）是一種重要的工具,，它使移動機器人能夠在未知環(huán)境中自主地導航,。正如SLAM這名稱所示，其核心為獲取環(huán)境的正確表示以及估計地圖中機器人位姿的正確軌跡,。主流的最先進的方法使用基于最小二乘法的圖優(yōu)化技術(shù)來求解位姿估計問題,，其中最流行的方法為g2o、Ceres,、GTSAM和SE-Sync等庫,。本文的目的是以統(tǒng)一的方式來描述這些方法，并且在一系列公開可用的合成位姿圖數(shù)據(jù)集和真實世界的位姿圖數(shù)據(jù)集上對它們進行評估,。在評估實驗中,，對四個優(yōu)化庫的計算時間和目標函數(shù)值進行分析。介紹基于濾波的方法已經(jīng)在SLAM場景中占據(jù)主導地位多年,，其越來越多地被基于優(yōu)化的方法所取代,。位姿圖優(yōu)化（PGO）首次在Lu等人的工作（Globally Consistent Range Scan Alignment for Environment Mapping）中被引入，但是由于計算效率低的原因,，它并不是非常流行,。如今，隨著計算能力的提高,，PGO方法已經(jīng)成為最先進的方法,，并且能夠快速且精確地求解SLAM優(yōu)化和估計問題,。基于優(yōu)化的SLAM方法通常由兩部分組成。第一部分通過基于傳感器數(shù)據(jù)的對應關(guān)系來識別新觀測數(shù)據(jù)和地圖之間的約束,。第二部分計算給定約束情況下的機器人位姿和地圖,，這能夠被分為圖和平滑方法。當前最先進的基于優(yōu)化方法的一個例子為g2o（A general framework for graph optimization）,，它是一種針對非線性最小二乘問題的通用優(yōu)化框架,。最初的平滑方法之一為，它由Dellaert等人提出（Square root SAM: Simultaneous localization and mapping via square root information smoothing）,。一種對該方法的改進為增量式的平滑和建圖（iSAM）,，它由Kaess等人引入（iSAM: Incremental smoothing and mapping）。iSAM對進行擴展以通過更新稀疏平滑信息矩陣的因子分解為整個SLAM問題提供一種有效的求解方式,。iSAM的升級版本iSAM2由Kaess等人提出（ISAM2: Incremental smoothing and mapping using the Bayes tree）,。這些平滑方法在GTSAM（https://gtsam. org）中實現(xiàn)，它是另一個最先進的優(yōu)化庫,。SLAM問題的一個很好的例子是所謂的位姿SLAM,，它避免了構(gòu)建環(huán)境的顯式地圖。位姿SLAM的目標是要在給定回環(huán)和里程計約束的情況下估計機器人的軌跡,。這些相對的位姿測量通常使用自身運動估計,、掃描匹配、迭代最近點（ICP）或者一些最小化視覺重投影誤差的形式,，從IMU,、激光傳感器、相機或者輪式里程計中獲取,。值得注意的是,，Eustice等人和Lenac等人提出的方法為基于濾波的位姿SLAM算法，但是本文的關(guān)注點為實現(xiàn)基于優(yōu)化的位姿SLAM的方法,。最流行的優(yōu)化方法有g(shù)2o,、Ceres Solver（http://
ceres-solver.org）、GTSAM和SE-Sync（特殊歐式群SE(n)上的同步）（SE-Sync: A certifiably correct algorithm for synchronization over the special Euclidean group）,。文獻中很少有比較這些方法的工作,。例如，Latif等人提供了視覺SLAM的概述,，并且將g2o,、GTSAM和HOG-Man作為后端進行比較。Carlone等人討論位姿圖估計中旋轉(zhuǎn)估計的重要性,，并且在基準數(shù)據(jù)集上將g2o和GTSAM與不同的旋轉(zhuǎn)估計技術(shù)進行比較,。Li等人在g2o框架下比較不同的優(yōu)化算法。Zhao等人提出統(tǒng)一的SLAM框架——GLSLAM,，它提供了各種SLAM算法的實現(xiàn),，還能夠?qū)Σ煌腟LAM方法進行基準測試,。然而，據(jù)我們所知,，目前沒有一種統(tǒng)一的方式來比較g2o,、Ceres、GTSAM和SE-Sync,。本文的目的為以統(tǒng)一的方式來描述這些方法,，并且在一系列公開可用的合成位姿圖數(shù)據(jù)集和真實世界位姿圖數(shù)據(jù)集上對它們進行評估，如下圖所示,。在未來,，我們想要通過這種比較來促進PGO方法的選擇。本文組織如下,。第二節(jié)描述通用的非線性圖優(yōu)化以及這四種方法中的每一種,。實驗是本文的主要部分，在第三節(jié)中進行描述,，在該節(jié)的第一部分,，描述了硬件,、實驗設備和基準數(shù)據(jù)集,。在該節(jié)末尾討論了實驗的結(jié)果。最后,，第四節(jié)對本文進行總結(jié),。非線性位姿圖優(yōu)化方法每個位姿圖由節(jié)點和邊組成。位姿圖中的節(jié)點對應于環(huán)境中機器人的位姿,，以及邊表示它們之間的空間約束,。相鄰節(jié)點間的邊為里程計約束，以及其余邊表示回環(huán)約束,。這在下圖圖（a）中可視化,。位姿圖優(yōu)化的目標是要尋找一種節(jié)點的配置，使得位姿圖中所有約束的最小二乘誤差最小,。一般而言,，非線性最小二乘優(yōu)化問題能夠被定義如下：這里,，表示連接節(jié)點間索引對的集合,，表示節(jié)點和節(jié)點之間的信息矩陣，為非線性誤差函數(shù),，它建模位姿和滿足由測量施加的約束的程度,。最后，每個約束用信息矩陣和誤差函數(shù)進行建模,，如下圖圖（b）所示,。傳統(tǒng)上,，公式（1）的解通過迭代優(yōu)化技術(shù)（例如，高斯-牛頓或者萊文貝格－馬夸特方法）獲取,。他們的思想是用誤差函數(shù)在當前初始估計附近的一階泰勒展開來近似該誤差函數(shù),。一般而言，它們由四個主要步驟組成： 3）求解步驟2）并且把求解結(jié)果作為新的初始估計值； 位姿SLAM更容易求解,，因為它沒有構(gòu)建環(huán)境的地圖。由圖構(gòu)建的問題具有稀疏的結(jié)構(gòu),，所以計算速度更快,，另一個優(yōu)勢在于它對糟糕的初始估計值具有魯棒性。位姿SLAM的缺陷在于它通常對異常值不魯棒,，并且當存在很多錯誤回環(huán)時無法收斂,。此外，旋轉(zhuǎn)估計使得它成為一個困難的非凸優(yōu)化問題,，因此凸松弛導致問題具有局部極小解,，并且無法保證全局最優(yōu)。在本節(jié)中,，我們簡要地描述基于非線性最小二乘方法的優(yōu)化框架,，這些框架以位姿圖的形式提供解決方案。g2o是一個開源的通用框架,，用于優(yōu)化能夠被定義為圖的非線性函數(shù),。它的優(yōu)勢在于易于擴展、高效并且適用于廣泛的問題,。作者在他們的工作中指出,，他們的系統(tǒng)與其它最先進的SLAM算法相比，同時具有高度通用性和可擴展性,。他們通過利用圖的稀疏連接和特殊結(jié)構(gòu),、使用改進的方法來求解稀疏線性系統(tǒng)并且利用現(xiàn)代處理器的特性，從而實現(xiàn)高效性,。該框架包含三種不同的方法來求解PGO：Gauss-Newton,、Levenberg-Marquardt和Powell的Dogleg方法?？蚣苤饕糜谇蠼鈾C器人中的SLAM問題和計算機視覺中的bundle adjustment問題,。ORB-SLAM使用g2o作為相機位姿優(yōu)化的后端，SVO將它用于視覺里程計,。Ceres Solver是一個開源的C++庫,，用于建模并且求解大規(guī)模,、復雜的優(yōu)化問題。它主要致力于求解非線性最小二乘問題（bundle adjustment和SLAM）,，但是也能夠求解通用的無約束優(yōu)化問題,。該框架容易使用、移植并且被廣泛優(yōu)化以提供具有低計算時間的求解質(zhì)量,。Ceres被設計用于支持使用者定義并修改目標函數(shù)和優(yōu)化求解器,。實現(xiàn)的求解器包括置信域求解器（Levenberg-Marquardt，Powell的Dogleg）和線搜索求解器,。由于Ceres具有很多優(yōu)勢,，因此它被用于很多應用和領域。OKVIS和VINS使用Ceres來優(yōu)化定義為圖的非線性問題,。GTSAM是另一個開源的C++庫,，它實現(xiàn)了針對機器人和計算機視覺應用的傳感器融合。GTSAM能夠被用于求解SLAM,、視覺里程計和由運動恢復結(jié)構(gòu)（SfM）中的優(yōu)化問題,，它使用因子圖來建模復雜的估計問題，并且利用它們的稀疏性來提高計算效率,。GTSAM實現(xiàn)Levenberg-Marquardt和Gauss-Newton風格的優(yōu)化器,、共軛梯度優(yōu)化器、Dogleg和iSAM（增量式平滑和建圖）,。GTSAM與學術(shù)界和工業(yè)界中各種傳感器前端一起使用,。例如，SVO的一種變體（On-Manifold Preintegration for Real-Time Visual-Inertial Odometry）使用GTSAM作為視覺里程計的后端,。SE-Sync是一種被證實正確的算法，用于在特殊歐式群上執(zhí)行同步,。SE-Sync的目標是在給定節(jié)點間相對變化的噪聲測量情況下估計一組未知的位姿（歐氏空間中位置和姿態(tài)）值,，它的主要應用是在2D和3D幾何估計，例如位姿圖SLAM（機器人中）,、相機運動估計（計算機視覺中）和傳感器網(wǎng)絡定位（分布式感知中）,。作者在其工作中指出，SE-Sync通過利用一種新型的特殊歐式同步問題的（凸）半正定松弛來直接搜索全局最優(yōu)的解以改進之前的方法,，并且能夠?qū)τ谡业降慕馍烧_性的計算證明,。他們應用截斷的牛頓黎曼置信域方法來尋找位姿的高效估計。實驗本文的目標是要通過實驗評估上一節(jié)中描述的優(yōu)化框架,，并且比較它們,。出于這個目的，本文在總計算時間和目標函數(shù)的最終值（由公式（2）描述）方面考慮它們的性能,。本文使用公開可用的合成基準數(shù)據(jù)集和真實世界的基準數(shù)據(jù)集,。實驗是在配備有八核Intel Core i7-6700HQ CPU,、2.60GHz運行頻率和16GB內(nèi)存的Lenovo ThinkPad P50上進行的。該計算機運行Ubuntu 20.04系統(tǒng),。g2o,、Ceres和GTSAM框架選擇相同的求解器Levenberg-Marquardt，而SE-Sync使用黎曼置信域（RTR）方法,。每種算法限制最大迭代100次,。停止條件基于達到最大迭代次數(shù)和相對誤差減少。SE-Sync使用一種稍微不同的方法,，所以必須指定一種額外的基于黎曼梯度范數(shù)的條件,。相對誤差減少的公差范圍被設置為，以及梯度的范數(shù)被設置為,。根據(jù)SE-Sync工作中的建議來選擇參數(shù),。Carlone等人研究姿態(tài)初始化對于尋找全局最優(yōu)的影響（Initialization techniques for 3D SLAM: A survey on rotation estimation and its use in pose graph optimization）。受到該工作的啟發(fā),，我們還在優(yōu)化過程之前進行位姿圖初始化,。為了取得更好的結(jié)果，我們使用生成樹方法來獲取初始的位姿圖,。Carlone等人獲取六個兩維的位姿圖,，包括兩個真實世界的位姿圖和四個在仿真中創(chuàng)建的位姿圖。INTEL和MIT位姿圖是真實世界的數(shù)據(jù)集,，它們通過處理在西雅圖Intel Research Lab和MIT Killian Court采集的原始輪式里程計和激光雷達傳感器測量數(shù)據(jù)來創(chuàng)建,。M3500位姿圖是一個仿真的曼哈頓世界（Fast iterative alignment of pose graphs with poor initial estimates）。M3500a,、M3500b和M3500c數(shù)據(jù)集是M3500數(shù)據(jù)集的變體,，它們在相對姿態(tài)測量數(shù)據(jù)中加入高斯噪聲。噪聲的標準偏差分別為0.1,、0.2和0.3弧度,。Carlone等人還獲取六個三維的數(shù)據(jù)集。位姿圖Sphere-a,、Torus和Cube在仿真中創(chuàng)建,，其中Sphere-a數(shù)據(jù)集是由Stachniss等人發(fā)布的一個有挑戰(zhàn)性的問題（http://www./）。其它三個位姿圖數(shù)據(jù)集是真實世界的數(shù)據(jù)集,。Kummerle等人引入Garage數(shù)據(jù)集（Autonomous driving in a multi-level parking structure）,，Cubicle和Rim數(shù)據(jù)集是通過在來自佐治亞理工學院RIM中心的3D激光傳感器的點云上使用ICP方法來獲取的。所有這些位姿圖在本文第一張圖中可視化,，圖中顯示它們的里程計和回環(huán)約束,。表格I中包含每個數(shù)據(jù)集的節(jié)點和邊的數(shù)量。這些數(shù)量確定了優(yōu)化過程中參數(shù)的數(shù)量和問題的復雜度。本文對于總計算時間和目標函數(shù)值（公式（2））方面在表格II中總結(jié)了所有的性能結(jié)果,。對于每一個數(shù)據(jù)集,，我們指出算法的終止原因。如果算法在最大迭代次數(shù)的限制內(nèi)完成優(yōu)化,，則該算法收斂,。我們還給出SE-Sync的驗證時間以證明全局最優(yōu)。下面三張圖展示針對基準測試問題的優(yōu)化后的位姿圖,。1）INTEL：INTEL是最易求解的問題之一,。所有方法均已經(jīng)成功對其進行求解。所有軌跡顯示在下圖圖（a）中,，并且由圖可知它們均獲得了相似的結(jié)果,。GTSAM耗費最長的時間來完成優(yōu)化，但是取得最低的目標函數(shù)值,。在這種情況下,，SE-Sync速度最快，并且其目標函數(shù)值略大于GTSAM,?？紤]到這一點，SE-Sync似乎是最好的求解方案,。2）MIT：MIT是最小的問題,，但是僅具有一些回環(huán)約束。因此,，從一個好的初始估計值開始優(yōu)化是非常重要的,。否則，GTSAM,、Ceres和g2o無法通過Levenberg-Marquardt算法收斂到一個有意義的解,。所有算法在最大迭代次數(shù)內(nèi)收斂，并且它們獲得幾乎相同的目標函數(shù)值,。優(yōu)化問題在半秒內(nèi)求解,，并且Ceres和g2o速度最快。Ceres還獲得最低的目標函數(shù)值,，因此似乎是數(shù)據(jù)集MIT的最佳求解器。最終的軌跡如下圖圖（b）所示,。3）M3500：M3500的所有四種變體均在這里展示,。GTSAM和Ceres在求解基礎的M3500問題中表現(xiàn)最佳，因為它們均取得最低的目標函數(shù)值,，但是Ceres速度稍微更快,。所有方法都能成功求解該問題，并且它們優(yōu)化后的位姿圖如下圖圖（c）所示。M3500a是一個更為困難的問題,，因為它將噪聲加入到相對姿態(tài)中,。盡管如此，所有方法均能求解它,，但是如下圖圖（d）所示,，g2o解決方案偏離了其它的解決方案。Ceres和GTSAM收斂到最低的目標函數(shù)值,，但是Ceres再次速度更快,。M3500b和M3500c是非常有挑戰(zhàn)性的問題，因為其噪聲量很高,。GTSAM在這兩種情況下沒有收斂,，并且Ceres和g2o陷入局部極小值。SE-Sync是求解這個問題兩種變體的最為成功的方法,?？紤]到缺少連接位姿圖左邊部分和右邊部分的回環(huán)約束以及測量中的噪聲量，SE-Sync已經(jīng)實現(xiàn)相當好的解決方案,。這些位姿圖如下圖圖（a）和下圖圖（b）所示,。4）Sphere-a：Sphere-a問題也具有挑戰(zhàn)性，因為大量噪聲被加入到相對姿態(tài)測量數(shù)據(jù)中,。即使給出一個初始估計值,，僅SE-Sync能夠求解它，它僅在0.15s內(nèi)收斂到全局最優(yōu),，比g2o速度快50倍并且比Ceres和GTSAM速度快100倍,。由SE-Sync獲得的最優(yōu)解如上圖圖（c）所示，而其它三種方法獲得局部最優(yōu)解,。5）Cubicle和Rim：Cubicle數(shù)據(jù)集是Rim數(shù)據(jù)集的一個子集,，所以本文一起討論它們。這兩個數(shù)據(jù)集具有挑戰(zhàn)性,，因為它們都包含大量的節(jié)點和邊,。此外，大量的邊為錯誤的回環(huán),。僅GTSAM和SE-Sync在這種情況下收斂到一個解,。優(yōu)化后的位姿圖如上圖圖（d）和上圖圖（e）所示。Ceres和g2o無法找到一個解,。GTSAM求解Cubicle的速度比SE-Sync快兩倍,，但是SE-Sync收斂到全局最優(yōu)。SE-Sync優(yōu)化Rim的速度比GTSAM快五倍,，并且收斂到全局最優(yōu),。6）Torus：Torus是最容易的3D問題之一,。除了g2o之外的所有方法均收斂到全局最優(yōu)，但是SE-Sync再次在速度方面獲勝,。雖然g2o解決了該問題,，但是由下圖圖（a）所示，可以看出它優(yōu)化后的軌跡與其它三種方法相比略有漂移,。7）Cube：Cube數(shù)據(jù)集更為復雜并且耗時,，這是由于它包含大量的節(jié)點和邊，但是所有方法均找到一個解,。SE-Sync是迄今為止求解該問題速度最快的方法,，它耗費8秒，而其它的方法耗費超過一分鐘,。最終的解如下圖圖（b）所示,。8）Garage：Garage數(shù)據(jù)集是最小且最容易求解的3D數(shù)據(jù)集，每種方法均能獲得一個解,，如下圖圖（c）所示,。Ceres、GTSAM和SE-Sync收斂到相同的目標函數(shù)值,，GTSAM在這種情況下速度最快,。總結(jié)在本文中，我們比較用于SLAM中位姿估計的圖優(yōu)化方法,。我們考慮g2o和GTSAM（它們是當前最先進的方法）,、一個用戶友好的開源框架Ceres和一個用于位姿同步的新型且魯棒的方法SE-Sync。評估過程考慮了耗費的優(yōu)化時間和目標函數(shù)值,，12個基準數(shù)據(jù)集的結(jié)果以表格的形式給出,。當與其它三種方法比較時，SE-Sync在大多數(shù)數(shù)據(jù)集上取得最少的總耗時,。如果需要,，它還能夠驗證全局最優(yōu)，但是以額外的計算時間為代價,。g2o具有最高的總耗時,，但是在簡單的2D數(shù)據(jù)集上表現(xiàn)良好。Ceres易于使用,，提供很多的靈活性,，并且速度相對較快。GTSAM表現(xiàn)與SE-Sync幾乎一樣好,，除了在噪聲非常多的2D數(shù)據(jù)集上,。搭配一個正確的前端，這些方法在解決SLAM問題中能夠非常強大,。對于較差的數(shù)據(jù)關(guān)聯(lián),、高噪聲和一個表現(xiàn)較差的前端，似乎最好使用SE-Sync作為后端,。GTSAM具有好的初始化方法,，因此它似乎也表現(xiàn)相當不錯。如果前端非常出色,、數(shù)據(jù)集相對簡單或者噪聲較低,，那么在這些后端中做出選擇是個人偏好的問題。我們希望這種比較能夠幫助其他研究者為他們的SLAM應用選擇一種后端方法,。本文僅做學術(shù)分享,，如有侵權(quán)，請聯(lián)系刪文,。
|
