gmapping 源码详解

本文使用的源码地址：https://github.com/ros-perception/slam_gmapping.git

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

slam_gmapping 程序包作为激光建图的应用层，实现激光点云、里程计数据读取、粒子滤波以及地图的更新。实现topic的订阅、发布。

openslam_gmapping 程序包负责算法具体实现，包括栅格地图构建、粒子滤波、扫描匹配等方法，为应用层提供程序接口。

• 程序框架分析

　　1.main.cpp函数为程序入口，完成了Node的初始化、SlamGMapping类的调用，进入startLiveSlam()成员函数。

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

　　2.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));
  transform_thread_ = new boost::thread(boost::bind(&SlamGMapping::publishLoop, this, transform_publish_period_));
}

　函数订阅了激光扫描、地图等话题，获取到的激光数据后首先通过消息过滤器，实现激光扫描数据的订阅以及tf、frame变换，之后激光数据消息处理回调函数。

　3.laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) 消息回调函数。

　　(1)  函数中判断激光是否为第一帧，当为第一帧时初始化地图，并修改标志位；

　　(2)  调用addScan()函数，添加激光数据，若返回值为真执行(3)；

　　(3)  获取(2)中处理后，粒子最佳匹配位置；

　　(4)  将(3)中获取的位姿用于激光到地图、里程计到地图、地图到里程计 tf 变换；

　　(5)  执行地图更新。

bool got_first_scan_ = false;
void 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);
  if(!got_first_scan_)//直到非第一帧扫描时，初始化地图
  {
    if(!initMapper(*scan))
      return;
    got_first_scan_ = true;
  }
  GMapping::OrientedPoint odom_pose;
  if(addScan(*scan, odom_pose))//添加激光扫描数据
  {
    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变换
    tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));//里程计到地图tf变换
    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");
}

　　4.addScan(const sensor_msgs::LaserScan& scan, GMapping::OrientedPoint& gmap_pose) 函数。

　　输入：scan；

　　返回:pose、processScan()函数返回值。

　　(1)  确保获取到上一次里程计的位姿、时间戳以及相同数量的激光扫描点；

　　(2)  判断是否需将激光扫描逆向反转；

　　(3)  执行RangeReading()，读取点云大小、点云扫描值、gsp_laser_(RangeSensor()返回值）、单位为秒的时间戳到reading，并设置reading的位姿；

　　(4)  执行processScan(reading)。

bool SlamGMapping::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);
}

　　5.openslam_gmapping\gridfastslam\gridslamprocessor文件中的GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticle)；

　　(1)  调用drawFromMotion() 构建里程计误差模型，里程计误差累积；

　　(2)  更新文件输出；

　　(3)  机器人位移超过阈值或时间超过阈值，执行激光点云处理；

　　(4)  执行scanMatch() 实现扫描匹配；

　　(5)  updateTreeWeights(false);//权重更新 形参为权重是否归一化；

　　(6)  resample(plainReading, adaptParticles, reading_copy);//重采样。

 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;
    }
    //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);
      pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);//里程计误差模型构建、叠加
    }
    // update the output file
    if (m_outputStream.is_open()){
      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
      m_outputStream << "ODOM ";
      m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
      m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
      m_outputStream << reading.getTime();
      m_outputStream << endl;
    }
    if (m_outputStream.is_open())
　　{
      m_outputStream << setiosflags(ios::fixed) << setprecision(6);
      m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
      for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
　　  {
       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 << reading.getTime();
      m_outputStream << endl;
    }
    
    //invoke the callback
    onOdometryUpdate();//empty!
    // 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){
      cerr << "***********************************************************************" << endl;
      cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
      cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
      cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 
       << " " <<m_odoPose.theta << endl;
      cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y 
       << " " <<relPose.theta << endl;
      cerr << "***********************************************************************" << endl;
      cerr << "** The Odometry has a big jump here. This is probably a bug in the   **" << endl;
      cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
      cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
      cerr << "***********************************************************************" << endl;
    }
    m_odoPose=relPose;
    bool processed=false;
    // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed 判断位移超过阈值或时间超出执行激光点云处理
    if (! m_count 
    || m_linearDistance>=m_linearThresholdDistance 
    || m_angularDistance>=m_angularThresholdDistance
    || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_))
　　{
      last_update_time_ = reading.getTime();      
      if (m_outputStream.is_open())
　　　　{
    　　　m_outputStream << setiosflags(ios::fixed) << setprecision(6);
    　　　m_outputStream << "FRAME " <<  m_readingCount;
    　　　m_outputStream << " " << m_linearDistance;
         m_outputStream << " " << m_angularDistance << endl;
      }
      if (m_infoStream)
    　    m_infoStream << "update frame " <<  m_readingCount << endl<< "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
      cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y << " " << reading.getPose().theta << endl;
      //this is for converting the reading in a scan-matcher feedable form
      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;
      RangeReading* reading_copy = new RangeReading(reading.size(),//深拷贝
                               &(reading[0]),
                               static_cast<const RangeSensor*>(reading.getSensor()),
                               reading.getTime());
      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);//权重更新 形参为(bool weightsAlreadyNormalized)   
    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, reading_copy);//重采样
   } 
