Efficient Sparse Pose Adjustment（SPA） for 2D Mapping 翻譯和總結

Konolige K , Grisetti G , Rainer Kümmerle, et al. Efficient sparse pose adjustment for 2D mapping[C]// 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2010.
這是一篇講解2D建圖中的高效稀疏矩陣圖優化的文章，谷歌的cartographer激光slam中位姿圖優化主要便是基於思想來實現的。文章主要貢獻是在$LM$優化過程中使用喬里斯基分解（Cholesky decomposition）實現稀疏$H$矩陣下的狀態增量$\Delta$x高效快速求解。

1. 摘要

圖優化已經成爲一種解決slam問題的主流方法。位姿圖是由一系列非線性約束連接機器人位姿點構成，這些約束來自於位姿點周圍共同的特徵觀測。由於直接非線性優化的計算時間與圖的大小的3次方成比例增長，優化大型位姿圖成爲移動機器人一個比較大的瓶頸問題。文章爲了解決這個問題，提出一種有效的方法去構建和解決其中的線性化子問題，這是常規直接非線性優化方法中的性能瓶頸所在。作者將文中所提方法命名爲SPA（Sparse Pose Adjustment ），實驗表明收斂速度和精度都要顯著由於其他方法。實驗採用大量的室內實際場景地圖數據和一個大型仿真數據集進行驗證了該方法的有效性。開源的C++代碼和數據集都已經對外公開。

2. 要點介紹

LM的核心問題是大型線性稀疏矩陣的求解。文章提出一種採用直接線性稀疏矩陣求解方法來高效計算約束圖的大型稀疏矩陣。作者稱爲稀疏姿態優化（SPA）方法, 因爲其屬於處理有限數量的pose間約束問題。SPA方法和圖優化相結合來解決線性子問題具有如下優勢：

• 考慮約束包含的協方差信息，可提高計算精度
• 在增量式和批量式求解中，SPA對於初始參數均比較魯棒，不容易失敗（陷入局部極小）
• 非常快的收斂速度，因爲在LM方法中只需要很少的迭代步驟
• 不像EKF或者信息濾波，SPA是完全非線性的，在每一次迭代中均會相對當前pose的所有約束進行線性化
• SPA在增量式和批量式處理時都是有效的

核心思想：文章提出一種採用直接稀疏喬里斯基分解求解線性系統的方法來有效處理2D位姿圖的優化問題。線性系統在求解過程中能夠有效減小內存消耗和內存泄漏，因此能夠顯著提升計算性能。

3. 系統結構

目前比較流行解決SLAM問題的方式是所謂的基於圖（Graph based）或者基於網絡(Network based)的方法, 主要思想是用圖來表示機器人歷史的測量結果。圖的每一個節點表示一個傳感器的測量值，或者一個子地圖，並同時包含該測量值是在哪個位置獲取的位置標籤。邊表示兩個連接節點之間的旋轉約束，通過該旋轉可以將兩個節點的測量值進行對齊。
圖優化中，典型的需要考慮兩個不同的問題。一個是基於傳感器數據確認約束關係，也被稱爲數據關聯問題。因爲環境存在模糊性和對稱性，這也是非常難的一件事情，通常SLAM系統中的前端（front-end）負責直接處理此數據關聯問題。另一個是基於獲取的約束關係去校正機器人的位姿從而得到一個一致的環境地圖。此部分工作一般在SLAM裏稱爲優化器或者後端（back-end），其目標是尋找一個節點間的配置關係使得節點間約束的測量概率最大。可以將其類比爲彈簧-質量塊，節點類比質量塊，彈簧類比邊的約束，彈簧和質量塊間的最小能量配置便類比建圖問題的一個solver。

在圖優化SLAM中，一般將前端和後端分開處理，如上圖2所示。這樣要求是因爲前端需要一個半優化狀態的地圖去限制潛在約束關係的搜索範圍。當前後端的估計越準，前端生成的約束關係越魯棒而且越快。因此，此優化問題的精度和性能，對整個建圖系統具有決定性的影響。
作者文中非常詳細的描述了一個高效處理2D位姿圖的完整優化方法。並提出該算法可以與其他處理各種傳感器的任意前端進行組合使用。爲了使得描述更加清晰，文中只是基於激光laser數據描述前端，但是一般的內容都可以直接應用在不同的傳感器中。

4. SPA (Sparse Pose Adjustment)

爲了優化一系列位姿和約束，作者使用LM（Levenberg-Marquardt）方法作爲優化框架，在2D建圖中遇到稀疏矩陣時通過特殊的實現來提高效率。相對於計算機視覺中通過LM有效處理相機和特徵的稀疏集束優化方法SBA（Sparse Bundle Adjustment), 作者稱自己的系統爲SPA。

4.1 誤差模型

