前段时间为了分享激光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;
}