ROS笔记：gmapping源码分析

概述

源码地址：

https://github.com/ros-perception/openslam_gmapping

https://github.com/ros-perception/slam_gmapping

程序入口：main.cpp

int
main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");
 
  SlamGMapping gn;
  gn.startLiveSlam();
  ros::spin();
 
  return(0);
}

定义生成gn时，运行了SlamGMapping::init()这个成员函数：

void SlamGMapping::init()
{
  // 省略... ...
  gsp_ = new GMapping::GridSlamProcessor(); //构造GridSlamProcessor类 利用粒子滤波
  ROS_ASSERT(gsp_);

  tfB_ = new tf::TransformBroadcaster(); //创建TransformBroadcaster
  ROS_ASSERT(tfB_);
  
  // 省略... ...
   
  // Parameters used by our GMapping wrapper
  if(!private_nh_.getParam("throttle_scans", throttle_scans_))
    throttle_scans_ = 1;
  if(!private_nh_.getParam("base_frame", base_frame_))
    base_frame_ = "base_link";
  if(!private_nh_.getParam("map_frame", map_frame_))
    map_frame_ = "map";
  if(!private_nh_.getParam("odom_frame", odom_frame_))
    odom_frame_ = "odom";

  private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);

  double tmp;
  if(!private_nh_.getParam("map_update_interval", tmp))
    tmp = 5.0;
  map_update_interval_.fromSec(tmp);
  
  // Parameters used by GMapping itself #GMapping 所用的各种参数 
  maxUrange_ = 0.0;  maxRange_ = 0.0; // preliminary default, will be set in initMapper()
 
  // 省略... ...
}

SlamGMapping::startLiveSlam()中
（1）先订阅激光雷达的数据
（2）通过boost::bind开启两个线程：laserCallback()和 publishLoop()，
在核心线程laserCallback中 ：

laserCallback()
{
  initMapper()  //进行参数初始化
                //gsp_->GridSlamProcessor::init()。
  addScan()     //调用openslam_gmappinig核心算法 gsp_->processScan（）

  updateMap()   //对地图进行更新
}

initMapper函数处理第一次收到激光数据时候地图的初始化问题，被laserCallback函数调用。
updateMap函数每超过map_update_interval_时间就要被laserCallback函数调用调用更新地图。
在addScan函数中：

SlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose)
{

//省略的对激光数据的处理 ... ...

  GMapping::RangeReading reading(scan.ranges.size(),
                                 ranges_double,
                                 gsp_laser_,
                                 scan.header.stamp.toSec());
  delete[] ranges_double;
  reading.setPose(gmap_pose);
  ROS_DEBUG("processing scan");
  return gsp_->processScan(reading); 
}

调用了openslam里面的GridSlamProcessor::processScan函数，这个是gmapping核心的代码。我们先来看看核心算法部分的代码主要内容。

