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

介紹

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

基于優化的SLAM方法通常由兩部分組成。第一部分通過基于傳感器數據的對應關系來識別新觀測數據和地圖之間的約束。第二部分計算給定約束情況下的機器人位姿和地圖，這能夠被分為圖和平滑方法。當前最先進的基于優化方法的一個例子為g2o（A general framework for graph optimization），它是一種針對非線性最小二乘問題的通用優化框架。最初的平滑方法之一為，它由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）中實現，它是另一個最先進的優化庫。

SLAM問題的一個很好的例子是所謂的位姿SLAM，它避免了構建環境的顯式地圖。位姿SLAM的目標是要在給定回環和里程計約束的情況下估計機器人的軌跡。這些相對的位姿測量通常使用自身運動估計、掃描匹配、迭代最近點（ICP）或者一些最小化視覺重投影誤差的形式，從IMU、激光傳感器、相機或者輪式里程計中獲取。值得注意的是，Eustice等人和Lenac等人提出的方法為基于濾波的位姿SLAM算法，但是本文的關注點為實現基于優化的位姿SLAM的方法。

最流行的優化方法有g2o、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等人討論位姿圖估計中旋轉估計的重要性，并且在基準數據集上將g2o和GTSAM與不同的旋轉估計技術進行比較。Li等人在g2o框架下比較不同的優化算法。Zhao等人提出統一的SLAM框架——GLSLAM，它提供了各種SLAM算法的實現，還能夠對不同的SLAM方法進行基準測試。然而，據我們所知，目前沒有一種統一的方式來比較g2o、Ceres、GTSAM和SE-Sync。本文的目的為以統一的方式來描述這些方法，并且在一系列公開可用的合成位姿圖數據集和真實世界位姿圖數據集上對它們進行評估，如下圖所示。

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

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

非線性位姿圖優化方法

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

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

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

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

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

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

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

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

4）重復步驟2）直到收斂。

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

A.g2o

g2o是一個開源的通用框架，用于優化能夠被定義為圖的非線性函數。它的優勢在于易于擴展、高效并且適用于廣泛的問題。作者在他們的工作中指出，他們的系統與其它最先進的SLAM算法相比，同時具有高度通用性和可擴展性。他們通過利用圖的稀疏連接和特殊結構、使用改進的方法來求解稀疏線性系統并且利用現代處理器的特性，從而實現高效性。該框架包含三種不同的方法來求解PGO：Gauss-Newton、Levenberg-Marquardt和Powell的Dogleg方法?？蚣苤饕糜谇蠼鈾C器人中的SLAM問題和計算機視覺中的bundle adjustment問題。ORB-SLAM使用g2o作為相機位姿優化的后端，SVO將它用于視覺里程計。

B.Ceres

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

C.GTSAM

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

D.SE-Sync

SE-Sync是一種被證實正確的算法，用于在特殊歐式群上執行同步。SE-Sync的目標是在給定節點間相對變化的噪聲測量情況下估計一組未知的位姿（歐氏空間中位置和姿態）值，它的主要應用是在2D和3D幾何估計，例如位姿圖SLAM（機器人中）、相機運動估計（計算機視覺中）和傳感器網絡定位（分布式感知中）。作者在其工作中指出，SE-Sync通過利用一種新型的特殊歐式同步問題的（凸）半正定松弛來直接搜索全局最優的解以改進之前的方法，并且能夠對于找到的解生成正確性的計算證明。他們應用截斷的牛頓黎曼置信域方法來尋找位姿的高效估計。

實驗

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

A.實驗設備

實驗是在配備有八核Intel Core i7-6700HQ CPU、2.60GHz運行頻率和16GB內存的Lenovo ThinkPad P50上進行的。該計算機運行Ubuntu 20.04系統。g2o、Ceres和GTSAM框架選擇相同的求解器Levenberg-Marquardt，而SE-Sync使用黎曼置信域（RTR）方法。每種算法限制最大迭代100次。停止條件基于達到最大迭代次數和相對誤差減少。SE-Sync使用一種稍微不同的方法，所以必須指定一種額外的基于黎曼梯度范數的條件。相對誤差減少的公差范圍被設置為，以及梯度的范數被設置為。根據SE-Sync工作中的建議來選擇參數。Carlone等人研究姿態初始化對于尋找全局最優的影響（Initialization techniques for 3D SLAM： A survey on rotation estimation and its use in pose graph optimization）。受到該工作的啟發，我們還在優化過程之前進行位姿圖初始化。為了取得更好的結果，我們使用生成樹方法來獲取初始的位姿圖。

