【SLAM】（二）Cartographer的原理探究——GraphSLAM理論基礎

版權聲明：本文爲博主原創文章，歡迎轉載。轉載請註明出處http://blog.csdn.net/jsgaobiao https://blog.csdn.net/jsgaobiao/article/details/65628918

現在真是越來越懶了……好久沒有更新過博客。上一篇博客中提到，在室外進行Cartographer實驗的時候遇到了莫名的卡頓——一方面是由於小車搭載處理器的性能瓶頸，另一方面，室外場景確實消耗更多的運算資源，只要把地圖分辨率降低，優化的頻率降低或者縮短Lidar的max_range就可以得到實時的效果。

從寒假到現在挺長的一段時間，在Cartographer的研究方面大概做了以下幾件事：

1. 研究了下使用velodyne 32E的數據（PointCloud2）跑cartographer的方法和效果，順便學習了一下ROS的使用。ROS真是板磚利器呀233；
2. 用實驗室採集的室外數據試跑了一下cartographer，發現了一些問題：在較大的室外場景下，cartographer會出現內存溢出的錯誤（約300*100m2 的範圍）；由於cartographer本來就是爲室內場景設計，所以將submap都放在內存裏。如果想要實現室外的SLAM建圖，還需要加入地圖內存的管理機制才行；
3. 爲了從純粹的cartographer使用者蛻變爲融會貫通的開發者，我花了些時間閱讀代碼，以及瞭解GraphSLAM理論。

【下面主要記錄一下我學習GraphSLAM的過程】
Cartographer背後主要的思想是GraphSLAM。GraphSLAM又被稱爲Graph-based SLAM，它的基本思想是將機器人不同時刻的位姿抽象爲點（pose），機器人在不同位置上的觀測所產生的約束被抽象爲點之間的邊，或者叫約束（constraint）。
所謂的約束可以有多種多樣的形式，比如機器人在A點和B點都看到同一個消防栓（我們可以認爲這是固定在地圖上的landmark），那麼機器人在AB點觀測到消防栓的相對位置，就對機器人在A點和B點的位姿產生了約束，進一步的，AB兩點之間也產生了約束。
GraphSLAM就是在機器人運動的過程中構建出若干點（pose）和邊（constraint）組成的圖（Graph），再從全圖的角度進行優化。

GraphSLAM更具體的入門示例可以參考“白巧克力亦唯心”的博文《graph slam tutorial ：從推導到應用1》：
http://blog.csdn.net/heyijia0327/article/details/47686523

看過上面這篇博文後，可以說對GraphSLAM就有了一個比較感性的認識。但是《graph slam tutorial ：從推導到應用２》中包含了一大堆莫名其妙的數學公式，缺少SLAM理論基礎的同學看起來可能會比較吃力（我當時就這麼覺得……）

經過一番周折後，我發現了一篇論文講解的非常細緻：
《The GraphSLAM algorithm with applications to large-scale mapping of urban structures》

這篇文章的作者是神書《Probabilistic Robotics》的作者之一，內容和書上也非常類似，建議有志於深入瞭解GraphSLAM的同學仔細閱讀這篇paper（搭配書閱讀風味更佳）

貫穿GraphSLAM中的一個非常重要的概念就是信息矩陣（information matrix）和信息向量（information vector），通常用 Ω 和 ξ 來表示。想要理解它們在GraphSLAM中的作用，我們首先得知道它們從何而來。
大家所熟悉的高斯分佈，在SLAM中常常用於表示傳感器噪聲分佈等，是SLAM算法重要的組成元素。通常情況下，我們習慣用µ（均值）和Σ（協方差）來描述一個高斯分佈。而上面提到的信息矩陣和信息向量，其實是另一組描述高斯分佈的參數，叫做canonical parameterization（似乎應該翻譯成“典範參數”），他們的關係如下：

