同時定位和建圖（SLAM）是一種重要的工具，它使移動機器人能夠在未知環(huán)境中自主地導(dǎo)航。正如SLAM這名稱所示，其核心為獲取環(huán)境的正確表示以及估計地圖中機器人位姿的正確軌跡。主流的最先進(jìn)的方法使用基于最小二乘法的圖優(yōu)化技術(shù)來求解位姿估計問題，其中最流行的方法為g2o、Ceres、GTSAM和SE-Sync等庫。本文的目的是以統(tǒng)一的方式來描述這些方法，并且在一系列公開可用的合成位姿圖數(shù)據(jù)集和真實世界的位姿圖數(shù)據(jù)集上對它們進(jìn)行評估。在評估實驗中，對四個優(yōu)化庫的計算時間和目標(biāo)函數(shù)值進(jìn)行分析。

介紹

基于濾波的方法已經(jīng)在SLAM場景中占據(jù)主導(dǎo)地位多年，其越來越多地被基于優(yōu)化的方法所取代。位姿圖優(yōu)化（PGO）首次在Lu等人的工作（Globally Consistent Range Scan Alignment for Environment Mapping）中被引入，但是由于計算效率低的原因，它并不是非常流行。如今，隨著計算能力的提高，PGO方法已經(jīng)成為最先進(jìn)的方法，并且能夠快速且精確地求解SLAM優(yōu)化和估計問題。

基于優(yōu)化的SLAM方法通常由兩部分組成。第一部分通過基于傳感器數(shù)據(jù)的對應(yīng)關(guān)系來識別新觀測數(shù)據(jù)和地圖之間的約束。第二部分計算給定約束情況下的機器人位姿和地圖，這能夠被分為圖和平滑方法。當(dāng)前最先進(jìn)的基于優(yōu)化方法的一個例子為g2o（A general framework for graph optimization），它是一種針對非線性最小二乘問題的通用優(yōu)化框架。最初的平滑方法之一為，它由Dellaert等人提出（Square root SAM： Simultaneous localization and mapping via square root information smoothing）。一種對該方法的改進(jìn)為增量式的平滑和建圖（iSAM），它由Kaess等人引入（iSAM： Incremental smoothing and mapping）。iSAM對進(jìn)行擴(kuò)展以通過更新稀疏平滑信息矩陣的因子分解為整個SLAM問題提供一種有效的求解方式。iSAM的升級版本iSAM2由Kaess等人提出（ISAM2： Incremental smoothing and mapping using the Bayes tree）。這些平滑方法在GTSAM（https://gtsam. org）中實現(xiàn)，它是另一個最先進(jìn)的優(yōu)化庫。

SLAM問題的一個很好的例子是所謂的位姿SLAM，它避免了構(gòu)建環(huán)境的顯式地圖。位姿SLAM的目標(biāo)是要在給定回環(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）。文獻(xiàn)中很少有比較這些方法的工作。例如，Latif等人提供了視覺SLAM的概述，并且將g2o、GTSAM和HOG-Man作為后端進(jìn)行比較。Carlone等人討論位姿圖估計中旋轉(zhuǎn)估計的重要性，并且在基準(zhǔn)數(shù)據(jù)集上將g2o和GTSAM與不同的旋轉(zhuǎn)估計技術(shù)進(jìn)行比較。Li等人在g2o框架下比較不同的優(yōu)化算法。Zhao等人提出統(tǒng)一的SLAM框架——GLSLAM，它提供了各種SLAM算法的實現(xiàn)，還能夠?qū)Σ煌腟LAM方法進(jìn)行基準(zhǔn)測試。然而，據(jù)我們所知，目前沒有一種統(tǒng)一的方式來比較g2o、Ceres、GTSAM和SE-Sync。本文的目的為以統(tǒng)一的方式來描述這些方法，并且在一系列公開可用的合成位姿圖數(shù)據(jù)集和真實世界位姿圖數(shù)據(jù)集上對它們進(jìn)行評估，如下圖所示。

在未來，我們想要通過這種比較來促進(jìn)PGO方法的選擇。

本文組織如下。第二節(jié)描述通用的非線性圖優(yōu)化以及這四種方法中的每一種。實驗是本文的主要部分，在第三節(jié)中進(jìn)行描述，在該節(jié)的第一部分，描述了硬件、實驗設(shè)備和基準(zhǔn)數(shù)據(jù)集。在該節(jié)末尾討論了實驗的結(jié)果。最后，第四節(jié)對本文進(jìn)行總結(jié)。

