Gmapping_slam源码框架分析

作为非常经典的激光slam系统，发展到现在，非常有值得学习的地方。
同样因为经典，类似的分析类博客也是非常多了，这里仅从阅读源码入手，就当作是一次学习记录吧。

参考论文：Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
源码阅读详解
GMapping原理分析

gmappinig的重点是扫描匹配和粒子滤波，没有回环估计。

一、总体结构

通过阅读源码，对各个函数的调用关系进行了梳理，流程如下：

其中，重要函数都标记成了红色；两个涉及的两个文件夹各自包括了CPP文件夹和H头文件文件夹。

二、扫描匹配

扫描匹配的思路是在基于运动模型预测的位姿，向负x，正x，负y，正y，左旋转，右旋转共六个状态移动预测位姿，计算每个状态下的匹配得分，取最高得分对应的位姿为最优位姿。
扫描匹配的重点就在于如何计算匹配得分，取得分最高对应的位姿为估计位姿。那么，计算匹配得分的函数就是下面的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;
			const PointAccumulator& cell=map.cell(pr);//获得障碍物点占据值
			const PointAccumulator& fcell=map.cell(pf);//获得非障碍物点空闲值
			
			//必须要满足cell是被占用的，而fcell是空闲的
			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;
}

得分计算： s +=exp(-1.0 / m_gaussianSigma * bestMu * besMu)（ 参考NDT算法：距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型）。

相关数学原理可参考：slam-gmapping之scanMatch算法原理

三、粒子滤波

改进的提议分布：不但考虑运动（里程计）信息还考虑最近的一次观测（激光）信息这样就可以使提议分布的更加精确从而更加接近目标分布。
选择性重采样：通过设定阈值，只有在粒子权重变化超过阈值时才执行重采样从而大大减少重采样的次数。
第二个重点就是粒子滤波里的重采样了，工程方面尤其需要注意粒子的添加操作和删除操作：

inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)
{
	  bool hasResampled = false;

	//备份老的粒子
	  TNodeVector oldGeneration;
	  for (unsigned int i=0; i<m_particles.size(); i++){
		oldGeneration.push_back(m_particles[i].node);
	  }
	  
	  //如果Neff小于阈值则重采样
	  if (m_neff<m_resampleThreshold*m_particles.size()){		
    
		if (m_infoStream)
			m_infoStream  << "*************RESAMPLE***************" << std::endl;
    
    	//进行重采样，返回保留粒子的下标
		uniform_resampler<double, double> resampler;
		m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
    
		if (m_outputStream.is_open()){
			m_outputStream << "RESAMPLE "<< m_indexes.size() << " ";
			for (std::vector<unsigned int>::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){
			  m_outputStream << *it <<  " ";
			}
			m_outputStream << std::endl;
		}
    
		onResampleUpdate();//空函数
		//BEGIN: BUILDING TREE
		ParticleVector temp;
		unsigned int j=0;
		
		//记录需要删除的粒子的下标
		std::vector<unsigned int> deletedParticles;  	
		//		cerr << "Existing Nodes:" ;
		for (unsigned int i=0; i<m_indexes.size(); i++){//枚举每一个要被保留的粒子
			//			cerr << " " << m_indexes[i];
			while(j<m_indexes[i]){
			  deletedParticles.push_back(j);//统计要被删除的粒子
			  j++;
			}
			if (j==m_indexes[i])
			  j++;
			Particle & p=m_particles[m_indexes[i]];

			//为保留的粒子增加新节点，并改父节点为保留粒子
			TNode* node=0;
			TNode* oldNode=oldGeneration[m_indexes[i]];
			//			cerr << i << "->" << m_indexes[i] << "B("<<oldNode->childs <<") ";
			node=new	TNode(p.pose, 0, oldNode, 0);
			//node->reading=0;
			node->reading=reading;
			//			cerr << "A("<<node->parent->childs <<") " <<endl;
		  
			temp.push_back(p);
			temp.back().node=node;
			temp.back().previousIndex=m_indexes[i];
		}
		while(j<m_indexes.size()){
		  deletedParticles.push_back(j);
		  j++;
		}

		//删除粒子
		//		cerr << endl;
		std::cerr <<  "Deleting Nodes:";
		for (unsigned int i=0; i<deletedParticles.size(); i++){
		  std::cerr <<" " << deletedParticles[i];
		  delete m_particles[deletedParticles[i]].node;
		  m_particles[deletedParticles[i]].node=0;
		}
		std::cerr  << " Done" <<std::endl;
		
		//END: BUILDING TREE
		std::cerr << "Deleting old particles..." ;
		m_particles.clear();
		std::cerr << "Done" << std::endl;
		std::cerr << "Copying Particles and  Registering  scans...";

		//对保留下来的每个粒子进行地图更新
		for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
		  it->setWeight(0);
		  m_matcher.invalidateActiveArea();
		  m_matcher.registerScan(it->map, it->pose, plainReading);
		  m_particles.push_back(*it);
		}
		std::cerr  << " Done" <<std::endl;
		hasResampled = true;
	} else {//不进行重采样的话，直接进行粒子的地图更新
		  int index=0;
		  std::cerr << "Registering Scans:";
		  TNodeVector::iterator node_it=oldGeneration.begin();
		  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
			//create a new node in the particle tree and add it to the old tree
			//BEGIN: BUILDING TREE  
			TNode* node=0;
			node=new TNode(it->pose, 0.0, *node_it, 0);
			
			//node->reading=0;
			node->reading=reading;
			it->node=node;

			//END: BUILDING TREE
			m_matcher.invalidateActiveArea();
			m_matcher.registerScan(it->map, it->pose, plainReading);
			it->previousIndex=index;
			index++;
			node_it++;
			
		  }
		  std::cerr  << "Done" <<std::endl;
	}
		//END: BUILDING TREE
	return hasResampled;
}

四、优缺点

1.优点

（1）小场景构图计算量较小，精度较高。
（2）对激光雷达频率要求低、鲁棒性高。

2.缺点

（1）严重依赖里程计，不适用于地面不平坦的情况。
（2）构建大地图时计算量大，因为每个粒子会携带一张地图。
（3）没有回环检测，不适合于环路场景。