　　else //m_count==0
　　{
    m_infoStream << "Registering First Scan"<< endl;
    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
　　　　{    
       m_matcher.invalidateActiveArea();//设置 m_activeAreaComputed = false
　　　  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;
      node->reading = reading_copy;
      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;
  }

　　　　5.1  drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) 

　　　　功能：构建里程计误差模型

　　　　输入：粒子位姿，里程计真实位姿，前一次位姿

　　　　输出：叠加误差后的粒子位姿

OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
    double sxy=0.3*srr;
    OrientedPoint delta=absoluteDifference(pnew, pold);
    OrientedPoint noisypoint(delta);
    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);
}

　　　　　5.2 scanMatch() 计算粒子得分。

　　　　　(1)  函数调用optimize()计算激光、地图匹配结果，若得分超过阈值认为匹配成功更新位姿，若匹配失败则使用里程计更新位姿；

　　　　　(2)  调用likelihoodAndScore()函数计算粒子的似然和表示权重并更新。

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);//计算每个粒子的得分//    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;    
}

　　　　函数中通过调用optimize()函数，计算粒子得分。

　　　　函数先通过score()函数寻找最优位姿，为使位姿更为准确，通过在初始位姿的基础上，各向移动，查找更高得分的位姿，作为最优位姿。

double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) 
    const{
    double bestScore=-1;
    OrientedPoint currentPose=init;
    double currentScore=score(map, currentPose, readings);//计算当前激光与地图匹配得分、位姿
    double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
    unsigned int refinement=0;
    enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};//枚举移动方向
/*    cout << __PRETTY_FUNCTION__<<  " readings: ";
    for (int i=0; i<m_laserBeams; i++){
        cout << readings[i] << " ";
    }
    cout << endl;
*/    int c_iterations=0;
    do{
          if(bestScore>=currentScore)
            {
                refinement++;
                adelta*=.5;
                ldelta*=.5;
            }
        bestScore=currentScore;
//        cout <<"score="<< currentScore << " refinement=" << refinement;
//        cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
        OrientedPoint bestLocalPose=currentPose;
        OrientedPoint localPose=currentPose;
            //周围各个方向移动，寻找最优位姿
        Move move=Front;
        do {
            localPose=currentPose;
            switch(move){
                case Front:
                    localPose.x+=ldelta;
                    move=Back;
                    break;
                case Back:
                    localPose.x-=ldelta;
                    move=Left;
                    break;
                case Left:
                    localPose.y-=ldelta;
                    move=Right;
                    break;
                case Right:
                    localPose.y+=ldelta;
                    move=TurnLeft;
                    break;
                case TurnLeft:
                    localPose.theta+=adelta;
                    move=TurnRight;
                    break;
                case TurnRight:
                    localPose.theta-=adelta;
                    move=Done;
                    break;
                default:;
            }
            double odo_gain=1;//通过可信度计算里程计增益系数 
            if (m_angularOdometryReliability>0.)
            {
                double dth=init.theta-localPose.theta;     
                dth=atan2(sin(dth), cos(dth));     dth*=dth;
                odo_gain*=exp(-m_angularOdometryReliability*dth);
            }
            if (m_linearOdometryReliability>0.)
            {
                double dx=init.x-localPose.x;
                double dy=init.y-localPose.y;
                double drho=dx*dx+dy*dy;
                odo_gain*=exp(-m_linearOdometryReliability*drho);
            }
            double localScore=odo_gain*score(map, localPose, readings);
            if (localScore>currentScore)
            {
                currentScore=localScore;
                bestLocalPose=localPose;
            }
            c_iterations++;
        } while(move!=Done);
        currentPose=bestLocalPose;
//        cout << "currentScore=" << currentScore<< endl;
        //here we look for the best move;
    }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
    //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
    //cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
    pnew=currentPose;
    return bestScore;
}