非線性位姿圖優(yōu)化方法

每個位姿圖由節(jié)點和邊組成。位姿圖中的節(jié)點對應(yīng)于環(huán)境中機器人的位姿，以及邊表示它們之間的空間約束。相鄰節(jié)點間的邊為里程計約束，以及其余邊表示回環(huán)約束。這在下圖圖（a）中可視化。

位姿圖優(yōu)化的目標(biāo)是要尋找一種節(jié)點的配置，使得位姿圖中所有約束的最小二乘誤差最小。一般而言，非線性最小二乘優(yōu)化問題能夠被定義如下：

其中，為圖中所有約束上的誤差總和：

這里，表示連接節(jié)點間索引對的集合，表示節(jié)點和節(jié)點之間的信息矩陣，為非線性誤差函數(shù)，它建模位姿和滿足由測量施加的約束的程度。最后，每個約束用信息矩陣和誤差函數(shù)進(jìn)行建模，如下圖圖（b）所示。

傳統(tǒng)上，公式（1）的解通過迭代優(yōu)化技術(shù)（例如，高斯-牛頓或者萊文貝格－馬夸特方法）獲取。他們的思想是用誤差函數(shù)在當(dāng)前初始估計附近的一階泰勒展開來近似該誤差函數(shù)。一般而言，它們由四個主要步驟組成：

1）固定一個初始估計值；

2）將問題近似為凸問題；

3）求解步驟2）并且把求解結(jié)果作為新的初始估計值；

4）重復(fù)步驟2）直到收斂。

位姿SLAM更容易求解，因為它沒有構(gòu)建環(huán)境的地圖。由圖構(gòu)建的問題具有稀疏的結(jié)構(gòu)，所以計算速度更快，另一個優(yōu)勢在于它對糟糕的初始估計值具有魯棒性。位姿SLAM的缺陷在于它通常對異常值不魯棒，并且當(dāng)存在很多錯誤回環(huán)時無法收斂。此外，旋轉(zhuǎn)估計使得它成為一個困難的非凸優(yōu)化問題，因此凸松弛導(dǎo)致問題具有局部極小解，并且無法保證全局最優(yōu)。在本節(jié)中，我們簡要地描述基于非線性最小二乘方法的優(yōu)化框架，這些框架以位姿圖的形式提供解決方案。

A.g2o

g2o是一個開源的通用框架，用于優(yōu)化能夠被定義為圖的非線性函數(shù)。它的優(yōu)勢在于易于擴(kuò)展、高效并且適用于廣泛的問題。作者在他們的工作中指出，他們的系統(tǒng)與其它最先進(jìn)的SLAM算法相比，同時具有高度通用性和可擴(kuò)展性。他們通過利用圖的稀疏連接和特殊結(jié)構(gòu)、使用改進(jìn)的方法來求解稀疏線性系統(tǒng)并且利用現(xiàn)代處理器的特性，從而實現(xiàn)高效性。該框架包含三種不同的方法來求解PGO：Gauss-Newton、Levenberg-Marquardt和Powell的Dogleg方法。框架主要用于求解機器人中的SLAM問題和計算機視覺中的bundle adjustment問題。ORB-SLAM使用g2o作為相機位姿優(yōu)化的后端，SVO將它用于視覺里程計。

B.Ceres

Ceres Solver是一個開源的C++庫，用于建模并且求解大規(guī)模、復(fù)雜的優(yōu)化問題。它主要致力于求解非線性最小二乘問題（bundle adjustment和SLAM），但是也能夠求解通用的無約束優(yōu)化問題。該框架容易使用、移植并且被廣泛優(yōu)化以提供具有低計算時間的求解質(zhì)量。Ceres被設(shè)計用于支持使用者定義并修改目標(biāo)函數(shù)和優(yōu)化求解器。實現(xiàn)的求解器包括置信域求解器（Levenberg-Marquardt，Powell的Dogleg）和線搜索求解器。由于Ceres具有很多優(yōu)勢，因此它被用于很多應(yīng)用和領(lǐng)域。OKVIS和VINS使用Ceres來優(yōu)化定義為圖的非線性問題。

C.GTSAM

