GMapping源码分析之随手笔记

GMapping代码分析的随手笔记记载如下：

参考博客： http://blog.csdn.net/roadseek_zw/article/details/53316177  gmapping分析

都说最好的理解就是说出来让别人听懂，所以我也写下我的分析来加深自己的理解，同时给刚刚入门的同学们带个头。这一篇是我对gmapping源码的分析与理解(刚入门，难免有错，欢迎指正，相互学习)。恩，现在还只是大概的分析，还有很多代码没有懂，由于博主不需要很深入的研究SLAM，所以只是浅浅的谈一下，不喜勿喷。

首先我们从Slam_gamping-hydro-devel包的主函数开始看起：

intmain(int argc, char** argv){  ros::init(argc, argv, "slam_gmapping");    SlamGMapping gn;         gn.startLiveSlam();        //这里进入slam_gammping.cpp的startLiveSlam()函数  ros::spin();  return(0);}

我们看一下startLiveSlam函数：

void SlamGMapping::startLiveSlam(){  entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);  sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);  sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);  ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);    scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);  scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);  scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));    //从这里进入同一个文件的laserCallback函数  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));// line 339}

SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan){  laser_count_++;  if ((laser_count_ % throttle_scans_) != 0)    return;  static ros::Time last_map_update(0,0);  // We can't initialize the mapper until we've got the first scan  if(!got_first_scan_)  {    if(!initMapper(*scan))     // 进入initMapper 函数      return;    got_first_scan_ = true;  }  GMapping::OrientedPoint odom_pose;  if(addScan(*scan, odom_pose))　　　// 进入addScan函数，  {    ROS_DEBUG("scan processed");    GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;    ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);    ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);    ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);    tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));    map_to_odom_mutex_.lock();    map_to_odom_ = (odom_to_laser * laser_to_map).inverse();    map_to_odom_mutex_.unlock();    if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)    {      updateMap(*scan);      last_map_update = scan->header.stamp;      ROS_DEBUG("Updated the map");    }  } else    ROS_DEBUG("cannot process scan");}

addScan函数：

boolSlamGMapping::addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose){  if(!getOdomPose(gmap_pose, scan.header.stamp))     return false;    if(scan.ranges.size() != gsp_laser_beam_count_)    return false;  // GMapping wants an array of doubles...  double* ranges_double = new double[scan.ranges.size()];  // If the angle increment is negative, we have to invert the order of the readings.  if (do_reverse_range_)  {    ROS_DEBUG("Inverting scan");    int num_ranges = scan.ranges.size();    for(int i=0; i < num_ranges; i++)// 把小于扫描最短范围的数据滤出    {      // Must filter out short readings, because the mapper won't      if(scan.ranges[num_ranges - i - 1] < scan.range_min)        ranges_double[i] = (double)scan.range_max;      else        ranges_double[i] = (double)scan.ranges[num_ranges - i - 1];    }  } else   {    for(unsigned int i=0; i < scan.ranges.size(); i++)// 把小于扫描最短范围的数据滤出    {      // Must filter out short readings, because the mapper won't      if(scan.ranges[i] < scan.range_min)        ranges_double[i] = (double)scan.range_max;      else        ranges_double[i] = (double)scan.ranges[i];    }  }  GMapping::RangeReading reading(scan.ranges.size(),                                 ranges_double,                                 gsp_laser_,                                 scan.header.stamp.toSec());  // ...but it deep copies them in RangeReading constructor, so we don't  // need to keep our array around.  delete[] ranges_double;  reading.setPose(gmap_pose);  /*  ROS_DEBUG("scanpose (%.3f): %.3f %.3f %.3f\n",            scan.header.stamp.toSec(),            gmap_pose.x,            gmap_pose.y,            gmap_pose.theta);            */  ROS_DEBUG("processing scan");  return gsp_->processScan(reading);　　　// 从这里进入particle filter }

processScan 在openslam.org上下载的gmapping源码包里定义：

gridslamprocessor.h 中

namespace GMapping {　　    bool processScan(const RangeReading & reading, int adaptParticles=0);｝

之后进入gridslamprocessor.cpp中看一下该方法过程，之后就是一个160多行的代码了。。。在这里暴露了博主c++小白一个，哈哈

  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){        /**retireve the position from the reading, and compute the odometry*/    OrientedPoint relPose=reading.getPose();    if (!m_count){      m_lastPartPose=m_odoPose=relPose;    } // ...省略一些行 　m_odoPose=relPose;        bool processed=false;    // process a scan only if the robot has traveled a given distance    if (! m_count || m_linearDistance>m_linearThresholdDistance || m_angularDistance>m_angularThresholdDistance){　　// 只有当距离超过阈值时才进行一个scan      // ... 省略　　　 if (m_count>0){scanMatch(plainReading);　　// 进入扫描匹配if (m_outputStream.is_open()){  m_outputStream << "LASER_READING "<< reading.size() << " ";  m_outputStream << setiosflags(ios::fixed) << setprecision(2);  for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){    m_outputStream << *b << " ";  }  OrientedPoint p=reading.getPose();  m_outputStream << setiosflags(ios::fixed) << setprecision(6);  m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;  m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){    const OrientedPoint& pose=it->pose;    m_outputStream << setiosflags(ios::fixed) << setprecision(3) <<  pose.x << " " << pose.y << " ";    m_outputStream << setiosflags(ios::fixed) << setprecision(6) <<  pose.theta << " " << it-> weight << " ";  }  m_outputStream << endl;}onScanmatchUpdate();updateTreeWeights(false);if (m_infoStream){  m_infoStream << "neff= " << m_neff  << endl;}if (m_outputStream.is_open()){  m_outputStream << setiosflags(ios::fixed) << setprecision(6);  m_outputStream << "NEFF " << m_neff << endl;}resample(plainReading, adaptParticles);      } else {m_infoStream << "Registering First Scan"<< endl;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=newTNode(it->pose, 0., it->node,  0);  node->reading=0;  it->node=node;  }      }      //cerr  << "Tree: normalizing, resetting and propagating weights at the end..." ;      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;      }          }    if (m_outputStream.is_open())      m_outputStream << flush;    m_readingCount++;    return processed;  }  

今天先写到这里，之后再改，目前代码看到这里

上图是processScan 的函数调用

scanMatch 函数体：

inline void GridSlamProcessor::scanMatch(const double* plainReading){  // sample a new pose from each scan in the reference    double sumScore=0;  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);　//　optimize 将调用　score();    //    it->pose=corrected;    if (score>m_minimumScore){      it->pose=corrected;    } else {if (m_infoStream){  m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;  m_infoStream << "lp:" << m_lastPartPose.x << " "  << m_lastPartPose.y << " "<< m_lastPartPose.theta <<std::endl;  m_infoStream << "op:" << m_odoPose.x << " " << m_odoPose.y << " "<< m_odoPose.theta <<std::endl;}    }    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();    m_matcher.computeActiveArea(it->map, it->pose, plainReading);  }  if (m_infoStream)    m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;}

恩，现在看不懂数学过程，具体的还得仔细看书啊。　

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 (*r>m_usableRange) continue;if (skip) 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;}elsebestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;}//}}if (found)s+=exp(-1./m_gaussianSigma*bestMu*bestMu);}return s;}

之后　权重计算　updateTreeWeights(false);

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){  if (!weightsAlreadyNormalized) {    normalize();  }  resetTree();  propagateWeights();}

resample();

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);  }    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;  //this is for deleteing the particles which have been resampled away.        //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=newTNode(p.pose, 0, oldNode, 0);      node->reading=0;      //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;      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;}