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