GTSAM是另一個開源的C++庫，它實現(xiàn)了針對機器人和計算機視覺應(yīng)用的傳感器融合。GTSAM能夠被用于求解SLAM、視覺里程計和由運動恢復(fù)結(jié)構(gòu)（SfM）中的優(yōu)化問題，它使用因子圖來建模復(fù)雜的估計問題，并且利用它們的稀疏性來提高計算效率。GTSAM實現(xiàn)Levenberg-Marquardt和Gauss-Newton風(fēng)格的優(yōu)化器、共軛梯度優(yōu)化器、Dogleg和iSAM（增量式平滑和建圖）。GTSAM與學(xué)術(shù)界和工業(yè)界中各種傳感器前端一起使用。例如，SVO的一種變體（On-Manifold Preintegration for Real-Time Visual-Inertial Odometry）使用GTSAM作為視覺里程計的后端。

D.SE-Sync

SE-Sync是一種被證實正確的算法，用于在特殊歐式群上執(zhí)行同步。SE-Sync的目標(biāo)是在給定節(jié)點間相對變化的噪聲測量情況下估計一組未知的位姿（歐氏空間中位置和姿態(tài)）值，它的主要應(yīng)用是在2D和3D幾何估計，例如位姿圖SLAM（機器人中）、相機運動估計（計算機視覺中）和傳感器網(wǎng)絡(luò)定位（分布式感知中）。作者在其工作中指出，SE-Sync通過利用一種新型的特殊歐式同步問題的（凸）半正定松弛來直接搜索全局最優(yōu)的解以改進(jìn)之前的方法，并且能夠?qū)τ谡业降慕馍烧_性的計算證明。他們應(yīng)用截斷的牛頓黎曼置信域方法來尋找位姿的高效估計。

實驗

本文的目標(biāo)是要通過實驗評估上一節(jié)中描述的優(yōu)化框架，并且比較它們。出于這個目的，本文在總計算時間和目標(biāo)函數(shù)的最終值（由公式（2）描述）方面考慮它們的性能。本文使用公開可用的合成基準(zhǔn)數(shù)據(jù)集和真實世界的基準(zhǔn)數(shù)據(jù)集。

A.實驗設(shè)備

實驗是在配備有八核Intel Core i7-6700HQ CPU、2.60GHz運行頻率和16GB內(nèi)存的Lenovo ThinkPad P50上進(jìn)行的。該計算機運行Ubuntu 20.04系統(tǒng)。g2o、Ceres和GTSAM框架選擇相同的求解器Levenberg-Marquardt，而SE-Sync使用黎曼置信域（RTR）方法。每種算法限制最大迭代100次。停止條件基于達(dá)到最大迭代次數(shù)和相對誤差減少。SE-Sync使用一種稍微不同的方法，所以必須指定一種額外的基于黎曼梯度范數(shù)的條件。相對誤差減少的公差范圍被設(shè)置為，以及梯度的范數(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)化過程之前進(jìn)行位姿圖初始化。為了取得更好的結(jié)果，我們使用生成樹方法來獲取初始的位姿圖。

B.基準(zhǔn)數(shù)據(jù)集

Carlone等人獲取六個兩維的位姿圖，包括兩個真實世界的位姿圖和四個在仿真中創(chuàng)建的位姿圖。INTEL和MIT位姿圖是真實世界的數(shù)據(jù)集，它們通過處理在西雅圖Intel Research Lab和MIT Killian Court采集的原始輪式里程計和激光雷達(dá)傳感器測量數(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ù)中加入高斯噪聲。噪聲的標(biāo)準(zhǔn)偏差分別為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.openslam.org/）。其它三個位姿圖數(shù)據(jù)集是真實世界的數(shù)據(jù)集。Kummerle等人引入Garage數(shù)據(jù)集（Autonomous driving in a multi-level parking structure），Cubicle和Rim數(shù)據(jù)集是通過在來自佐治亞理工學(xué)院RIM中心的3D激光傳感器的點云上使用ICP方法來獲取的。所有這些位姿圖在本文第一張圖中可視化，圖中顯示它們的里程計和回環(huán)約束。表格I中包含每個數(shù)據(jù)集的節(jié)點和邊的數(shù)量。這些數(shù)量確定了優(yōu)化過程中參數(shù)的數(shù)量和問題的復(fù)雜度。

C.結(jié)果