系統的變量是機器人global系下的位姿$c$集合, 其由位移和方位角組成：
$c_i = [t_i, \theta_i] =[x_i, y_i, \theta_i]^T$
一個約束是從一個節點$c_i$到節點$c_j$間的測量值。測量所得的節點$c_i$和$c_j$之間在$c_i$系下的偏差稱爲$\bar{z}_{ij}$，此偏差同時具有精度矩陣$\Lambda_{ij}$(測量協方差的逆)。
對於真實的$c_i$和$c_j$間的偏差（待估計量），可以表示爲：
$h(c_i, c_j) = \begin{cases} R_i^T(t_j - t_i)\\ \theta_j - \theta_i \end{cases} \tag{1}$
上式（1）中$R_i$是$\theta_i$的2*2的旋轉矩陣，$h(c_i, c_j)$便被稱爲量測方程（描述待估計變量和測量值間關係的方程）。
誤差函數對應一個約束，繼而形成總的誤差如下：
$e_{ij} = \bar{z}_{ij} - h(c_i, c_j)\\ \chi^2(c, p) = \sum_{ij}e_{ij}^T\Lambda_{ij}e_{ij} \tag{2}$
上式$h(c_i, c_j)$中角度一般會被歸一化爲$(-\pi, \pi]$之間。

4.2 系統線性化

通過最小化上式（2）中的總誤差得到優化的節點位姿$c$。標準的解決方法便是LM，通過在當前位姿附近處迭代線性解來實現。線性求解系統的構建首先將變量$c$堆成一個向量$x$，以及誤差函數堆成$v$。接着定義：

構成的LM系統爲：
$(H + \lambda diagH)\Delta{x}= J^T\Lambda e \tag4$
此處的$\lambda$是一個在梯度下降和牛頓-歐拉法之間正係數因子。梯度下降法更加魯棒且不容易陷入局部極小，但是收斂比較慢；牛頓歐拉法剛好相反。
對每一次測量$h(c_i, c_j)$矩陣$H$會增加下面四個分量：

上式中$J_i$表示$e_{ij}$相對$c_i$的雅各比，上述分量都是33的矩陣塊。等式的右邊同時根據每個約束會增加31的矩陣塊$J_{c_i}\Lambda_{ij}e_{ij}$和$J_{c_j}\Lambda_{ij}e_{ij}$。
求解完上述線性系統可以得到一個增量$\Delta{x}$, 將該增量更新到當前的$x$：
$t_i = t_i + \Delta{t}\\ \theta_i = \theta_i + \Delta{\theta_i} \tag6$

4.3 誤差雅各比

等式(4)中的測量函數$h$的雅各比見下：

4.4 稀疏性

實際大型系統中，位姿pose $c$可能一萬多（甚至無限大）。目前的系統變量數量是3||c||， H大小是$||c||^2$，或許會超過$10^8$個元素。對這麼大的矩陣進行操作是非常消耗資源的。好在是，對於大部分場景，邊構成的約束與poses的數量成線性增長關係，因此H非常稀疏。此處可以利用大型矩陣的稀疏性高效率解決線性系統的求解問題。
作者考慮使用CSparse package以稀疏結構解決等式(4)所構建的線性系統。該功能包包含一個高度優化集成的用於求解稀疏線性系統的喬里斯基分解求解器。當H比較大時，其使用了好幾個策略去實現H的有效分解，包括邏輯排序，和一個近似最小度（approximate minimal degree, AMD）算法去重拍排變量。
一般情況下，喬里斯基分解的時間複雜度是$O(n^3)$, n表示變量的數量。對於稀疏矩陣，該複雜度依賴於喬里斯基因子的密度，該因子依賴於H矩陣的結構和變量的順序。文獻【19】的Mahon et al.分析過SLAM系統中喬里斯基因子與迴環之間的函數關係。如果迴環數量是常量，則喬里斯基因子密度是$O(n)$，對應的分解複雜度也是$O(n)$；如果迴環的數量與變量的個數成線性正比例增長，則喬里斯基因子密度是$O(n^2)$，對應的分解複雜度爲$O(n^3)$。

4.5 壓縮列存儲

LM算法的迭代有三個步驟：1）構建線性系統；2）分解H矩陣；3）通過倒轉替換尋找$\Delta{x}$。構建此線性系統與約束的數量成線性關係（同時對於大多數圖優化SLAM系統，與變量的數量也成線性關係）。大部分情況下，此過程可能是線性求解器最耗費資源的部分。作者使用一種有效的方法解決由約束生成的等式(5)構造稀疏H矩陣。
CSparse使用壓縮列存儲（compressed column storage，CCS）格式處理稀疏矩陣。下圖列出裏其思想：

