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）沒有迴環檢測，不適合於環路場景。