轉自：https://blog.csdn.net/xiaoma_bk/article/details/81269250
《概率機器人》這本書 第11章詳細介紹了圖優化
實際代碼中 pose_2d中添加約束 以及後端的優化有這一部分。
1、簡介
Cartographer背後主要的思想是GraphSLAM。GraphSLAM又被稱爲Graph-based SLAM，它的基本思想是將機器人不同時刻的位姿抽象爲點（pose），機器人在不同位置上的觀測所產生的約束被抽象爲點之間的邊，或者叫約束（constraint）。
所謂的約束可以有多種多樣的形式，比如機器人在A點和B點都看到同一個消防栓（我們可以認爲這是固定在地圖上的landmark），那麼機器人在AB點觀測到消防栓的相對位置，就對機器人在A點和B點的位姿產生了約束，進一步的，AB兩點之間也產生了約束。 GraphSLAM就是在機器人運動的過程中構建出若干點（pose）和邊（constraint）組成的圖（Graph），再從全圖的角度進行優化。
SLAM問題的處理方法主要分爲濾波和圖優化兩類。濾波的方法中常見的是擴展卡爾曼濾波、粒子濾波、信息濾波等，熟悉濾波思想的同學應該容易知道這類SLAM問題是遞增的、實時的處理數據並矯正機器人位姿。比如基於粒子濾波的SLAM的處理思路是：假設機器人知道當前時刻的位姿，利用編碼器或者IMU之類的慣性導航又能夠計算下一時刻的位姿，然而這類傳感器有累計誤差，所以再將每個粒子的激光傳感器數據或者圖像特徵對比當前建立好的地圖中的特徵，挑選和地圖特徵匹配最好的粒子的位姿當做當前位姿，如此往復。當然在gmapping、hector_slam這類算法中，不會如此輕易的使用激光數據，激光測距這麼準，當然不能只用來計算粒子權重，而是將激光數據與地圖環境進行匹配（scan match）估計機器人位姿，比用編碼器之流精度高出很多。
目前SLAM主流研究熱點幾乎都是基於圖優化的，爲啥都用圖優化了。我想這和傳感器有很大關係，以前使用激光構建二維的地圖，現在研究熱點都是用單目、雙目、RGB-D構建地圖。處理視覺SLAM如果用EKF，隨着時間推移地圖擴大，內存消耗，計算量都很大；而使用圖優化計算在高建圖精度的前提下效率還快。 在圖優化的方法中（graph-based slam），處理數據的方式就和濾波的方法不同了，它不是在線的糾正位姿，而是把所有數據記下來，最後一次性算賬。
2、描述
圖是由節點和邊構成，SLAM問題怎麼構成圖呢？在graph-based SLAM中，機器人的位姿是一個節點（node）或頂點(vertex)，位姿之間的關係構成邊(edge)。具體而言比如t+1時刻和t時刻之間的odometry關係構成邊，或者由視覺計算出來的位姿轉換矩陣也可以構成邊。一旦圖構建完成了，就要調整機器人的位姿去儘量滿足這些邊構成的約束。
所以圖優化SLAM問題能夠分解成兩個任務：
1. 構建圖，機器人位姿當做頂點，位姿間關係當做邊，這一步常常被成爲前端（front-end），往往是傳感器信息的堆積。
2. 優化圖，調整機器人位姿頂點儘量滿足邊的約束，這一步稱爲後端（back-end）。
圖優化過程如下圖所示：先堆積數據，機器人位姿爲構建的頂點。邊是位姿之間的關係，可以是編碼器數據計算的位姿，也可以是通過ICP匹配計算出來的位姿，還可以是閉環檢測的位姿關係。構建的圖和原始未經優化的地圖如下：
夠建好圖以後，就能調整頂點滿足邊的約束，最後得到的優化後的地圖如下圖右所示。
爲了更好的理解這個過程，將用一個很好的例子作說明。如下圖所示，假設一個機器人初始起點在0處，然後機器人向前移動，通過編碼器測得它向前移動了1m，到達第二個地點。接着，又向後返回，編碼器測得它向後移動了0.8米。但是，通過閉環檢測，發現它回到了原始起點。可以看出，編碼器誤差導致計算的位姿和觀測到有差異，那機器人這幾個狀態中的位姿到底是怎麼樣的才最好的滿足這些條件呢？
首先構建位姿之間的關係，即圖的邊：
線性方程組中變量小於方程的個數，要計算出最優的結果，使出殺手鐗最小二乘法。先構建殘差平方和函數：
爲了使殘差平方和最小，我們對上面的函數每個變量求偏導，並使得偏導數等於0.
整理得到：
接着矩陣求解線性方程組：
所以調整以後爲滿足這些邊的條件，機器人的位姿爲：
前面是用閉環檢測，這次用觀測的路標（landmark）來構建邊。如下圖所示，假設一個機器人初始起點在0處，並觀測到其正前方2m處有一個路標。然後機器人向前移動，通過編碼器測得它向前移動了1m，這時觀測到路標在其前方0.8m。請問，機器人位姿和路標位姿的最優狀態？
在這個圖中，我們把路標也當作了一個頂點。構建邊的關係如下:
即:
殘差平方和：
求偏導數：
最後整理並計算得：
得到路標和機器人位姿：
接下來，將引入了一個重要的概念。我們知道傳感器的精度是有差別的，也就是說我們對傳感器的相信程度應該不同。比如假設這裏編碼器信息很精確，測得的路標距離不準，我們應該賦予編碼器信息更高的權重，假設是10。重新得到殘差平方和如下：
求偏導得：
轉換爲矩陣：
最後計算得到:
將這個結果和之前對比，可以看到這裏的機器人位姿x1更靠近編碼器測量的結果。請記住這種思想，這裏的權重就是在後面將要經常提到的邊的信息矩陣，在後面還將介紹。
reference：
1. Grisetti. 《A Tutorial on Graph-Based SLAM》
2. University of Alberta 很棒的機器人課程 https://webdocs.cs.ualberta.ca/~zhang/c631/主要以作業爲主，例子來源於該課程。
3. Strasdat. 《Visual SLAM: Why Filter?》
4. Grisetti. 課件 《SLAM Back-end》（可以直接搜到）
5. Rainer & Grisetti 《g2o: A General Framework for Graph Optimization》
-----------------------------------------------------------------------------------------------------------------------------------------------------
在上一部分中通過一個例子大致瞭解了graph based slam的優化過程。在本篇博客中將提升一個層次，對圖優化的求解過程進行推導。由於博文關注的在圖構建好以後，如何調整機器人位姿使誤差最下。因此，本文主要涉及的是圖優化的後端（back-end）。
我們已經知道圖優化問題轉變成了一個最小二乘問題。根據上篇博客最後一個例子，求機器人SLAM過程中最優軌跡可以表示成求解機器人位姿使得下面誤差平方函數最小。
其中，{\color{Red} X_{i}}表示圖頂點的參數向量，如機器人位姿。{\color{Red} Z_{ij}}表示測量值，{\color{Red} \Omega _{ij}}表示該誤差所佔權重的矩陣。{\color{Red} e\left ( X_{i},Y_{i},Z_{ij} \right )}是一個向量誤差函數表示{\color{Red} X_{i}}和{\color{Red} X_{j}}之間的關係與測量{\color{Red} Z_{ij}}之間有多吻合。後面爲了簡化，將誤差函數寫成下面的形式：
下圖爲圖優化的一個簡單例子
對於上述最小二乘問題還可以從最大似然的角度解釋。這裏也作簡單介紹，主要是爲了更進一步的理解誤差的權重矩陣。對於傳感器的測量，我們可以假設它受高斯白噪聲的影響。所以每一個測量值的分佈可以看作是以真值爲中心的高斯分佈。如果測量是多變量的，那就是多元高斯分佈。
多元高斯分佈的協方差矩陣的某一維越大，高斯曲線越矮胖，表示在這個方向上越不確定。並且高斯分佈中，均值部位概率最大。所以，對於某個測量，我們應該使它出現在概率最大的地方，這就是最大似然概率。可以得到似然概率的log形式的計算公式：
上式和前面的誤差平方和函數很像，只不過這裏顯式的指明瞭誤差函數的形式。所以我們發現，誤差的權重矩陣（正式名稱爲信息矩陣）等於協防差矩陣的逆。由於圖優化裏每一條邊代表一個測量值，如表示相鄰位姿關係的編碼器測量值或者圖像（激光）匹配得到的位姿變換矩陣。所以圖優化裏每一條邊的信息矩陣就是這些測量協防差矩陣的逆。如果協防差越小，表示這次測量越準越值得相信，信息權重就越大。
我們看到圖優化問題變成了求解最小二乘問題，然而機器人位姿之間的變化函數不是線性的，所以是個非線性最小二乘問題，得通過迭代法進行求解。如果迭代開始時有一個好的初始假設值，那我們就能用Guass-Newton法或者 Levenberg-Marquardt法了。
假設我們已經有了好的初始假設值{\color{Red}\breve{a} }，需要在這個值附近迭代尋求最優解。求解的方法是把誤差函數在該初始值附近進行一節泰勒展開
其中{\color{Red}J_{ij} }是誤差函數{\color{Red}e_{ij}\left ( x \right ) }在{\color{Red}\breve{a} }附近的雅克比矩陣。並且爲了書寫方便，使用{\color{Red}e_{ij} }代替了{\color{Red}e_{ij}\left ( \breve{x} \right ) }。
將上面的(5)式代入誤差平方和中的某一項可以得到
注意，這裏{\color{Red}b_{ij} }是列向量，{\color{Red}\Delta x }是列向量，{\color{Red}e_{ij} }也是列向量，{\color{Red}c_{ij} }是一個數值。將（10）式代入，可以重寫誤差平方和函數如下：
其中。爲了求解上式，使得其最小，還是求其一階導數並使其等於0，可以得
其中H是系統的信息矩陣（注意與邊的信息矩陣{\color{Red} \Omega _{ij}}區分），系統的解就是在初始值上疊加這個增量：
Guass-Newton迭代法就是不斷重複這個過程直到收斂。LM法引入了一個鬆弛因子來控制迭代速度：
然而，一個值得注意的問題是，上面的步進迭代是用的兩個向量直接相加，這是在歐式空間中的做法。而機器人SLAM問題中涉及到平移和旋轉，平移是在歐氏空間中，可是旋轉就是在非歐空間了。在航跡推演的公式中我們也能看到相鄰時刻間位姿的遞增由於航向角的存在已經不是簡單的相加了。所以得重新定義一個非線性算子來代表增量。
將這個非線性算子用在移動機器人相鄰時刻的航跡推演，能夠得到：
這裏可能有點抽象，別急，在後面2d slam的圖優化例子中將具體介紹。
上面的求解過程看起來簡單，計算b，H，然後計算增量迭代直到收斂。然而，實際計算的時候，b,H,雅克比矩陣到底是啥得弄清楚。接下來，具體解析它們的結構。
誤差函數{\color{Red}e_{ij} }只和{\color{Red}x_{i} }、{\color{Red}x_{j} }有關，因此它的雅克比矩陣和{\color{Red}x_{i} }、{\color{Red}x_{j} }無關的列都爲0，有如下結構:
清楚了雅克比矩陣的結構，再來看b和H。我們已經知道{\color{Red}b_{ij} }是列向量，可以推算出結構如下，注意下圖中{\color{Red} \Omega _{ij}}和{\color{Red}e_{ij} }下面的色塊代表矩陣和列向量，並且藍色塊代表0，紅色塊非0。可以看到只有和{\color{Red}x_{i} }、{\color{Red}x_{j} }有關的區域非零。
從上圖中可以看出系統是稀疏的，並且正如上面圖中的累加一樣，程序中也可以對特定的ij計算相應的Hij,bij，然後再把所有的Hij,bij累加起來就行了。
最後把整個求解過程總結如下：
reference：
1. Grisetti. 《A Tutorial on Graph-Based SLAM》
2. Grisetti. 課件 《SLAM Back-end》（可以直接搜到）
3. Rainer & Grisetti 《g2o: A General Framework for Graph Optimization》