數組裏的每個非零元素被放在val向量裏。非零元素按照列排序，再按照行排序。對所有的列，col_ptr對每個列只有一個入口，其數值爲上一列數值加上上一個總的非零元素數量。col_ptr指向每一列的起始位置。最後由row_ind給出行號，實現對應行列元素的檢索。
CCS格式是一種高效存儲大型稀疏矩陣的方式，但是因爲每個非零元素的插入某個列都會引起該列所有序列的移動，所以頻繁插入時複雜度較高。效率最高也就是以智能列順序創建稀疏矩陣，這也需要循環遍歷約束||c||次。作者提出一種只需要遍歷約束一次，並將每個33的矩陣塊$J_i^T\Lambda_{ij}J_i$存儲至一個特殊的指向性塊數據結構中， 該數據結構與CCS格式基本一致。具體算法如表1所示，通過對約束的一次遍歷將33的矩陣塊存儲進C++的std::map數據格式中，一個map表示一列。maps可以基於他們的鍵值實現有序高效插入，這便是行索引。一旦如步驟2中的數據結構已經創建完畢，則通過遍歷此天然有序的maps轉換爲H矩陣的稀疏CCS格式。此處在數值插入時將列和行分開進行創建是因爲在LM的一組迭代過程中列只需要被創建一次。【此處的操作過程主要是爲了提高CCS格式的存儲在多次迭代時的快速高效更新，否則同樣會影響計算性能】
因爲CSpase裏的喬里斯基求解器只關注H的上三角元數，因此此處只存儲H的上三角部分元素，並假設矩陣時是對稱的。

上述表1中是創建稀疏矩陣的過程：
$H = CreatSparse(e, c_f)$
輸入：一組約束$e_{ij}$，一組節點（變量）
輸出：以CCS格式表示的稀疏上三角H矩陣
1）以$||c_f||$的大小創建該長度的std::map向量vector；每個map與H矩陣對應的列相關聯。map的key是行索引號，裏面的data是3*3的矩陣塊。map[i, j]表示第i個map的第j個入口。
2）對每個約束$e_{ij}$，假設i < j：

1. 如果其不存在，則在下面的步驟中創建map的元素；
2. 如果$c_i$是自由的節點，則令$map[i, i] += J_i^T\Lambda_{ij}J_i$；
3. 如果$c_j$是自由的節點，則令$map[j, j] += J_j^T\Lambda_{ij}J_j$；
4. 如果$c_i, c_j$都是自由的節點，則令$map[j, i] += J_i^T\Lambda_{ij}J_j$；（因爲對稱，只存儲上三角）。

3）創建稀疏上三角矩陣H

1. 在下面的步驟中，忽略3*3map[i, i]中處於下三角區域的元素；
2. 按先列後行的順序遍歷map[]向量，通過其行和列分別對應創建CCS格式中的col_ptr和row_ind；
3. 再次遍歷map向量，將其中的值插入到CCS格式中對應的val序列中。

4.6 可持續LM系統

LM算法如下表2所示，對於一組節點$c$和與其關聯的測量進行一次流程操作。爲了在迭代中間增加更多的節點，在增量式LM中允許運行單次迭代。算法過程是可持續的，$\lambda$在迭代之間是存儲的，這樣在下一次迭代時可以基於結果調整$\lambda$。內含的思想依據是增加少量的節點和測量不會對系統造成太大改變，因此$\lambda$的值能夠體現梯度下降法和牛頓歐拉法的狀態。當出現迴環時，系統可能無法找到比較好的最小值，此時會在下幾次迭代時提高$\lambda$啓動系統傳向比較好的搜索路徑。
對於$\lambda$的調整有許多方法，作者選擇了了一種較爲簡單的方式。系統啓動時$\lambda$設置的較小，爲$10^{-4}$。如果一次更新之後比上一次的誤差更小，則$\lambda$折半；如果誤差相同或者更大，則$\lambda$翻倍。這在增量式優化過程中表現得很不錯。只要當增加節點時，誤差下降，則$\lambda$同樣減小，系統保持在牛頓歐拉法得區域裏。當一個連接約束得加入導致系統出現比較大不能矯正的干擾時，$\lambda$則增大，系統退回到更加魯棒的梯度下降法。

上述表2是LM方法單次迭代的過程：
輸入：節點$c$和約束$e$，對角線增量$\lambda$
輸出：更新後的的節點$c$
1）如果$\lambda$=0， 則將$\lambda$設定爲上次運行時的值
2）使用$CreatSparse(e, c-c_0)$構建新的新的稀疏矩陣H，其中$c_0$是固定的pose, 也就是上次優化後的pose，$c-c_0$是新增加的節點；
3）使用基於AMD（Approximate Minimal Degree）的稀疏喬里斯基分解求解線性系統$(H+ \lambda diagH) \Delta x = J^T\Lambda e$
4) 使用等式（6）更新新增加的節點$c-c_0$的估計值；
5）如果誤差減小，則將$\lambda$折半並存儲，並返回更新後的$c-c_0$的poses;
6) 如果誤差增加，則將$\lambda$翻倍並存儲，並返回更新前的$c-c_0$的poses。