核心算法处理

  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
    
    /**retireve the position from the reading, and compute the odometry*/
    /*从reading中获取odometry数据*/
    OrientedPoint relPose=reading.getPose();
    /*第0次是特殊处理,将当前里程计值赋值给上一次的值。relPose 表示里程计算出来的当前位置*/
    /*m_odoPose 表示里程计上一次的位置 m_count 表示更新次数*/
    if (!m_count){
      m_lastPartPose=m_odoPose=relPose;
    }
    
    //write the state of the reading and update all the particles using the motion model
    /*对于每一个粒子，都从里程计运动模型中采样*/
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
      OrientedPoint& pose(it->pose);
      // Step.1※※里程计运动模型问题※※
      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
    }

    // update the output file
    if (m_outputStream.is_open()){
		//省略 。。。 。。。
    }
    
    //invoke the callback
    onOdometryUpdate();
    
    // accumulate the robot translation and rotation
    // 算出里程计在这个周期内跑了多远
    OrientedPoint move=relPose-m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));
    m_linearDistance+=sqrt(move*move);
    m_angularDistance+=fabs(move.theta);
    
    // if the robot jumps throw a warning
    // 如果算出跑的距离超过了阈值，就报警
    if (m_linearDistance>m_distanceThresholdCheck){
       //省略 。。。 。。。
    }   
     //下次迭代开始
    m_odoPose=relPose;
    bool processed=false;

    // process a scan only if the robot has traveled a given distance
    // 如果里程计大于某个阈值，就处理一批激光数据。可参照视觉SLAM关键帧的内在含义   
    if (! m_count 
	|| m_linearDistance>m_linearThresholdDistance 
	|| m_angularDistance>m_angularThresholdDistance){
      
      //省略 。。。 。。。
     
      //this is for converting the reading in a scan-matcher feedable form
      //把激光雷达数据转换成openslam_gmapping可以读懂的格式     
      assert(reading.size()==m_beams);
      double * plainReading = new double[m_beams];
      for(unsigned int i=0; i<m_beams; i++){
		plainReading[i]=reading[i];
      }
      m_infoStream << "m_count " << m_count << endl;
      if (m_count>0){ //如果不是第一帧激光数据
	      scanMatch(plainReading); //Step.2 为每个粒子进行scanMatch，计算出每个粒子的最优位姿
       //省略 。。。 。。。
	
	onScanmatchUpdate();
	//Step.3 由于scanmatch之中对粒子的权重进行了更新，那么这时候 各个粒子的轨迹上的累计权重都需要重新计算
	updateTreeWeights(false);
    //省略 。。。 。。。
    // Step.4 重采样
	resample(plainReading, adaptParticles);
	
      } else { //如果是第一帧激光数据，直接计算ActiveArea，此时机器人的位姿都是（0，0，0）
		for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){	
		  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;
		}
      }
      updateTreeWeights(false);
      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;
      }
    }
    if (m_outputStream.is_open())
      m_outputStream << flush;
    m_readingCount++;
    //bool 型函数，处理完毕processed=true。
    return processed;
  }
  

core.1 运动模型

先来看一下 在motionmodel.cpp文件中的drawFromMotion这个函数，它实现了里程计运动模型，采用如下图的公式（取自中文版《概率机器人》第103页）。

core.2 权重计算

接下来看一下GMapping::GridSlamProcessor::scanMatch这个函数。

scanMatch()：实现通过扫描匹配每一个粒子，采样出新的位姿，进行权重计算。利用最近一次的观测来提高Proposal distribution进行优化。
rbpf-gmapping直接使用的是运动模型作为提议分布

score()：计算最优的粒子，原理就参考 《Probabilistic Robot》 一书的P143 页 ， likelihood_field_range_finder_mode。
在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_gaussianSigmabestMubesMu) 参考NDT算法,距离越大，分数越小，分数的较大值集中在距离最小值处，符合正态分布模型
至此 score 函数结束并返回粒子（currentPose）得分，然后回到optimize函数
optimize() 干的事就是 currentPose 的位姿进行微调，前、后、左、右、左转、右转 共6次，然后选取得分最高的位姿，返回最终的得分

likelihoodAndScore()：重新计算权重。

/**Just scan match every single particle.
If the scan matching fails, the particle gets a default likelihood.*/
inline void GridSlamProcessor::scanMatch(const double* plainReading){
  // sample a new pose from each scan in the reference
  //  激光数据以plainReading的封装形式加入到函数当中来
  double sumScore=0;
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
    OrientedPoint corrected;
    double score, l, s;
    //optimize 函数返回了bestScore，同时计算了corrected，即新位姿
    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
    //    it->pose=corrected;
    // 当计算出的分数比较靠谱后，就用corrected值更新当前粒子的位姿
    if (score>m_minimumScore){
      it->pose=corrected;
    } else {
		 //省略。。。 。。。
    }
    //likelihoodAndSocre 返回了l的数值用于计算粒子的权重
    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
    m_matcher.invalidateActiveArea(); // invalidateActiveArea是这个函数的开关
    //建图的过程，生成地图的函数，使用Bresenham算法
    // computeActiveArea函数的作用有两个：一是调整地图：把每个粒子所对应的扫描范围给计算出来，然后调整地图的大小使得上述扫描范围不会超过地图的大小。
    // 二是计算活动区域：调用gridLine() 函数将所有粒子与其所对应的phit(激光打到障碍物上的终点)连接起来，从而形成一片activeArea。
    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
  }
}

core.3 权重更新