本文對于總計算時間和目標(biāo)函數(shù)值（公式（2））方面在表格II中總結(jié)了所有的性能結(jié)果。對于每一個數(shù)據(jù)集，我們指出算法的終止原因。如果算法在最大迭代次數(shù)的限制內(nèi)完成優(yōu)化，則該算法收斂。

我們還給出SE-Sync的驗證時間以證明全局最優(yōu)。下面三張圖展示針對基準(zhǔn)測試問題的優(yōu)化后的位姿圖。

1）INTEL：INTEL是最易求解的問題之一。所有方法均已經(jīng)成功對其進(jìn)行求解。所有軌跡顯示在下圖圖（a）中，并且由圖可知它們均獲得了相似的結(jié)果。GTSAM耗費最長的時間來完成優(yōu)化，但是取得最低的目標(biāo)函數(shù)值。在這種情況下，SE-Sync速度最快，并且其目標(biāo)函數(shù)值略大于GTSAM?？紤]到這一點，SE-Sync似乎是最好的求解方案。

2）MIT：MIT是最小的問題，但是僅具有一些回環(huán)約束。因此，從一個好的初始估計值開始優(yōu)化是非常重要的。否則，GTSAM、Ceres和g2o無法通過Levenberg-Marquardt算法收斂到一個有意義的解。所有算法在最大迭代次數(shù)內(nèi)收斂，并且它們獲得幾乎相同的目標(biāo)函數(shù)值。優(yōu)化問題在半秒內(nèi)求解，并且Ceres和g2o速度最快。Ceres還獲得最低的目標(biāo)函數(shù)值，因此似乎是數(shù)據(jù)集MIT的最佳求解器。最終的軌跡如下圖圖（b）所示。

3）M3500：M3500的所有四種變體均在這里展示。GTSAM和Ceres在求解基礎(chǔ)的M3500問題中表現(xiàn)最佳，因為它們均取得最低的目標(biāo)函數(shù)值，但是Ceres速度稍微更快。所有方法都能成功求解該問題，并且它們優(yōu)化后的位姿圖如下圖圖（c）所示。M3500a是一個更為困難的問題，因為它將噪聲加入到相對姿態(tài)中。盡管如此，所有方法均能求解它，但是如下圖圖（d）所示，g2o解決方案偏離了其它的解決方案。Ceres和GTSAM收斂到最低的目標(biāo)函數(shù)值，但是Ceres再次速度更快。

M3500b和M3500c是非常有挑戰(zhàn)性的問題，因為其噪聲量很高。GTSAM在這兩種情況下沒有收斂，并且Ceres和g2o陷入局部極小值。SE-Sync是求解這個問題兩種變體的最為成功的方法?？紤]到缺少連接位姿圖左邊部分和右邊部分的回環(huán)約束以及測量中的噪聲量，SE-Sync已經(jīng)實現(xiàn)相當(dāng)好的解決方案。這些位姿圖如下圖圖（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ù)集更為復(fù)雜并且耗時，這是由于它包含大量的節(jié)點和邊，但是所有方法均找到一個解。SE-Sync是迄今為止求解該問題速度最快的方法，它耗費8秒，而其它的方法耗費超過一分鐘。最終的解如下圖圖（b）所示。

8）Garage：Garage數(shù)據(jù)集是最小且最容易求解的3D數(shù)據(jù)集，每種方法均能獲得一個解，如下圖圖（c）所示。Ceres、GTSAM和SE-Sync收斂到相同的目標(biāo)函數(shù)值，GTSAM在這種情況下速度最快。

總結(jié)

在本文中，我們比較用于SLAM中位姿估計的圖優(yōu)化方法。我們考慮g2o和GTSAM（它們是當(dāng)前最先進(jìn)的方法）、一個用戶友好的開源框架Ceres和一個用于位姿同步的新型且魯棒的方法SE-Sync。評估過程考慮了耗費的優(yōu)化時間和目標(biāo)函數(shù)值，12個基準(zhǔn)數(shù)據(jù)集的結(jié)果以表格的形式給出。

當(dāng)與其它三種方法比較時，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)相當(dāng)不錯。如果前端非常出色、數(shù)據(jù)集相對簡單或者噪聲較低，那么在這些后端中做出選擇是個人偏好的問題。我們希望這種比較能夠幫助其他研究者為他們的SLAM應(yīng)用選擇一種后端方法。

審核編輯：郭婷