最近想着多学习学习slam的相关知识，所以这一篇源码阅读实际上更像是学习记录。
openslam的官方上都吐槽这个当作黑盒子来用还是ok的但是想对算法进行修改需要相当的cpp功力，加油吧。。。
对应的文件会用文件表示，对应的函数用粗体表示，对应的变量用斜体表示。
因为本人水平有限，也是刚开始接触slam，欢迎在评论区交流或者直接邮箱联系我：[email protected]，毕竟大家一起学习是最快的进步方式嘛。
ros的gmapping包是对openslam的gmapping算法做了一层包装，所以想要看gmapping本身算法的话还请在github上下载。
本文参考了gmapping 分析、gmapping源码阅读。感谢大神们。
main.cpp与GMapping初始化
main.cpp中没有什么内容，构造SlamGMapping gn并开启slam。SlamGMapping的构造函数调用gn.init()。
在**gn.init()**中在堆上申请类GMapping::GridSlamProcessor ，是一种基础的FastSLAM算法，使用rb粒子滤波器（可以看概率机器人第13章）。每一个粒子含有自己的姿态以及地图信息。每当读取一对里程计/激光雷达信息时，首先根据里程计信息更新机器人姿态，随后根据新的机器人姿态进行激光匹配算法。真正储存粒子与进行更新都是在这个类中操作。
之后为ros包装与gmapping本身读取数据，至此init()结束。
gn.gn.startLiveSlam()，其中用到tf::MessageFilter，其作用请参考roswiki。这里的代码如下:
//scan_filter_sub_监听激光雷达信息
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
//scan_filter_中第三项是目标frame_id，最后一项是队列长度。
//只有能够进行数据转换的情况下才会调用回调函数。
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
//注册回调函数。
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
//这个很奇怪，该线程只有在析构的时候才会join()，本身又包含了一个while(ros::ok())循环,循环内容为发布tf转换。又看了下整个CPP中只有一个join()，请懂得同学告诉我下这里是怎么做到的。
transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
最后为ros::spin();
startLiveSlam()
mapper初始化initMapper()
得到第一个扫描数据之后开始初始化mapper。确定能不能从base转换到激光雷达的，可以的话记录激光的位置laser_pose，无法转换直接报错跳出。设定在激光雷达z方向1m以上的点，尝试通过tf对这个点进行转换，如果它的确在激光雷达位置上1m的位置，检查tf有没有问题。
如果tf转换也没有问题，那么根据激光雷达位置与0的相对关系以及扫描的最大最小值确定雷达的安装位置以及是否有反转。
根据激光雷达topic的最大最小值标记每个激光数值对应角度。最后为GMapping::GridSlamProcessor参数设置。注意下面这一句，真正对gmapping算法进行初始化：
//传入参数分别是参数数量，x、y方向的最大值最小值(m)，每个像素的大小(m)，初始化姿态（包含当前点的x、y位置以及偏航角）。
gsp_->GridSlamProcessor::init(particles_, xmin_, ymin_, xmax_, ymax_,
delta_, initialPose);
其中内容牵扯到算法本身，还是需要展开说，具体代码在gridslamprocessor.cpp中264行-300行。这里我们会遇到FastSLAM中使用的粒子以及对粒子进行搜索的平衡二叉树（代码中直接使用map）。
void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
m_xmin=xmin;
m_ymin=ymin;
m_xmax=xmax;
m_ymax=ymax;
m_delta=delta;
m_particles.clear();
//构造轨迹树的节点，其构造函数参数如下：机器人在轨迹中的姿态，当前点的权重，累计权重，父节点，子节点的个数。
TNode* node=new TNode(initialPose, 0, 0, 0);
//ScanMatcherMap为typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
//to_do:PointAccumulator现在还不知道具体做什么，字面翻译是点加权器。
//to_do:HierarchicalArray2D现在还不知道具体做什么，字面翻译是2D分级数组。
ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
//m_particles就是vector<particles>，将所有的粒子（默认30个）初始化。
for (unsigned int i=0; i<size; i++){
m_particles.push_back(Particle(lmap));
m_particles.back().pose=initialPose;
m_particles.back().previousPose=initialPose;
m_particles.back().setWeight(0);
m_particles.back().previousIndex=0;
// this is not needed
// m_particles.back().node=new TNode(initialPose, 0, node, 0);
// we use the root directly
m_particles.back().node= node;
}
m_neff=(double)size;
m_count=0;
m_readingCount=0;
m_linearDistance=m_angularDistance=0;
}
至此initMapper()结束。
核心算法包装：add_scan()
gmap_pose是GMapping::OrientedPoint，也就是带方向的点。尝试将当前激光雷达的中心位置转移到里程计坐标系上。如果能够转换成功，就将中心位置的x坐标y坐标与偏航角赋予gmap_pose。
之后将激光雷达的数据、机器人姿态设定至reading中，开始核心算法gsp_->processScan(reading)。
核心算法：processScan(reading)
使用运动模型更新所有粒子
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint& pose(it->pose);
//传入数值分别为：该粒子的姿态，新的姿态，就得姿态
pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
}
//drawFromMotion的内容
OrientedPoint
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
//我也不知道这里为什么是0.3,参考的文章中说这里可能是两轮轴间耦合方差
double sxy=0.3*srr;
//噪声估计
OrientedPoint delta=absoluteDifference(pnew, pold);
OrientedPoint noisypoint(delta);
//生成以0为均值，刮号内值为方差的高斯分布
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
if (noisypoint.theta>M_PI)
noisypoint.theta-=2*M_PI;
//叠加噪声并返回
return absoluteSum(p,noisypoint);
}
//回到processScan
//一个虚函数，这里没有什么内容
onOdometryUpdate();
// accumulate the robot translation and rotation
//重载减号，x、y、theta对应相减
OrientedPoint move=relPose-m_odoPose;
move.theta=atan2(sin(move.theta), cos(move.theta));
//重载乘号，x、y对应相乘
m_linearDistance+=sqrt(move*move);
m_angularDistance+=fabs(move.theta);
m_odoPose=relPose;
bool processed=false;
//只有当是第一次扫描或者距离/角度移动超过某个阈值之后才会更新scan
if (! m_count
|| m_linearDistance>m_linearThresholdDistance
|| m_angularDistance>m_angularThresholdDistance){
//scan-matcher需要读取指针，这里拷贝激光雷达信息给数组指针。
assert(reading.size()==m_beams);
double * plainReading = new double[m_beams];
for(unsigned int i=0; i<m_beams; i++){
plainReading[i]=reading[i];
}
RangeReading* reading_copy =
new RangeReading(reading.size(),
&(reading[0]),
static_cast<const RangeSensor*>(reading.getSensor()),
reading.getTime());
//不是第一次扫描
if (m_count>0){
//扫描匹配最好的粒子
scanMatch(plainReading);
//什么都没做的函数
onScanmatchUpdate();
// 更新树权重。1、归一化；2、重置树（将节点的累计权重置0，让节点等于自己的父节点）；3、重新累加每个粒子的权重
updateTreeWeights(false);
resample(plainReading, adaptParticles);
} /*是第一次扫描*/else {
m_infoStream << "Registering First Scan"<< endl;
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
//只是将布尔值m_activeAreaComputed设置为false
m_matcher.invalidateActiveArea();
//这个函数有点复杂，但是作用好像就是计算每个粒子的（最大）范围
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
//这个函数也有点复杂，计算地图中每个点的交叉熵并且累加
m_matcher.registerScan(it->map, it->pose, plainReading);
// cyr: not needed anymore, particles refer to the root in the beginning!
TNode* node=new TNode(it->pose, 0., it->node, 0);
node->reading=0;
it->node=node;
}
}
// cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
// 更新树权重。1、归一化；2、重置树（将节点的累计权重置0，让节点等于自己的父节点）；3、重新累加每个粒子的权重
updateTreeWeights(false);
// cerr << ".done!" <<endl;
//接下来就是为下次更新做准备。
delete [] plainReading;
m_lastPartPose=m_odoPose; //update the past pose for the next iteration
m_linearDistance=0;
m_angularDistance=0;
m_count++;
processed=true;
//keep ready for the next step
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
it->previousPose=it->pose;
}
}
m_readingCount++;
return processed;
}
inline void GridSlamProcessor::scanMatch(const double* plainReading){
// sample a new pose from each scan in the reference
double sumScore=0;
// 摘抄自第一篇博客，概率机器人6.4节，基本就是把
/* 计算最优的粒子
optimize 调用了 score 这个函数 （计算粒子得分）
在score 函数里，首先计算障碍物的坐标phit，然后将phit转换成网格坐标iPhit
计算光束上与障碍物相邻的非障碍物网格坐标pfree,pfrree由phit沿激光束方向移动一个网格的距离得到，将pfree转换成网格坐标ipfree（增量，并不是实际值）
在iphit 及其附近8个（m_kernelSize:default=1）栅格（pr,对应自由栅格为pf）搜索最优可能是障碍物的栅格。
最优准则： pr 大于某一阈值，pf小于该阈值，且pr栅格的phit的平均坐标与phit的距离bestMu最小。
得分计算： s +=exp(-1.0/m_gaussianSigma*bestMu*besMu) 参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型
至此 score 函数结束并返回粒子（currentPose）得分，然后回到optimize函数
optimize 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转 共6次，然后选取得分最高的位姿，返回最终的得分
*/
//程序6.3返回的是可能的测量值
//NDT算法看这里（http://www.cnblogs.com/21207-iHome/p/8039741.html），简单来说就是如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
OrientedPoint corrected;
double score, l, s;
score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
// it->pose=corrected;
if (score>m_minimumScore){
it->pose=corrected;
}
/* likelihoodAndSocre 作用是计算粒子的权重和（l），如果出现匹配失败，则 l=noHit */
m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
sumScore+=score;
it->weight+=l;
it->weightSum+=l;
//来自第一个文献的说法
/* 计算可活动区域
//set up the selective copy of the active area
//by detaching the areas that will be updated
computeActiveArea 用于计算每个粒子相应的位姿所扫描到的区域
计算过程首先考虑了每个粒子的扫描范围会不会超过子地图的大小，如果会，则resize地图的大小
然后定义了一个activeArea 用于设置可活动区域，调用了gridLine() 函数,这个函数如何实现的，
请参考百度文库那篇介绍。
*/
m_matcher.invalidateActiveArea();
m_matcher.computeActiveArea(it->map, it->pose, plainReading);
}
}