5. 掃描匹配（Scan Matching）

SPA要求從激光掃描的匹配中得到精度估計（協方差的逆）。現有的好幾種掃描匹配（scan-match）方法均可以提供此信息，比如Gutmann等使用激光點去匹配在參考掃描幀中提取出來的線從而得到一個誤差的高斯估計。還有Konolige and Chou提出的相關匹配（correlation method）方法, Olson[22]對其進行了拓展，該方法基於給定的一幀掃描可以得到最優的全局匹配結果，同時返回精確的協方差。該方法也允許單幀掃描和多幀對齊的掃描之間相互進行匹配。SRI的建圖系統$Karto$[12]在序列掃描的局部匹配中和多幀掃描的迴環匹配中均使用了此方法。
爲了產生實際場景的數據用於實驗，作者使用63個不同大小的離線存儲的機器人數據運行$Karto$，使用該方法的掃描匹配和優化器建圖併產生約束，包括迴環。該圖被離線存儲，並在實驗部分對所有的對比方法作爲輸入進行比較。

6. 實驗

作者將提出的SPA方法與當時比較先進的方法在63個真實場景的數據和一組比較大的仿真數據中進行比較。其他對比方法如下：

• 信息濾波器：DSIF[7]
• 隨機梯度下降：TORO[10]
• 分解非線性系統：Treemap[8]
• 稀疏位姿優化：SPA，包含 a) 稀疏直接喬里斯基求解器， 和 b) 可迭代PCG(Preconditioned Conjugate Gradient, 預調節共軛梯度法)[15]

作者使用上述相同的“可持續LM”步驟作爲SPA更新了PCG中的實現。唯一的不同是線性求解器。預調節器（preconditioner）是一個不完善的喬里斯基方法，而且以稀疏矩陣的格式實現了共軛梯度。
作者同樣評估了一個稠密喬里斯基求解器，但是計算複雜度和內存消耗都是其他方法的好幾倍。比如，1600個約束和800個節點使用一次稠密喬里斯基分解一次迭代需要2.1秒，其他方法只需要幾毫秒。所有使用均在2.67GHz的Inter Core i7-920上運行。

6.1 精度測量

對於室內數據集，沒有真值。一個位姿約束系統測量的好壞可以使用約束的權重協方差平方誤差，或者使用$\chi^2$來進行體驗。如果掃描的匹配是精確的，則$\chi^2$會更小，表示掃描結果對齊的更好。下圖3展示了實際數據中$\chi^2$的不同在對齊上的體現：

6.2 離線優化

因爲離線優化強烈依賴於初始值，作者使用了當時兩種常用的初始化策略進行：

• 里程計方法：圖中的節點使用增量的約束進行初始化。這是在大部分圖優化中使用的標準方法。
• 旋轉樹方法：旋轉樹基於圖的廣度優先遍歷進行構建。樹的根節點是圖中的第一個節點。新節點的位置初始化根據所構建的旋轉樹深度優先遍歷實現。節點孩子的位置根據連接約束設置爲轉換後的雙親節點位置。在作者的實驗中，此初始化方法表現最好。
  測試結果如下：
  
  此處的離線數據測試，兩種初始化方法沒有太明顯的差異。左圖中PCG和SPA重合，可見SPA在保證優化精度的情況下，計算複雜度最小。

6.3 在線測試

在線實驗中， SPA同樣在保證最優優化精度的情況下，計算時間消耗遠小於其他方法，且隨時間變化更加穩定。

6.4 大的對稱結構環境測試

上圖可以看出在大型複雜環境中，基於旋轉樹初始化新節點的PCG和SPA均可以較好的完成優化過程，SPA的時間複雜度更小。使用里程計方式初始化的PCG和SPA均不能較好地完成優化。

7. 結論

作者提出和驗證了一種用於2D位姿圖優化的有效非線性優化方法，稱爲SPA（Sparse Pose Adjustment）。SPA依賴於高效的線性矩陣構建和稀疏非迭代喬里斯基分解去有效表示和求解大型稀疏位姿圖。大的全局地圖優化需要幾秒，但是實時的運行計算最糟糕的時候也可以達到10ms/節點。
與其他方法相比，SPA更快且收斂更好。唯一的缺點是初始化參數不好時會導致失敗，但是可以與旋轉樹初始化方式進行配合處理和優化，甚至可以在大型複雜包好對稱結構的環境中運行。與一個掃描匹配前端進行配合，SPA同樣可以實現實時的地圖探索和構建。同時，SPA也可以進行地圖拼接和刪減，可以用來維護長期的建圖過程。