B.基準數據集

Carlone等人獲取六個兩維的位姿圖，包括兩個真實世界的位姿圖和四個在仿真中創建的位姿圖。INTEL和MIT位姿圖是真實世界的數據集，它們通過處理在西雅圖Intel Research Lab和MIT Killian Court采集的原始輪式里程計和激光雷達傳感器測量數據來創建。M3500位姿圖是一個仿真的曼哈頓世界（Fast iterative alignment of pose graphs with poor initial estimates）。M3500a、M3500b和M3500c數據集是M3500數據集的變體，它們在相對姿態測量數據中加入高斯噪聲。噪聲的標準偏差分別為0.1、0.2和0.3弧度。Carlone等人還獲取六個三維的數據集。位姿圖Sphere-a、Torus和Cube在仿真中創建，其中Sphere-a數據集是由Stachniss等人發布的一個有挑戰性的問題（http://www.openslam.org/）。其它三個位姿圖數據集是真實世界的數據集。Kummerle等人引入Garage數據集（Autonomous driving in a multi-level parking structure），Cubicle和Rim數據集是通過在來自佐治亞理工學院RIM中心的3D激光傳感器的點云上使用ICP方法來獲取的。所有這些位姿圖在本文第一張圖中可視化，圖中顯示它們的里程計和回環約束。表格I中包含每個數據集的節點和邊的數量。這些數量確定了優化過程中參數的數量和問題的復雜度。

C.結果

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

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

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

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

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

M3500b和M3500c是非常有挑戰性的問題，因為其噪聲量很高。GTSAM在這兩種情況下沒有收斂，并且Ceres和g2o陷入局部極小值。SE-Sync是求解這個問題兩種變體的最為成功的方法。考慮到缺少連接位姿圖左邊部分和右邊部分的回環約束以及測量中的噪聲量，SE-Sync已經實現相當好的解決方案。這些位姿圖如下圖圖（a）和下圖圖（b）所示。

4）Sphere-a：Sphere-a問題也具有挑戰性，因為大量噪聲被加入到相對姿態測量數據中。即使給出一個初始估計值，僅SE-Sync能夠求解它，它僅在0.15s內收斂到全局最優，比g2o速度快50倍并且比Ceres和GTSAM速度快100倍。由SE-Sync獲得的最優解如上圖圖（c）所示，而其它三種方法獲得局部最優解。

5）Cubicle和Rim：Cubicle數據集是Rim數據集的一個子集，所以本文一起討論它們。這兩個數據集具有挑戰性，因為它們都包含大量的節點和邊。此外，大量的邊為錯誤的回環。僅GTSAM和SE-Sync在這種情況下收斂到一個解。優化后的位姿圖如上圖圖（d）和上圖圖（e）所示。Ceres和g2o無法找到一個解。GTSAM求解Cubicle的速度比SE-Sync快兩倍，但是SE-Sync收斂到全局最優。SE-Sync優化Rim的速度比GTSAM快五倍，并且收斂到全局最優。

6）Torus：Torus是最容易的3D問題之一。除了g2o之外的所有方法均收斂到全局最優，但是SE-Sync再次在速度方面獲勝。雖然g2o解決了該問題，但是由下圖圖（a）所示，可以看出它優化后的軌跡與其它三種方法相比略有漂移。

7）Cube：Cube數據集更為復雜并且耗時，這是由于它包含大量的節點和邊，但是所有方法均找到一個解。SE-Sync是迄今為止求解該問題速度最快的方法，它耗費8秒，而其它的方法耗費超過一分鐘。最終的解如下圖圖（b）所示。

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

總結

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

當與其它三種方法比較時，SE-Sync在大多數數據集上取得最少的總耗時。如果需要，它還能夠驗證全局最優，但是以額外的計算時間為代價。g2o具有最高的總耗時，但是在簡單的2D數據集上表現良好。Ceres易于使用，提供很多的靈活性，并且速度相對較快。GTSAM表現與SE-Sync幾乎一樣好，除了在噪聲非常多的2D數據集上。搭配一個正確的前端，這些方法在解決SLAM問題中能夠非常強大。對于較差的數據關聯、高噪聲和一個表現較差的前端，似乎最好使用SE-Sync作為后端。GTSAM具有好的初始化方法，因此它似乎也表現相當不錯。如果前端非常出色、數據集相對簡單或者噪聲較低，那么在這些后端中做出選擇是個人偏好的問題。我們希望這種比較能夠幫助其他研究者為他們的SLAM應用選擇一種后端方法。

審核編輯：郭婷