　　　　函数中得分通过sore()函数计算得出。

　　　　(1) 将lidar数据转换到世界坐标系下；

　　　　(2) 计算每条激光线击中的点的坐标phit、前一个空闲点free以及各自对应的cell坐标；

　　　　(3) 若cell(phit)>占据阈值且cell(free)<占据阈值转到(4)，否则转到(1)；

　　　　(4) Mu为击中点坐标与cell坐标均值的差值，并标记为找到；

　　　　(5) 计算所有光线的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;//LASER_ANGLES+SKIP
    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;//free 空间分辨率
    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;
            //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;
                    }
                    else
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;//保存最小Mu
                }
            //}
        }
        if (found)//计算雷达地图匹配的 score
            s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
    }
    return s;
}

 　　　　再分析一下scanMatch()中的likelihoodAndScore()函数。

　　　　(1) 计算原理和score函数是一样的，只不过函数里的位姿是匹配成功的最优位姿或者是（匹配不成功时）的里程计位姿；

　　　　(2) 除了该粒子的得分外，还计算了该粒子的所有似然值的和，这个似然和是把没有击中障碍物的激光束也算进去了，这个似然和用来表示该粒子的权重。

inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
    using namespace std;
    l=0;//似然，粒子权重
    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;
    double noHit=nullLikelihood/(m_likelihoodSigma);//激光点未击中 nullLikelihood=-0.5
    unsigned int skip=0;
    unsigned int c=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;//隔m_likelihoodSkip个激光束就跳过本次激光束处理
        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-freeDelta)*cos(lp.theta+*angle);
        pfree.y+=(*r-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;
                    }
　　　　　　　　　　　　else
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
                }
            //}    
        }
        if (found)//计算得分
　　　　　{
            s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
            c++;
        }
        if (!skip)//计算击中似然与未击中的似然总和
　　　　　{
            double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
            l+=(found)?f:noHit;
        }
    }
    return c;
}

 　　　　5.3  至此scanMatch()内主要函数分析完了，再回到processScan()函数，进入updateTreeWeights()函数。

　　　　(1) 若未归一化，执行归一化；

　　　　(2) 重置树的节点信息；

　　　　(3) 更新节点传播权重。

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
  if (!weightsAlreadyNormalized) 
  {
    normalize();//归一化
  }
  resetTree();//重置树
  propagateWeights();//更新树结点的遗传权重
}

 　　　　5.4  resample() 重采样函数。

　　　　重采样函数是为了消除早期SIS粒子滤波器的粒子退化问题。 其基本思想是对赋予权重的粒子集合进行重新采样，从中取出权重较小的粒子，增加权重较大的粒子。

　　　　重采样操作之后，所有的粒子权重都会被设置为一样的。

　　　　虽然，这一操作成功地解决了粒子退化问题，但它带来了一种所谓的粒子匮乏地问题，随着迭代次数的增加，粒子的多样性在下降。

　　　　为了缓解重采样的粒子退化问题，GMapping采取了一种自适应的方式进行重采样。

　　　　它定义了一个指标Neff来评价粒子权重的相似度，只有在Neff小于一个给定的阈值的时候才进行重采样。其背后的思想是，粒子的权重差异越小， 得到的粒子则更接近于对目标分布的采样。

　　　　(1) 根据m_neff判断是否进行重采样；

　　　　(2) 对选取的粒子执行重采样；

　　　　(3) 更新粒子地图与轨迹。

inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)
{
  bool hasResampled = false;//重采样完成标志
  TNodeVector oldGeneration;//存储粒子状态的容器 Vector
  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())//m_neff在updateTreeWeights中更新，衡量粒子权重的相似性 当小于阈值时，执行重采样
  {         
    if (m_infoStream)
      m_infoStream  << "*************RESAMPLE***************" << std::endl; 
    uniform_resampler<double, double> resampler;
    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);//返回一个std::vector<unsigned int>类型的数组，记录了采样后的样本在原始粒子集合的索引。
    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=new    TNode(p.pose, 0, oldNode, 0);
      //node->reading=0;
      node->reading=reading;
      //            cerr << "A("<<node->parent->childs <<") " <<endl;
      temp.push_back(p);//将选取的样本记录在集合temp中
      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;
}

 　　6.  最后回到 lasercallback()执行updateMap();

　　根据粒子权值预测下一时刻粒子的最优解，并进行地图更新。

void SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
  ROS_DEBUG("Update map");
  boost::mutex::scoped_lock map_lock (map_mutex_);
  GMapping::ScanMatcher matcher;
  matcher.setLaserParameters(scan.ranges.size(), &(laser_angles_[0]),gsp_laser_->getPose());
  matcher.setlaserMaxRange(maxRange_);
  matcher.setusableRange(maxUrange_);
  matcher.setgenerateMap(true);
  GMapping::GridSlamProcessor::Particle best = gsp_->getParticles()[gsp_->getBestParticleIndex()];//将权重最大的粒子作为里程计下一时刻的最优解
  std_msgs::Float64 entropy;
  entropy.data = computePoseEntropy();//计算粒子集的熵
  if(entropy.data > 0.0)
    entropy_publisher_.publish(entropy);
  if(!got_map_) {
    map_.map.info.resolution = delta_;
    map_.map.info.origin.position.x = 0.0;
    map_.map.info.origin.position.y = 0.0;
    map_.map.info.origin.position.z = 0.0;
    map_.map.info.origin.orientation.x = 0.0;
    map_.map.info.origin.orientation.y = 0.0;
    map_.map.info.origin.orientation.z = 0.0;
    map_.map.info.origin.orientation.w = 1.0;
  } 
  GMapping::Point center;
  center.x=(xmin_ + xmax_) / 2.0;
  center.y=(ymin_ + ymax_) / 2.0;
  GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, delta_);
  ROS_DEBUG("Trajectory tree:");
  for(GMapping::GridSlamProcessor::TNode* n = best.node;n;n = n->parent)
  {
    ROS_DEBUG("  %.3f %.3f %.3f",
              n->pose.x,
              n->pose.y,
              n->pose.theta);
    if(!n->reading)
    {
      ROS_DEBUG("Reading is NULL");
      continue;
    }
    matcher.invalidateActiveArea();//计算粒子扫描到的自由空间
    matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
    matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
  }
  // the map may have expanded, so resize ros message as well 
  if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) //地图已膨胀，更新地图消息
  {
    // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
    //       so we must obtain the bounding box in a different way
    GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
    GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
    xmin_ = wmin.x; ymin_ = wmin.y;
    xmax_ = wmax.x; ymax_ = wmax.y;
    ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),xmin_, ymin_, xmax_, ymax_);
    map_.map.info.width = smap.getMapSizeX();
    map_.map.info.height = smap.getMapSizeY();
    map_.map.info.origin.position.x = xmin_;
    map_.map.info.origin.position.y = ymin_;
    map_.map.data.resize(map_.map.info.width * map_.map.info.height);
    ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
  }
  for(int x=0; x < smap.getMapSizeX(); x++)
  {
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
      /// @todo Sort out the unknown vs. free vs. obstacle thresholding
      GMapping::IntPoint p(x, y);
      double occ=smap.cell(p);
      assert(occ <= 1.0);
      if(occ < 0)//未知空间
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
      else if(occ > occ_thresh_)//超过阈值，为障碍物
      {
        //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
      }
      else
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
    }
  }
  got_map_ = true;
  //make sure to set the header information on the map
  map_.map.header.stamp = ros::Time::now();
  map_.map.header.frame_id = tf_.resolve( map_frame_ );
  sst_.publish(map_.map);
  sstm_.publish(map_.map.info);
}

 

• 思路概括：

　　(1) 接收激光信息、里程计信息，并根据里程计信息构建误差模型；

　　(2) 初始化粒子群，遍历上一时刻粒子群，获取位姿、权重、地图；

　　(3) 通过里程计进行位姿更新，通过极大似然估计求得局部极值，获取最优粒子；

　　(4) 若未找到局部极值，通过观测模型对位姿更新；

　　(5) 若找到局部极值，选取周围k个粒子位姿，计算其高斯分布、均值、权重、方差，近似新位姿，并对对粒子位姿权重进行更新；

　　(6) 扩充地图、粒子群；

　　(7) 根据粒子权重离散程度选择是否对新的粒子位姿重采样。

• 参考资料

　　[1] https://zhuanlan.zhihu.com/p/262287388?utm_source=wechat_session

　　[2] https://blog.csdn.net/weixin_42048023/article/details/85620544

　　[3] http://gaoyichao.com/Xiaotu/?book=turtlebot&title=gmapping%E7%9A%84%E9%87%8D%E9%87%87%E6%A0%B7%E8%BF%87%E7%A8%8B