{Ω=Σ−1ξ=Σ−1µ

在使用典範參數表示negative log likelihood的時候，形式比用Ω、ξ表示更加優美：−logp(x)=const.+12xTΩx−xTξ
這也是GraphSLAM中引入信息矩陣和信息向量的原因。

下面三張圖非常生動的描述了GraphSLAM中建立information matrix的過程。

三張圖的右側就是信息矩陣，白色格點表示該處的數值爲0，黑色表示正值；圖中的xi 表示機器人所在的位姿，例如可以用（x, y, θ ）來描述的二維位姿；圖中的mj 表示在某個位置xi 所觀測到的路標（landmark），可以通過路標和xi 的相對距離和角度來描述。
相鄰兩個xi 之間的移動可以通過運動模型和里程計來產生約束，而xi 和mj 之間則通過觀測模型來產生約束。圖A->B->C清晰的描述了這一過程。
這些所謂的約束其實反應了運動模型/觀測模型的不確定性，簡單地說，如果某個傳感器的誤差越大，那麼信息矩陣中對應的約束值就越小；反之亦然。

在GraphSLAM中，我們需要通過優化信息矩陣來求得合理的pose和map，現在信息矩陣已經得到了，剩下的工作似乎顯而易見。可是，爲什麼很早就被提出的GraphSLAM算法理論，卻在近幾年纔剛剛興起呢？
我們注意到，隨着地圖的擴大和landmark的增多，信息矩陣中mj 項會迅速增加，而它帶來的巨大運算量是系統難以承受的。因此，人們普遍認爲，過大的運算量使得GraphSLAM不具備良好的實時性和運算效率。
而然近幾年SLAM研究者們發現，我們構建出來的Graph其實是非常稀疏的——因爲每個pose xi 只能觀察到其附近有限的路標mj ，並與他們產生約束；而其他大量的mj 與當前的pose卻沒有發生聯繫。人們發現利用稀疏代數的相關技巧，可以將mj 從信息矩陣中消去，進而減少了大量的運算。
下圖是這個被稱爲variable elimination算法的示意。可以看到，每消除一個mj ，能夠觀測到mj 的所有{xi }之間的約束都會被增強。例如下圖中m3 的消除就增強了<x2 , x3 >,<x3 , x4 >之間的約束，並在<x2 , x4 >之間增加了一條邊（約束）。經過消除算法之後的信息矩陣就只剩下所有pose之間的約束了。

至此，我們可以列出GraphSLAM算法的基本流程：

1. 選擇節點和邊的類型，確定他們的參數化形式，加入圖中
2. 求節點pose初始的估計值µ0:t (可以用航位推算法等)，開始迭代
3. 對信息矩陣降維 (variable elimination algorithm)，求優化的梯度方向
4. 繼續迭代。如果迭代結束，返回對pose和map的優化結果。

這個流程中的2、3、4步，已經有現成的工具可以使用（例如g2o, ceres等），它也被稱爲GraphSLAM的後端（back-end）。第1步則被稱爲GraphSLAM的前端（front-end），也是我們設計GraphSLAM算法的難點——如何根據不同的傳感器和任務場景設計Graph中邊（constraint）的參數化形式。

如果你不太理解什麼是邊的參數化形式，可以參考“半閒居士”的博文《深入理解圖優化與g2o：圖優化篇》，其中有比較詳細的解釋：
http://www.cnblogs.com/gaoxiang12/p/5244828.html

目前網上比較多的基本是基於視覺的GraphSLAM方法，大概是因爲視覺圖像中比較好定義landmark的概念。對於laser-based場景，我目前沒有找到比較滿意的開源方案來入手。
下面的計劃：
1. 以g2o爲切入點熟悉GraphSLAM的程序結構和後端優化的使用方法；
2. 繼續閱讀cartographer的代碼，看看google怎樣通過laser之間的scan matching 提取出相應的constraint。

PS:
原本實驗室是沒有Velodyne的，老師說：“要有（激）光”，於是就有了Velodyne （配上非常霸氣的tank圖，蛤蛤）