# Lidar & Graph based online ICP SLAM
****
## 自主移動式機器人建圖系統
* C++ & ROS based
* Sensor inputs: Lidar, Odometry (from encoder & IMU)
* Map type: occupancy grid map
* Scan matching algorithm: ICP from PCL
* Loop Closure Detection: Fast Correlative Scan Matching (branch & bound)
* Graph Slam (G2O Optimization)
* Map consists of lots of submaps.

****
## System Architecture
 
 
       上圖為本研究SLAM建圖之流程圖。左邊部分為感測器輸入，左上為光達或是任何其他測距器，為機器人的感知輸入，提供周遭環境的測量資訊。左下為里程計輸入，包括編碼器(encoder)以及IMU，並經由擴展卡爾曼濾波器(Extended Kalman filter)，將兩種數據進行整合。編碼器在具有一定解析度的狀態下，在短時間內對於位置直線位移的估計是具有一定能力的，然而對於旋轉的估計就沒有那麼好了，容易因為輪子打滑造成影響。IMU對於旋轉的估計短時間內則較為精準，而對加速度計的數據做積分所產生直線方向位移估計誤差累積快速。因此整合了兩種數據後，利用個別的優勢，直線主要依賴輪式編碼器，旋轉則主要由IMU估計，能為機器人提供更穩定的初始猜測(Initial Guess)。圖二右上的部分是關於地圖的管理，右下則為定位的部分。

## Scan Matching
       SLAM，同時定位及建圖，代表在建圖的當下也必須同時維持定位的精準度，才能提高地圖相對於真實環境的準確性。我們系統使用的定位方法為ICP(Iterative closest point)，主要的ICP函式則是使用PCL(Point Cloud Library)函式庫，而我們必須提供來源點雲以及目標點雲，ICP的匹配器會找出來源點雲以及目標點雲之間最有可能的相對關係，即轉換矩陣。
 
       而利用ICP方法建圖所需的輸入，來源點雲由Lidar最新的測距資訊產生，當數據進來後，一組距離及角度可以轉成2D平面座標下的一個點，累積所有角度的射線後即完成。而目標點雲則由兩組正在更新的子圖中較舊的那個所產生，由於是佔據柵格地圖，所以地圖上的每一個柵格都有佔據機率，若柵格的占據機率大於某個門檻，則在此柵格的中央產生一個點雲，遍歷所有柵格後，即產生目標點雲。有了兩組點雲後，即可進行ICP匹配，然而若是機器人移動速度太快或是感測器頻率太低，則結果容易落在區域最小值，定位可能就會有問題。因此ICP輸入加上了里程計給予的初始猜測，能在進行疊代計算之前，離最佳解更接近，進而避免區域最小值的出現，而初始猜測由上一刻估計出的位姿，加上這段期間里程計的變化所產生。
 
上圖紅色部分為Submap所產生之點雲，綠色為lidar當下產生之點雲

[ICP Scan Matching 實現](https://github.com/pk121150/mars_mapping/blob/031b0a0ddf6973d1111585898f0695857de9df3a/mars_slam/src/scan_matching/icp_scan_matcher.cpp#L29-L66)

## Optimiaztion
       SLAM的建圖過程中，即使再好的匹配方法，受限於圖的解析度以及感測器的噪音，都難以避免誤差累積。常見的方法有兩類，第一類為利用濾波器，如卡爾曼濾波器或是粒子濾波器，但現今較常被使用的方法是第二類的，基於圖優化(Graph-based)的方法，利用所有機器人在建圖過程中的位姿構建出圖的各個節點，並經由最佳化的方法找出一個最優的配置，讓預測以及觀測彼此間的誤差最小。在過去，由於電腦計算能力的限制，再加上圖優化的方法計算量太大，因此對於需要即時運算的應用較難以執行。經過時間的累積，現今硬體的計算能力跟過去已經有天壤之別，此外，過去計算量過大的問題，是由於矩陣的大小過於龐大，現今經由稀疏矩陣的算法，能過大大的加速運算速度，從而達成即時運算的目標。而上述的圖是由節點和邊所構成，機器人的位姿是一個節點(node)或頂點(vertex)，位姿之間的觀測關係則構成邊(edge)。而常見的邊或約束有兩種，第一種是時間相近的邊，也就是上一刻或是下一刻的位姿，會自動產生一種約束，而這個約束的數值就是兩個位姿之間差距；而另一種則是測量位置相近，但是時間相差較遠的邊，是經由迴環檢測發現回到過去經過的路徑所產生的約束。若是單純只有時間相近的約束，則預測以及觀測值會是相等的，圖無法進行優化來消除誤差累積，因此需要檢測出迴環來產生全域的約束，使得預測值與觀測值不一致。一但圖構建完成了，我們就可以調整機器人的位姿去儘量滿足這些邊構成的約束，使得預測值與觀測值彼此間的誤差最小。而在本研究中，我們使用g2o (General Graph Optimization)函式庫來達成圖優化的目的。
 
上圖為在ITRI LAB所建構之完整地圖
 
此圖為建圖過程中由機器人在地圖上的路徑所產生之節點，我們就藉由這些節點來對地圖進行優化

[G2O Optimization實現](https://github.com/pk121150/mars_mapping/blob/master/mars_slam/src/optimization/pose_optimizer.cpp)

## Demo Video
ITRI lab test
[](https://www.youtube.com/watch?v=DEHXX4RnT5s&feature=youtu.be&ab_channel=%E6%9E%97%E6%94%BF%E7%A2%A9 "ITRI lab test")

ITRI dinning room test (large-scale crowded environment)
[](https://www.youtube.com/watch?v=wi2SD8s-2AA&feature=youtu.be&ab_channel=%E6%9E%97%E6%94%BF%E7%A2%A9 "ITRI dinning room test")