前段時間爲了分享激光slam，重新整理學習了ROS下開源的幾種激光slam. 現在記下這段時間學習的一些關鍵點。
1. 開源代碼repo 說明：
1.1 gmapping
/slam_gmapping
ROS封裝的gmapping節點
/openslam_gmapping
gmapping的實現源碼
1.2 Hector
1.3 karto
/slam_karto
ROS封裝的karto節點
/open_karto
karto內部實現
/navigation_2d
1.4 cartographer
googlecartographer/cartographer 算法實現
googlecartographer/cartographer_ros
2. 算法組成說明
2.1 算法框架
（1） 濾波框架：
基於KF,EKF，UKF等，這些一般在比較早期的提特徵方式出現，不在當前熱門開源的範圍內。
基於PF： RBPF. gmapping就是基於這種方式進行的。
(2) 圖優化框架： karto cartographer
將激光幀或子圖作爲節點，形成彼此之間的關係邊，構建一張關聯的圖， 最後利用非線性優化調整每個節點的pose，最終以調整後的結果形成新的map,
(3)單純scanmatcher： hector
2.2 scanMatcher部分:
slam的處理離不開連續幀間的處理， 對應激光就是scan-scan, 由於連續幀間的匹配沒有scan-map匹配穩定,激光中基本採取的是scan-map的方式，（map也就是當前激光幀與前面生成的地圖、子圖或利用序列激光生成的地圖)scanMatcher其中需要注意的點往往就是： 匹配速度要快，匹配準確度要高。
2.3 地圖的生成： 2D激光中地圖的形式基本上是採用佔據柵格的 gridMap的形式
增量式地圖：後面的添加都是在前面的統計基礎上進行。。 地圖有一步開闢，後面慢慢填充， 動態開闢空間，
每次全更新： 每次地圖都是利用調整後的全部激光數據，或子圖生成地圖。（基本就是對應圖優化框架）
3. 框架詳解
3.1 gmapping 採取的RBPF的框架：詳細見論文
它將slam問題（利用歷史輸入和觀測 同時 定位機器人的pose和構建環境地圖的問題）轉化爲定位機器人的軌跡（MCL定位）和構建地圖兩部分。以粒子的方式模擬機器人位置分佈的概率。每個粒子代表一個機器人分佈的可能，存儲所有歷史時刻的pose,map，和對應權重。 （簡單點理解， 如果粒子數量無限多，那總有一個粒子代表真實吧， 那它的軌跡和地圖就是所求， 剛開始粒子權重都一樣，但隨着不同的輸入與觀測與地圖的對應差異，越接近真實的機器人表示的粒子權重會增加，反之逐漸減小）
需要關注的問題： 計算性能考慮，粒子數量不可能是無限增加的；
粒子有限，（更新中出現問題後調整不過來了）那粒子怎麼體現更真實的機器人pose, （weight update + resample）
3.2 圖優化框架： 以karto爲例介紹
圖優化框架主要分爲前端的圖構建過程，閉環檢測，和對構建的圖的約束關係進行非線性優化調整每個node的pose. 前端的過程其實就是scanMatcher過程， 閉環檢測其實也就是scanMatcher的過程。
vslam的圖構建大家一般比較好理解， 以爲圖像的信息豐富，只需要遍歷關鍵幀，找出與當前幀強匹配的幀。（key-key + 約束）。已建立feature（ref - map））在當前pose（可以認爲視覺里程計）下的觀測預測（map 成像模型轉化到 image frame），圖像當前觀測偏差（一般認爲爲較準確反應真實）與 觀測預測的偏差作爲調整的目標函數（優化feature的pose）.
激光的不同在於其信息量較少， 依據scan-scan的閉環檢測無法在實際中使用，基本採取scan-map的形式。我認爲最大的不同就是當前觀測量的使用。 圖優化的問題是減小機器人運動過程中的累積誤差。（可以先假定這樣的認識， 機器人前面的觀測對應的子地圖 比後面再經過相同地方的子地圖更能反映世界座標系下的地圖信息），那當前觀測量(Z)我們可以對應依據前段地圖（圖中藍色）與當前scan匹配得到調整pose Pj' , 當前觀測預測(h(i,j))依據當前子圖（圖中黃色）與當前scan匹配得到調整pose Pj. 如果沒有累積誤差那e(i,j)應該爲零， 現在就是構建優化函數優化pose使得最後的偏差儘可能小。。。
下面是karto link構建和閉環檢測的過程說明：
4. scanMatcher : scan-map
先介紹下sanmatcher匹配好壞的評價標準。 其實就是調整當前幀激光束對應的位置使得激光束與前面生成的gridmap儘量的重合，重合越高，分數越高，越具有參考價值。
當前開源算法中scanMatcher採取的方法主要是以下四種：
關注點： pose調整的過程要快（範圍要小）， 調整準確，儘可能避免局部最優的干擾（範圍要大）。 收斂要快 ==》 金字塔/分層/多分辨率
scanMatcher 主要涉及兩個評價函數， 一個score用於優化調整粒子pose作爲參考， 一個likelihoodAndScore用於確定優化後的每個粒子pose對應的權重更新作爲參考。 基本依據就是每預幀激光的每個激光點預測距離與統計距離（預測點對應柵格單元的pose統計均值）的高斯距離的統計。
score（快）0依賴收斂判定， 迭代次數與步長，搜索範圍限制。 計算的是每一次迭代的分數， 因此要求儘可能快， 所以只考慮了端點的匹配情況，評分很直接。
likelihoodAndScore （準 / 穩）的計算針對每個粒子權重，總的時長，只受限於粒子數， 計算權重評分時，爲了更穩定，引入了m_kernelSize的矩形窗口選擇，同時保證準確性，參考了 端點hit與鄰近端點的free對應cell的閾值。 使得這時的激光束與地圖的匹配評分更加魯棒。
score
inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
double s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
OrientedPoint lp=p;
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
unsigned int skip=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (skip||*r>m_usableRange||*r==0.0) continue;
Point phit=lp;
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
Point pfree=lp;
pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
bool found=false;
Point bestMu(0.,0.);
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
{
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
//AccessibilityState s=map.storage().cellState(pr);
//if (s&Inside && s&Allocated){
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
Point mu=phit-cell.mean();
if (!found){
bestMu=mu;
found=true;
}else
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
//}
}
}
if (found)
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
}
return s;
}
likelihoodAndScore:
inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
using namespace std;
l=0;
s=0;
const double * angle=m_laserAngles+m_initialBeamsSkip;
OrientedPoint lp=p;//lp laser_pose ref map
lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
lp.theta+=m_laserPose.theta;
double noHit=nullLikelihood/(m_likelihoodSigma);
unsigned int skip=0;
unsigned int c=0;
double freeDelta=map.getDelta()*m_freeCellRatio;
for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
skip++;
skip=skip>m_likelihoodSkip?0:skip;
if (*r>m_usableRange) continue;
if (skip) continue;
Point phit=lp;//laser hit endpoint
phit.x+=*r*cos(lp.theta+*angle);
phit.y+=*r*sin(lp.theta+*angle);
IntPoint iphit=map.world2map(phit);
Point pfree=lp; //laser free nearestly endpoint
pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
pfree=pfree-phit;
IntPoint ipfree=map.world2map(pfree);
bool found=false;
Point bestMu(0.,0.);
for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
{
for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
IntPoint pr=iphit+IntPoint(xx,yy);
IntPoint pf=pr+ipfree;
//AccessibilityState s=map.storage().cellState(pr);
//if (s&Inside && s&Allocated){
const PointAccumulator& cell=map.cell(pr);
const PointAccumulator& fcell=map.cell(pf);
if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
Point mu=phit-cell.mean();
if (!found){
bestMu=mu;
found=true;
}else
bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
}
//}
}
}
if (found){
s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
c++;
}
if (!skip){
double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
l+=(found)?f:noHit;
}
}
return c;
}