SLAM——圖優化篇
1.圖優化的入門
在上節中，直接用倆倆匹配搭建一個里程計，但是如果：1.錯誤匹配，整個圖就會看起來很奇怪，就是錯了。2.誤差會累積，常見的現象是：相機轉過去的過程能作對，但轉回來就很奇怪。3.效率低。由於地圖拼接中累計誤差是個很嚴重的問題，所以要保證每次匹配都精確無誤，而這是很難實現的。所以我們把所有整的資訊都考慮進來，成為一個全slam問題。
姿態圖（原理部分）
姿態圖，是由相機姿態構成的一個圖（graph）。這裡的圖，從圖論意義上來說，一個圖有節點（位姿）和邊構成（R，t）
在最簡單的情況下節點代表相機的各個位姿（四元數或者矩陣）：
而邊則代表各個位姿的轉換關係：
則我們對於簡單的迴環來說可以大概表示為:
用高博的圖即：
像這樣子的vo，我們除了對邊進行優化之外，沒有比較好的方法了，構造最小二乘法問題，使得整個VO的邊誤差最小，則有:
於是，我們的工作轉移到了對代價函式： 進行最小優化。
怎麼把SLAM問題表示成圖呢？
（參考高博的部落格加上自己的一些理解）
圖：頂點（需要調整的變數，即最小二乘中的決策變數）和邊（觀測方程z（測出來的東西）），然後我們是以為誤差，以 為目標函式。
那麼優化變數（即頂點）： 可以是一個機器人的pose（6自由度下為4x4的變換矩陣T或者3自由度下的x,y,）也可以是一個空間點（三維空間的[x,y,z]或者二維畫素的[x,y]）。觀測方程（這裡的觀測方程，我之前糾結了好久，但是通俗的講就是你感測器測出來的資料計算出的你想要求的變數）也有很多形式：
1.機器人倆個Pose之間的變換；
2.機器人在某個Pose處用鐳射測量到了某個空間點，得到了它離自己的距離和角度。
3.機器人在某個Pose處用相機觀測到了某個空間點，得到了它的畫素座標。
在圖中，以頂點為優化變數（在最小二乘中為決策變數），以邊為觀測方程（就是測出來的東東）。由於邊可以連結一個或多個頂點，所以我們把它的形式寫成更廣義的 ，以不限制頂點數量的意思。
1.機器人倆個pose之間的變換；——一條Binary Edge(二元邊)，邊的方程為：
2.機器人在某個Pose處用相機觀測到了某個空間點，得到它的畫素座標；——Binary Edge，頂點為一個3D Pose：T和一個空間點 ，觀測資料為畫素座標,觀測方程為：
總而言之，頂點就是優化變數（三維空間點或者Pose），邊就是觀測方程（T或者二維座標）
2.圖優化的深入理解
圖優化是什麼？在graph-based SLAM中，機器人的位姿是一個節點或頂點（vetex），位姿之間的關係構成邊（edge），比如t+1時刻和t時刻之間的odometry關係構成邊，或者由視覺計算出來的位姿轉換矩陣也可以構成邊。一旦圖構成了，就要調整機器人的位姿去儘量滿足這些邊構成約束。
圖優化SLAM能分解成倆個任務：
1.構建圖，機器人位姿當做頂點，位姿關係當做邊，稱為前端。
2.優化圖，調整機器人位姿頂點儘量滿足邊的約束，稱為後端。
圖優化過程中：先堆積資料，機器人位姿為構建的頂點。邊是位姿之間的關係，可以是編碼器資料計
算的位姿，也可以是通過ICP匹配計算出來的位姿，還可以是閉環檢測的位姿關係。
左圖為建完圖未經優化的圖，而右圖為調整頂點滿足邊的約束，最後得到的地圖。
—————一個幫助理解的例子：——————–
如下圖所示，假如機器人初始點在0處，然後機器人向前移動，通過編碼器測得是1，再編碼器又測得了它向後0.8，結果通過閉環檢測卻發現它又回到了起點。可以看出，編碼器出現了點小問題，那麼需要機器人這幾個狀態中的位姿到底是怎麼樣的才最好的滿足這些條件呢？
建立關係：