0. 這篇文章干了啥？

同時定位與地圖構建（SLAM）是一項關鍵技術，允許移動機器人在部分或完全未知的環境中自主導航。它包括使用機載傳感器同時估計機器人狀態和構建傳感器檢測到的環境地圖。SLAM可以根據傳感器和地圖構建技術的類別進行分類，如視覺SLAM、激光SLAM、慣性SLAM等。

解決SLAM問題的經典方法可以分為基于濾波的方法和基于圖的方法。在1986年提出SLAM問題的前二十年里，基于概率公式的濾波方法已經實現了準確的估計。然而，在大規模問題中更新協方差矩陣在計算上是昂貴的。基于圖的方法最早由Lu和Milios在1997年引入，隨著圖的增長，計算成本較低。隨著計算能力的提高，基于圖的SLAM的優化算法相比經典的濾波方法（如擴展卡爾曼濾波、Rao-Blackwellized粒子濾波和信息濾波）獲得了廣泛關注。Wilbers等人展示了基于圖的方法在定位方面比粒子濾波具有更高的精度。

姿態圖優化（PGO）可以建模為一個非凸優化問題，是基于圖的SLAM的基礎，其中它將每個姿態與一個頂點關聯，將每個測量與圖的一條邊關聯，需要從有噪聲的相對測量中估計多個未知姿態。在三維空間中的姿態通常包括旋轉和平移，旋轉可以使用歐拉角、軸角（so(3)）、特殊正交群（SO(3)）或四元數（Q）表示，平移由一個三維向量t指定。此外，整體姿態還可以使用特殊歐氏群（SE(3)）、李代數（se(3）或雙四元數（DQ）表示。不同的建模方法會產生不同的約束，如在se(3)中沒有約束，在SE(3)中有矩陣正交和行列式約束，或在Q中有球面約束。選擇與問題結構兼容的簡單表示將導致一個更容易解決和更準確的模型。

在過去的二十年中，許多模型已經根據噪聲的不同統計分布和姿態表示方法得到了發展。同時，也提出了許多高效的優化算法來解決這些模型。從模型的角度來看，旋轉噪聲的統計分布通常分為高斯分布或各向同性的馮米塞斯-費舍爾（vMF）分布，而平移噪聲統一表現為高斯噪聲。基于最大似然估計，在se(3)上的高斯噪聲可以直接導出一個無約束的非線性最小二乘模型。同樣，Cheng等人建立了基于單位雙四元數的最小二乘模型，并提出了一種更有效的方法來計算雅可比矩陣。通過消除兩個變量，他們的模型也是無約束的。另一種建模方法使用SO(3)表示旋轉，假定其服從vMF分布，并導出具有正交和行列式約束的模型。由于se(3)需要轉換來描述運動過程，用SO(3)或Q和一個三維向量表示的目標函數的表達式相比無約束模型更簡潔；然而，約束的引入增加了挑戰。

從算法的角度來看，提出了幾種高效且準確的方法來解決SLAM中的大規模問題。諸如隨機梯度下降等一階優化方法可以減少梯度計算的復雜性，并有效地解決無約束優化問題。收斂速度更快的算法，如高斯-牛頓方法、Levenberg--Marquardt方法、信賴域方法也被引入來解決該問題。與計算矩陣逆不同，使用QR或Cholesky分解等矩陣分解技術來降低復雜性，并提出了增量版本。Grisetti等人和Wagner等人提出了基于流形的高斯-牛頓算法，其中雅可比矩陣具有稀疏結構，更新過程避免了大規模線性方程系統的昂貴存儲。

然而，二階算法僅在局部區域具有快速收斂率，對于非凸問題通常返回局部極小值。后來的工作集中于找到更好的初始點并確認解的最優性。Rosen等人提出了一種基于Powell的Dog-Leg信賴域方法的穩健增量最小二乘估計，并提高了數值穩定性。Carlone等人通過檢查對偶間隙推導了一個帶約束的二次規劃并驗證了最優解。通過擴展可行集到其凸閉包，一種凸松弛方法有效地克服了非凸問題初始點選擇的難題。此外，Rosen等人將模型松弛為一個半定規劃，并證明了只要噪聲低于某個臨界閾值，其松弛的最小化結果提供了一個精確的最大似然估計。Fan和Murphey提出了PGO的一個上界，并通過廣義近端方法解決它，該方法可以收斂到一階臨界點且不依賴于黎曼梯度。另一種找到更好局部極小值或全局極小值的方法依賴于初始化技術。他們指出非凸旋轉估計是SLAM困難的真正原因，平移對旋轉估計影響較小。因此，計算一個好的旋轉估計將提高算法的性能。

我們提出了一種非凸姿態圖優化的近端線性化黎曼交替方向乘子法（PieADMM），它使用最新的部分信息更新其他變量。我們的子問題不僅具有閉式解，而且可以并行計算，從而使每次更新的時間復雜性較低。這一優勢在大規模數值實驗中得到了驗證。從理論上講，收斂性分析補充了我們的發現。

下面一起來閱讀一下這項工作~

1. 論文信息

標題：Non-convex Pose Graph Optimization in SLAM via Proximal Linearized Riemannian ADMM

作者：Xin Chen, Chunfeng Cui, Deren Han, Liqun Qi

機構：北京航空航天大學

原文鏈接：https://arxiv.org/abs/2404.18560

2. 摘要

位姿圖優化 (PGO) 是解決基于位姿的同時定位與地圖構建 (SLAM) 問題的一種著名技術。在本文中，我們使用單位四元數和三維向量表示旋轉和平移，并提出了一種基于馮·米塞斯-費舍爾分布的新型 PGO 模型。從單位四元數導出的約束是球面流形，投影到這些約束上可以通過歸一化來計算。然后，我們開發了一種近端線性化黎曼交替方向乘子法 (PieADMM) 來解決所提出的模型，該方法不僅具有低內存需求，而且可以并行更新位姿。此外，我們建立了 PieADMM 以 O(1/?2) 的迭代復雜度找到我們模型的 ?-駐點解。通過對兩個合成數據集和四個 3D SLAM 基準數據集的數值實驗，展示了我們所提出算法的效率。

3. 效果展示

首先，我們使用不同的算法測試n = 100，m = 100的環形數據集。圖2顯示了當σr = 0.01，σt = 0.05，并采用弦初始化時的俯視軌跡，三種方法在視覺上收斂于相同的解。我們還測試了里程計猜測初始化技術。由于恢復的軌跡幾乎重疊，并且很難觀察出差異，我們將它們省略了。

相反，我們在圖3中報告了優化過程，記錄了在不同方法和初始化技術下，Rel.Err和NRMSE的下降趨勢以及CPU時間。由于我們的PieADMM能夠針對每個頂點并行更新，因此它可以比其他方法更快地收斂。此外，弦初始化可以在旋轉更新后給出平移的估計，這提供了比其他方法更準確的初始點。在此初始化下，我們的PieADMM可以收斂到具有較低相對誤差的解。與里程計猜測初始化相比，我們的PieADMM通常不如mG-N的前幾步準確，但隨著迭代的進行，它可以實現略微更好的性能。因此，我們將弦初始化作為下一步實驗中的標準初始化技術。

4. 主要貢獻

(i) 我們提出了一種基于增強單位四元數和vMF分布的非凸姿態圖優化模型，其中數據存儲成本低，單位四元數的投影可以通過歸一化計算。

(ii) 我們提出了一種PieADMM，其子問題具有閉式解，并且可以并行更新。

(iii) 基于流形上的一階最優條件，我們定義了模型的一個?-駐點解。然后，我們建立了PieADMM在找到?-駐點解時的迭代復雜度O(1/?2)。

(iv) 我們在兩個不同數據規模的合成數據集和四個三維SLAM基準數據集上測試了我們的算法。數值實驗驗證了我們方法的有效性。

5. 基本原理是啥？

6. 實驗結果

我們在額外的噪聲水平下比較這些算法，并在表II中列出了關于Rel.Err、NRMSE和CPU時間的數值結果。我們發現PieADMM花費更少的時間并且獲得更好的結果。

我們還測試了姿態數量n的影響。實際上，由于我們限制了機器人軌跡的范圍，同等級別的噪聲將在頂點數量增加時產生更大的影響。因此，在比較不同n的數據大小的影響時，我們使用相對噪聲水平作為統一標準，這意味著σr = 100 × σrelr / n和σt = 100 × σrelt / n。結果如圖4所示。圖4a和4b顯示了PieADMM的性能平穩，有時略優于其他兩種方法。然而，PieADMM的運行時間增加速度比它們慢得多，參見圖4c。這是因為n的規模幾乎不影響旋轉子問題的成本，它可以并行計算。此外，平移子問題僅涉及矩陣乘法，并且不依賴于矩陣的逆。

對于立方體數據集，讓σt = σrelt /?n，其中σrelt表示平移的相對噪聲水平。我們首先考慮了?n = 5或8，σr = 0.1，σrelt = 0.1和pcube = 0.3的兩個例子。圖5a和5d顯示了真實軌跡，其中藍線由運動產生，紅色虛線由觀測產生。圖5b、5c和5e、5f分別是對應于不同?n的嘈雜和恢復的軌跡。圖6顯示了Rel.Err隨著CPU時間的下降趨勢，其中我們省略了圖像的上半部分以突出顯示細節。由于PGO模型是非凸的，而PieADMM是非單調的算法，曲線可能會振蕩。然而，它總是在更短的時間內收斂到更高精度的解。

我們還從2到10選擇?n，并在表III中展示了數值結果。圖7a顯示了立方體數據集的邊緣和頂點數量之間的關系，圖7b和7c說明了速度隨著?n的增加而上升的趨勢。mG-N和mL-M的成本增長都是立方的，而PieADMM的增長速度較慢。

我們測試了一些流行的3D SLAM數據集。車庫數據集是一個大規模的真實世界示例，另外三個（球1、球2和環面）是用來比較性能的常見數據集。與球1數據集不同，球2數據集添加了更大的噪聲。我們還使用弦初始化技術為所有方法計算了一個初始點。圖8顯示了軌跡的視覺結果，相應的數值結果列在表IV中。值得注意的是，我們的旋轉模型是基于vMF分布而不是傳統的高斯分布，因此恢復的解不相同，并且比較目標函數值或梯度是沒有意義的。我們在表中顯示了CPU時間，表明PieADMM收斂速度比mG-N和mL-M快。

7. 總結

在SLAM中的位姿圖優化是一種特殊的非凸優化，其中變量通常位于se(3)中，具有非線性目標函數，或在具有正交約束的特殊歐幾里得群中。復雜的模型使得找到全局解變得困難。本文提出了一種基于增強單位四元數和馮米塞斯-費舍爾分布的新非凸位姿圖優化模型，這是一個在單位球面上的大規模四次多項式優化。通過引入輔助變量，我們將其重新表述為多二次多項式優化、多線性最小二乘問題。然后，我們引入了一個針對PGO模型的近端線性化黎曼ADMM，其中子問題是簡單的投影問題，并且可以根據有向圖的結構并行解決，從而大大提高了效率。然后，基于我們PGO模型滿足的Lipschitz梯度連續性假設和流形上的一階最優性條件，我們建立了找到ε駐點解的迭代復雜度為O(1/?2)。在兩個具有不同數據規模和噪聲水平的合成數據集以及四個3D SLAM基準數據集上的數值實驗驗證了我們方法的有效性。