鐳射slam理論與實踐
本篇是記錄曾書格老師的課程《鐳射slam理論與實踐》
先貼一下個人總結（有理解的不正確的，麻煩指出來）：
第一章：鐳射SLAM簡要介紹
1、輸出Metrical map尺度地圖，slam分為兩種：基於濾波的 filter-based 的SLAM，和Graph-based的SLAM。
2、
（1）基於Graph-based的代表是cartographer，可以修復 t 時刻之前的誤差分為兩部分Front-end ，和Back-end。
（2）基於PF的代表gmapping，只能估計X(t)當前時刻的位姿，不能修復之前時刻產生的誤差。在大面積的情況下，就無法建圖了。粒子濾波實際上是一個貝葉斯估計的過程，其流程是一個狀態預測的過程，分為6步：
卡爾曼是貝葉斯線上性高斯的特殊情況。
（3）
實際中，IMU算角度，里程計算距離
（4）幀間匹配演算法：
ICP 是計算點對點的，沒人用了。而PI-ICP是計算點對線的。
NDT用於三維的slam。
CSM：是最主要的方法。
scan-scan已經沒人用了；
scan-to-Map代表是cartographer。
Map-to-Map還沒有用，但是效果最好。與X(t)時刻之前的幾幀進行迴環檢測。
3、2D鐳射SLAM（用的2D的鐳射雷達）
（1）2D鐳射slam的幀間匹配
（A）PI-ICP
（B）CSM(Correlation Scan Match)
（C）梯度優化的方法：Hector—SLAM
（D）CSM+梯度優化：cartographer
（2）2D 鐳射SLAM的迴環檢測方法
（A）分支定界Branch and Bound&lazy Decison（延遲決策） ，代表：cartographer
（B）Scan-to-Map
（3）
（A）EKF-SLAM構建的是feature 地圖，而不是grid柵格地圖，所以無法用於導航，只能用於定位。
（B）gmapping是Grid版本的FastSLAM，加入了scan-match。
（C）在Optimal RBPF在gmapping上做了優化，是MRPT上的一個開源演算法。
2010年之後就由FP 轉化為 圖優化的方法了。
Karto SLAM 執行時經常崩潰。所以在此基礎上Catographer作了優化。Catographer是所以開源演算法中效果最好的。
（4）、資料的預處理
4、3D鐳射雷達
（1）3D鐳射雷達的幀間匹配
（A）Point-to-Plance ICP 點到面的匹配
（B）Feature-based Method
第二章：感測器資料處理I：里程計運動模型及標定
1、里程計運動模型
（1）兩輪差分底盤的運動學模型
（2）航跡推算（Dead Reckoning）
2、里程計標定
（1）線性最小二乘的基本原理
通常就是 求解一個近似解。
（2）最小二乘的直線擬合
（3）最小二乘在里程計標定中的應用：
a，直接線性方法，精度不高，但實現簡單；用這個方法
b，基於模型的方法，精度高，但是現實起來相當複雜，是一個非線性的過程。不用。
作業：實現一個直接線性方法的里程計標定模組
小結：由於機器人結構的系統性誤差（比如輪子直徑的大小不一樣，機械結構縫隙等），所以需要坐里程計標定，達到去除系統誤差的目的，而隨機誤差無法去除。
第三章：感測器資料處理II：鐳射雷達數學模型和運動畸變去除
1、概念
（1）鐳射lidar介紹：基於三角測距原理，距離越近，精度相對越高。而飛行時間（TOF）遠距離時精度才可以，價格昂貴。高階的lidar有德國的sick，日本的北陽。
固態lidar：是一個發展趨勢，價格低，類似於深度相機了，100線——300線。
如果lidar的幀率小於10HZ的話，那麼運動畸變是非常明顯的。
（2）鐳射lidar數學模型介紹：
光束模型：一幀鐳射與地圖的重合程度，計算量太大，現在已經不用了。
似然場模型：對影象進行高斯平滑，計算量低。應用的多。
（3）運動畸變介紹：
產生的原因：在獲取一幀鐳射lidar的資料用時 t 時，機器人的位置發生了運動，導致每個鐳射點的基準位置不是固定的。
2、畸變去除方法
（1）純估計方法：ICP 的方法，就是點雲匹配。
考慮到機器人的運動，採用VICP的方法。（Velocity estimate ICP）
（2）感測器輔助方法：（IMU/odom）
odom,直接測量位移和角度。更新頻率（100Hz——200Hz），而IMU測量的是加速度，還需要積分。
二次曲線的近似：一幀鐳射資料N個鐳射點，每個鐳射點對應的位姿{P1，P2 ... Pn}通過這種方法得到，把轉換後的鐳射資料釋出出去。
作業：實現一個鐳射雷達運動畸變去除模組
小結：由於機器人存在運動畸變，所以要釋出publish經過去除畸變的鐳射lidar資料。
第四章：鐳射SLAM的前端配準方法
幀間匹配方法：
（1）ICP 匹配方法：點到點的距離作為誤差，一階收斂。
（2）PL-ICP方法：點到線的距離作為誤差，二階收斂，精度比ICP更高。
（3）基於優化的匹配方法(Optimization-based Method）：應用於Hector SLAM 和cartographer
梯度的優化：Hector SLAM，不需要里程計，純靠優化的方法。
cartographer 是CSM+優化。
（4）相關匹配方法和分枝定界加速：Karto SLAM，cartographer，分枝定界在相關方法中的加速作用。
第五章：基於濾波器的鐳射SLAM方法(Filter-based)
1、（1）只估計當前時刻的狀態X(t)叫濾波。視覺地圖——feature特徵地圖；鐳射——》柵格地圖
2、
（1）Bayes濾波：是一類方法的統稱，卡爾曼家族和PF都屬於貝葉斯濾波。
（2）粒子濾波：PF是Bayes濾波的特例。應用於物體跟蹤，機器人定位，SLAM導航。用一系列的粒子近似概率分佈。
有一個重取樣的過程，即去除權重小的粒子。權重表示和地圖的匹配程度。
重取樣的目的：生成後驗分佈的粒子群。總粒子數不變。
對於SLAM，存在粒子耗散問題，且無法解決，所以在環境小的時候才有用。
（3）FastSLAM：是gmapping的基本原理，也是基於PF的。存在三個問題和優化a,b,c：
a.每個粒子都包含自己的柵格地圖，佔用較大的記憶體；保持粒子數量較小，提升proposal取樣的位姿質量；
b.粒子耗散問題；緩解粒子耗散；減少重取樣的次數；
c.在a的基礎上再優化，把proposal限制在一個較小的區域
（4）Gmapping：以FastSLAM為基礎。在fastSLAM的基礎上實現了優化a和b，沒有使用優化c。
主要是兩個函式: addScan( ) 和 processScan( )
第六章：基於圖優化的鐳射SLAM方法(Grid-based)
1、Grid-based 是基於非線性最小二乘原理的求解。Grid-based包含前段Front-end和後端Back-end。
2、非線性最小二乘：f(x) = z ，f(x)是一個非線性的函式 ，z可以是一個預測值，然後通過感測器可以得到一個測量值z'，根據二者就可以去非線性最小二乘。
3、有迴環檢測。
4、高斯牛頓方法
5、cartographer方法中mapping_2d目錄：
第七章：基於已知定位的建圖
位姿估計包含 幀間匹配和優化的方法，兩者構成圖優化，通過對機器人的位姿（x ，y ， θ）來構建地圖。
1、地圖分類：（柵格地圖、特徵地圖和點雲地圖，都屬於）尺度地圖、拓撲地圖、語義地圖
2、覆蓋柵格建圖演算法（gmapping裡面用的這個）：
柵格地圖的數學模型：給定機器人的位姿（由里程計資訊得到）和鐳射lidar的資料，來估計出柵格地圖。
地圖估計：柵格是一個二元隨機變數。1 標準佔用，0表示沒被佔用。
鐳射穿過的柵格為free，鐳射擊中的柵格為occupied
3、計數（count model）建圖演算法：
鐳射擊中的次數 / 鐳射穿過的次數 到達一個比值，就表示障礙物，occupied。
第八章: 3D鐳射SLAM介紹
1、3D鐳射SLAM
2D鐳射SLAM和3D鐳射SLAM的區別：
（1）2D ：單線鐳射lidar，只能在一個截面，形成2D柵格地圖，室內機器人
（2）3D：多線鐳射lidar，形成三維點雲地圖或者三維柵格地圖，室外移動機器人
2、3D幀間匹配：
（1）點面ICP
(2) Feature-based method
(3) NDT 方法 normal-distribution-Transform
2、LOAM (lidar odometry and mapping)
（1）是3D 鐳射SLAM演算法，只有匹配，沒有迴環。
（2） KITTI資料集Odometry排行榜上長期霸佔第一