由于scanmatch之中对粒子的权重进行了更新，那么这时候 各个粒子的轨迹上的累计权重都需要重新计算, 因此我们需要用updateTreeWeights子函数进行粒子权重的更新。

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){

  if (!weightsAlreadyNormalized) {
    normalize();//归一化粒子权重,并计算权重相似度Neff，用于判定是否需要进行重采样
  }
  resetTree();//重置轨迹树：遍历了所有的粒子，并沿着各个粒子的node节点向上追溯到根节点，
//并将遍历过程中各个节点的权重计数和访问计数器都设置为0。
  propagateWeights();//更新轨迹树权重：从叶子节点向上追溯到根节点，更新沿途经过的各个节点的权重和累积权重
}

详细可参考：https://gaoyichao.com/Xiaotu

core.4 选择性重采样

processScan 这个函数调用了位于gridslamprocessor.hxx中的resample子函数，其代码如下：
registerScan()：分配内存，生成地图

/**
频繁的重采样导致粒子退化，历史久远的位姿丧失其自身的多样性
plainReading 是激光的原始数据；
adaptSize用于调节重采样的粒子数目；
reading包含了带时间戳的大量传感器信息
**/
inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* ){
  
  bool hasResampled = false; //条件开关，兼职返回值
  
  TNodeVector oldGeneration; //轨迹树向量的容器，记录上一代粒子状态
  for (unsigned int i=0; i<m_particles.size(); i++){
    oldGeneration.push_back(m_particles[i].node); //上一代粒子
  }
  /**
  * 阈值由这个积定义，当函数updateTreeWeights中定义的Neff 小于此阈值，则开始重采样
  **/
  if (m_neff<m_resampleThreshold*m_particles.size()){		
   	//采取重采样方法决定，哪些粒子会保留，保留的粒子会保留下标，有些粒子会重复采样
	//而另外一些粒子会消失掉 
    //uniform_resampler是结构体模板定义的重采样器，在particle.h中定义，包含resampleIndexes等成员
    uniform_resampler<double, double> resampler;
    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);
    //省略 。。。 。。。  
    onResampleUpdate(); //与算法无关
    // 开始对对粒子的更新处理
    //BEGIN: BUILDING TREE
    ParticleVector temp; //重采样后的粒子集合
    unsigned int j=0;
    std::vector<unsigned int> deletedParticles;  		//this is for deleteing the particles which have been resampled away.
    //		不在选择索引中的粒子被干掉
    for (unsigned int i=0; i<m_indexes.size(); 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]];
      node=new	TNode(p.pose, 0, oldNode, 0);
      node->reading=0;
// 记录到temp粒子集合中        
      temp.push_back(p);
      temp.back().node=node;
      temp.back().previousIndex=m_indexes[i];
    }
    while(j<m_indexes.size()){ //释放内存
      deletedParticles.push_back(j);
      j++;
    }
    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;
    }
    
    //END: BUILDING TREE
    // 重采样后的最新粒子数据去更新匹配器中的各种数据，如地图、位姿、激光等
    m_particles.clear();
    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);
    }
    hasResampled = true;
  } else {
/** 
如果不满足重采样条件，则依旧需要更新 1.各个粒子轨迹树；2.匹配器中的各种数据，如地图、位姿、激光、活动区域等   
**/ 
    int index=0;
    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;
      it->node=node;

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

频繁的重采样导致粒子退化，历史久远的位姿丧失其自身的多样性。GMapping使用 [公式] 这个指标来限制重采样次数。

其定义为： [公式]

当 [公式] 降低到某个阈值以下，说明粒子的分布与真实分布差距很大，具体表现为某些粒子离真实值很近，而很多粒子离真实值较远，重采样就应该在此时进行。

重采样的时间复杂度为 [公式] ，其中N为粒子数，M为地图大小，因此采用用[公式] 这个指标来限制重采样次数意义重大。

参考论文:
Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters
参考文献：
https://zhuanlan.zhihu.com
https://blog.csdn.net/
https://gaoyichao.com/Xiaotu/?
原理参考
http://blog.csdn.net/heyijia0327/article/details/40899819 pf原理讲解
http://blog.csdn.net/u010545732/article/details/17462941 pf代码实现
http://www.cnblogs.com/yhlx125/p/5634128.html gmapping分析
http://wenku.baidu.com/view/3a67461550e2524de4187e4d.html?from=search gmapping 分析
其他参考 ：
http://ishare.iask.sina.com.cn/f/24615049.html