Gmapping的个人理解

Gmapping

第0章 源码阅读的一些预准备

0.1、算法简介

对于建图,SLAM,也称为 CML (Concurrent Mapping and Localization), 我们从名字就可以得知,其包含机器人的定位与地图的构建两部分,或者说并发建图与定位。对于这个问题的模型,就是如果将一个机器人放入未知环境中的未知位置,是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图(完全的地图是指不受障碍行进到房间可进入的每个角落,也就是熟知地图的障碍点)。定位与建图相辅相成、互相影响。
首先,构建地图要知道机器人的精确位姿,精确定位又需要给定的地图做参考。先讲已知精确位姿(坐标和朝向)的地图创建,机器人位置已知,通过激光雷达扫描到环境特征,即障碍物距离。可通过机器人坐标和朝向以及障碍物距离计算出障碍物的坐标,采用bresenham直线段扫面算法,障碍物所处的栅格标注为 occupy,机器人所处的栅格与障碍物所处的栅格之间画直线,直线所到之处都为 free。当然每个栅格并不是简单的非 0 即 1,栅格的占据可用概率表示,若某一个栅格在激光束 a 扫描下标识为 occupy,在激光束 b 扫描下也标识为 occupy,那该栅格的占据概率就变大,反之,则变小。这样,机器人所处的环境就可以通过二维栅格地图来表征。

0.2、gmapping安装

运行指令：sudo apt-get install ros-kinetic-slam-gmapping

运行gmapping：

1.运行roscore

roscore

2.新开终端，设置参数，确保在任何节点使用前设定use_sim_time参数true，这个参数当回放bag数据集是设置为true，此时说明系统使用的是仿真时间

rosparam set use_sim_time true

3.新开终端，运行slam_gmapping节点，它将在base_scan主题上收听激光扫描数据

rosrun gmapping slam_gmapping scan:=base_scan

4.新开终端，进入下载的数据的位置，并play，例如我的是test.bag，把它改成你下载的数据的名字，它的作用是把激光数据发送给slam_gmapping节点。

rosbag play --clock test.bag

0.3、测试结果

第1章 源码阅读的一些预准备

从本文开始,我们会开始对gmapping源码的阅读。我会试图给出逐行或逐块的注释,来说明代码在做什么以及如何跳转。通常,我阅读一个大的project的源码最困难的地方就是难以理解程序跳转来转去的关系。

1.1、代码框架

代码中主要包含slam_gmapping和openslam_gmapping两个包，里面有大量的辅助文件，重点便是以上两个框里面的函数。

接下来便主要分析各个函数的全部作用以及其实现原理。

;

第2章 代码核心部分

2.1、main函数

#include

#include "slam_gmapping.h"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "slam_gmapping");*

  *
  SlamGMapping gn;
  gn.startLiveSlam();*
  ros::spin();*

  return(0);
}

主函数的目的主要是初始化节点，以及订阅一些话题，等待回调函数中实现对应算法功能。
这里着重介绍一下gn.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", &amp;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(&amp;SlamGMapping::laserCallback, this, _1));
        *
        std::cout <<"Subscribe LaserScan & odom!!!"<<std::endl;
    }

    **
    transform_thread_ = new boost::thread(boost::bind(&amp;SlamGMapping::publishLoop, this, transform_publish_period_));
}

这里我们开始阅读它的回调函数SlamGMapping::laserCallback()

2.2、laserCallback()函数

**
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::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");
}

下面从这个函数跳转到add_scan()

2.3、add_scan()函数

**
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;

    *
    double* ranges_double = new double[scan.ranges.size()];

    *
    *
    if (do_reverse_range_)
    {
        ROS_DEBUG("Inverting scan");
        int num_ranges = scan.ranges.size();
        for(int i=0; i < num_ranges; i++)
        {
            *
            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++)
        {
            *
            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());

    *
    *
    *
    delete[] ranges_double;

    *
    reading.setPose(gmap_pose);

    *
    ROS_DEBUG("processing scan");

    *
    return gsp_->processScan(reading);
}

这时候就进入processScan函数中了，这个函数里面放着SLAM算法，也是算法的核心函数。

2.4、processScan()函数

  **
  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
  {
    **
    **
        OrientedPoint relPose=reading.getPose();
    *

        **
        if (!m_count)
        {
      m_lastPartPose=m_odoPose=relPose;
    }

    *
        **
    int tmp_size = m_particles.size();

*
*
    for(int i = 0; i < tmp_size;i++)
    {
        OrientedPoint&amp; pose(m_particles[i].pose);
        *
        pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);
    }

    *
    **
    *onOdometryUpdate*();
    *
    *
    **
    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 (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;

    *
        **
    if (! m_count
        || m_linearDistance>=m_linearThresholdDistance
        || m_angularDistance>=m_angularThresholdDistance
    || (period_ >= 0.0 &amp;&amp; (reading.getTime() - last_update_time_) > period_))
        {
          last_update_time_ = reading.getTime();

          std::cout <<std::endl<<"Start to Process Scan##################"<<std::endl;

          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;
          }

*
*
*
*

*
*
*
*
*

          *
          **
*
*
*
*
*
*
*
*
          int beam_number = reading.getSize();
          double * plainReading = new double[beam_number];
          for(unsigned int i=0; i<beam_number; i++)
          {
            plainReading[i]=reading.m_dists[i];
          }
          *
          RangeReading* reading_copy;

          *
          if(reading.m_angles.size() == reading.m_dists.size())
          {*
              reading_copy = new RangeReading(beam_number,
                                   &amp;(reading.m_dists[0]),
                                   &amp;(reading.m_angles[0]),
                                   static_cast<const RangeSensor*>(reading.getSensor()),
                                   reading.getTime());

          }
          *
          else
          {
              reading_copy = new RangeReading(beam_number,
                                   &amp;(reading.m_dists[0]),
                                   static_cast<const RangeSensor*>(reading.getSensor()),
                                   reading.getTime());
          }
          **
          if (m_count>0)
          {
            **
            scanMatch(plainReading);

            *
            *onScanmatchUpdate*();

            **
            updateTreeWeights(false);

            **
            std::cerr<<"plainReading:"<<m_beams<<std::endl;
            resample(plainReading, adaptParticles, reading_copy);
          }
          **
          else
          {
            *
            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);
                *

                *
                *
                TNode* node=new        TNode(it->pose, 0., it->node,  0);
                node->reading = reading_copy;
                it->node=node;
            }
         }
          *
         *
         *
         updateTreeWeights(false);
          *

        delete [] plainReading;
        m_lastPartPose=m_odoPose; *

        *
        m_linearDistance=0;
        m_angularDistance=0;

        m_count++;
        processed=true;

          *
        for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
        {
            it->previousPose=it->pose;
        }
    }
    m_readingCount++;
    return processed;
 }

中间有几个比较重要的函数，其余可以参考注释看懂，重要的函数包括一下几个小标题描述。

2.4.1、drawFromMotion()函数

**
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);
}

采样系数的大小即高斯分布的方差。

2.4.2、scanMatch()函数

这个函数在 gridslamprocessor.hxx里面，它的作用是计算激光里程计，原理是模拟蒙特卡洛，即点图匹配：

inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
  *
  **
  double sumScore=0;
  int particle_number = m_particles.size();

  *
  *

*
  for (int i = 0; i < particle_number;i++)
  {
    OrientedPoint corrected;
    double score, l, s;

    **
    score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);

    **
    if (score>m_minimumScore)
    {
      m_particles[i].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, m_particles[i].map, m_particles[i].pose, plainReading);

    sumScore+=score;
    m_particles[i].weight+=l;
    m_particles[i].weightSum+=l;

    *
    *
    **
    m_matcher.invalidateActiveArea();
    m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);
  }
  if (m_infoStream)
    m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;
}

2.4.2.1、ScanMatcher::optimize()函数的原理

这个函数也是帧间匹配的核心函数，它的作用是计算最合适的位姿并返回其得分。向八个方向模拟移动粒子位姿，然后在八个移动位姿后的地方进行地图匹配计算找到最优的位姿。

** *zq* *commit*
*@desc*        *根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来*
*这个函数是真正被调用来进行scan-match的函数*
*@param*        *pnew*                *新的最优位置*
*@param*  *map*                        *地图*
*@param*        *init*                *初始位置*
*@param*  *readings*        *激光数据*
***计算当前位置的得分**所有时的步进增量**精确搜索的次数**搜索的方向*/enum* *Move{Front,* *Back,* *Left,* *Right,* *TurnLeft,* *TurnRight,* *Done};*
    int c_iterations=0;
    do
    {
        **
        if (bestScore>=currentScore)
        {
            refinement++;
            adelta*=.5;
            ldelta*=.5;
        }
        bestScore=currentScore;
        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;
    }while (currentScore>bestScore || refinement<m_optRecursiveIterations);

    **
    pnew=currentPose;
    return bestScore;
}

2.4.2.2、ScanMatcher::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 - 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; xxm_kernelSize; xx++)
            for (int yy=-m_kernelSize; yym_kernelSize; yy++)
            {
                **
                IntPoint pr=iphit+IntPoint(xx,yy);
                IntPoint pf=pr+ipfree;

                **
                const PointAccumulator&amp; cell=map.cell(pr);
                const PointAccumulator&amp; fcell=map.cell(pf);
                **
                if (((double)cell )> m_fullnessThreshold &amp;&amp; ((double)fcell )<m_fullnessThreshold)
                {
                    **
                    Point mu=phit-cell.mean();
                    if (!found)
                    {
                        bestMu=mu;
                        found=true;
                    }
                    else
                    {
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
                    }
                }
            }
        ** *sigma))* *这里的sigma表示方差* *不是标准差*/只在周围的9个栅格里面寻找，因此平方的意义不大。*
            *
        }
    }
        return s;
}

2.4.3、 onScanmatchUpdate ()函数

void GridSlamProcessorThread::*onScanmatchUpdate*(){
        pthread_mutex_lock(&hp_mutex);
        hypotheses.clear();
        weightSums.clear();
        unsigned int bestIdx=0;
        double bestWeight=-1e1000;
        unsigned int idx=0;
        for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
                hypotheses.push_back(part->pose);
                weightSums.push_back(part->weightSum);
                if(part->weightSum>bestWeight){
                        bestIdx=idx;
                        bestWeight=part->weightSum;
                }
                idx++;
        }

        ParticleMoveEvent* event=new ParticleMoveEvent;
        event->scanmatched=true;
        event->hypotheses=hypotheses;
        event->weightSums=weightSums;
        event->neff=m_neff;
        addEvent(event);

        if (! mapTimer){
                MapEvent* event=new MapEvent;
                event->index=bestIdx;
                event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
                event->pose=getParticles()[bestIdx].pose;
                addEvent(event);
        }

        mapTimer++;
        mapTimer=mapTimer%mapUpdateTime;

        pthread_mutex_unlock(&hp_mutex);

        *syncOdometryUpdate*();
}

这个函数的作用是初始化全部粒子的权值。

2.4.4、updateTreeWeights()函数

void  GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{

  if (!weightsAlreadyNormalized)
  {
    normalize();归一化权值
  }
  resetTree();
  propagateWeights();
}

2.4.4.1、resetTree()函数

**
void GridSlamProcessor::resetTree()
{
  *

        for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
        {
                TNode* n=it->node;
                while (n)
                {
                        n->accWeight=0;
                        n->visitCounter=0;
                        n=n->parent;
                }
        }
}

2.4.4.2、normalize()函数

**
inline void GridSlamProcessor::normalize()
{
  *
  double gain=1./(m_obsSigmaGain*m_particles.size());

  **
  double lmax= -std::numeric_limits<double>::max();
  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
  {
    lmax=it->weight>lmax?it->weight:lmax;
  }
  *

  **
  m_weights.clear();
  double wcum=0;
  m_neff=0;
  for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
  {
    m_weights.push_back(exp(gain*(it->weight-lmax)));
    wcum+=m_weights.back();
    *
  }

  **
  m_neff=0;
  for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++)
  {
    *it=*it/wcum;
    double w=*it;
    m_neff+=w*w;
  }
  m_neff=1./m_neff;*

}

2.4.5、resample函数

**

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);
  }
  **

  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();
    *
    *
    ParticleVector temp;
    unsigned int j=0;
  *
    std::vector<unsigned int> deletedParticles;                  *
    *
    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 &amp; 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=reading;

    *

      temp.push_back(p);

      temp.back().node=node;

      temp.back().previousIndex=m_indexes[i];

    }

    while(j<m_indexes.size())

  {

      deletedParticles.push_back(j);

      j++;

    }

  *

    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;

    std::cerr << "Deleting old particles..." ;

    std::cerr << "Done" << std::endl;

    *

    m_particles.clear();

    *

    std::cerr << "Copying Particles and  Registering  scans...";

    *

    *

    *

    int tmp_size = temp.size();

*

    for(int i = 0; i<tmp_size;i++)

    {

        *

        *

        temp[i].setWeight(0);

        *

        *

        m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);

        *

    }

    *

    for(int i = 0; i< tmp_size;i++)

        m_particles.push_back(temp[i]);

    std::cerr  << " Done" <<std::endl;

    hasResampled = true;

  }

  **

  else

  {

  *

    *

    int particle_size = m_particles.size();

*

    for(int i = 0; i < particle_size;i++)

    {

        *

        TNode* node = 0;

        node = new TNode(m_particles[i].pose,0.0,oldGeneration[i],0);

        *

        node->reading = reading;

        m_particles[i].node = node;

        *

        m_matcher.invalidateActiveArea();

        m_matcher.registerScan(m_particles[i].map, m_particles[i].pose, plainReading);

        m_particles[i].previousIndex = i;

    }

    std::cerr<<std::endl;

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

*

    std::cerr  << "Done" <<std::endl;

  }

  *

  return hasResampled;

}

重采样原理操作，使用轮盘赌对权值进行选择。

2.5、占据栅格建图原理

由图可见，当雷达打到障碍物，会将障碍物点设置为hit点，中间的部分设置为miss点。每当被激光打到，则

我们假设lofree为-0.4，looccu为0.8，则每当被打到则更新一次栅格概率，概率大于一定值时，则涂黑，中间部分用b氏算法画线。
代码：

void OccupanyMapping(std::vector<GeneralLaserScan>&amp; scans,std::vector<Eigen::Vector3d>&amp; robot_poses)
{
  std::cout <<"Scans Size:"<<scans.size()<<std::endl;
  std::cout <<"Poses Size:"<<robot_poses.size()<<std::endl;

  for(int i = 0; i < scans.size();i++)
  {

    GeneralLaserScan scan = scans[i];
    Eigen::Vector3d robotPose = robot_poses[i];

    GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));

    if(isValidGridIndex(robotIndex) == false) continue;

    for(int id = 0; id < scan.range_readings.size();id++)
    {

      double dist = scan.range_readings[id];
      double angle = scan.angle_readings[id];

      if(std::isinf(dist) || std::isnan(dist)) continue;

      double theta = robotPose(2);

      double laser_x =   dist * cos(theta + angle);
      double laser_y =  -dist * sin(theta + angle);

      double world_x = laser_x + robotPose(0);
      double world_y = laser_y + robotPose(1);

      GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);

      if(isValidGridIndex(mapIndex) == false)continue;

      std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);

      for(int k = 0; k < freeIndex.size();k++)
      {
        GridIndex tmpIndex = freeIndex[k];

        int linearIndex = GridIndexToLinearIndex(tmpIndex);

        int data = pMap[linearIndex];

        if(data > 0)
          data += mapParams.log_free;
        else
          data = 0;

        pMap[linearIndex] = data;
      }

      int tmpIndex = GridIndexToLinearIndex(mapIndex);
      int data = pMap[tmpIndex];

      if(data < 100)
        data += mapParams.log_occ;
      else
        data = 100;

      pMap[tmpIndex] = data;

    }

  }

}

第3章 传感器测量模型

3.1、测量仪的波术模型

测距仪是时下最流行的机器人传感器。因此本章第一个测量模型就是测距仪的近似物理模型。测距仪测量到附近物体的距离。可以沿着一个波束测量距离，或者可以在一个测量锥内测量距离。

3.1.1、基本测量算法

这里的模型采用四类测量误差,所有这些对使模型工作很重要。这四类误差包括小的测量噪声、意外对象引起的误差,以及由于未检测到对象引起的误差和随机庖外噪声。因此,期望模型 p(zIx，m) 是四个密度的混合,每一种密度都与一个特定类型的误差有关。

1.算法解析

在理想的测量模型中，测量仪测量的物体总是位于离测量仪最近的物体上，但是因为一些误差因素，往往结果会与实际误差有一定的区别。实际上，测量仪的测量结果符合一个高斯模型(人造传感器多为此模型)，均值为u，方差为o，u为最近物体的距离。

意外对象：移动机器人的环境是动态的,而地图 m 是静态的。因此,地图中不包含的对象可能引起测距仪产生惊人的短距离——至少与地图比较时。典型的移动对象就是与机器人共享操作空间的人。处理这类对象的一种方法是将它们作为状态向量的一部分来对待并估计它们的位置;另一种更简单的方法是将它们作为传感器噪声来处理。作为传感器噪声来处理,未建模对象具有这样的特性,即它们会导致比y原来更短而不是更长的距离。检测到意外对象的可能性随着距离而减少。想象有两个人,他们相互独立且在距离传感器的感知领域内出现的可能性是固定而且相同的。第一个人的距离是r1 第二个人的距离是 r2 。不失一般性的,进一步假定 rl < r2 。则更可能测量到r1 而不是 r2 。每当第一个人出现,传感器就会测量到 Tl 。而传感器要测量到r2则必须是在第二个人出现且第一个人不在的时候。这里我们常常用指数分布来模拟噪声模型。

检测失败。有时环境中障碍会被完全忽略。例如,在声呐传感器遇到了镜面反射时,会经常发生此情况。当用激光测距仪检测到黑色吸光的对象时,或者某些激光系统在明媚的阳光下测量物体时,也会发生检测失败。传感器检测失败 (sensor failure) 的典型结果是最大距离测量间题:传感器返回它的最大允许值 Zmax 。由于这样的事件经常发生,那么在测量模型中明确地建立最大测量范围的模型就很必要。这个噪声模型我们常常用一个均匀分布来表示。

随机测量。最后,测距仪偶尔会产生完全无法解释的测量。例如,当超声波被几面墙反弹或者它们受到不同传感器之间的串扰时,声呐常产生幻读。为了使之简单化,对千这样的测量,这里将使用一个分布在完整传感器测量范围[ 0; ZmaxJ 的均匀分布来建立模型:

以上四种模型用以下图来表示：

我们常常用到的测量算法即：

; 3.2、似然域模型

现在介绍另外一种模型,称为似然域，该模型缺乏一个合理的物理解释。事实上,它是一种”特设 (ad hoc)”算法,不必计算相对于任何有意义的传感器物理生成模型的条件概率。而且,这种方法在实践中运行效果良好。即使在混乱的空间,得到的后验也更光滑,同时计算更高效。主要思想就是首先将传感器扫描的终点Zhit, 映射到地图的全局坐标空间。这样做,必须了解相对于全局坐标系,机器人的局部坐标系位于何处?机器入传感器光束 Zk 起源千何处?传感器指向何处?照常,令 xt=(x,y,西塔)在时刻 t 的位姿。保持环境二维图,用 (xksense ,yksense)表示机器人Yksense表示与机器人固连的传感器的坐标位置。

当激光束发射出去，我们将重点Zhit以及传感器以及小车全部映射到地图上，这些坐标只有当传感器检测到一个障碍物时才是有意义的。如果测距传感器输出了最大值 Zt=Zmax 则这些坐标在物理世界没有任何意义(即使测量的确携带了信息)。似然域测量模型简单地将最大距离读数丢弃。因为只有测量到障碍物才可以有效的与地图进行障碍物匹配。Gmapping中的scanmather就是用的这个思路。


图中就是似然域的示意图。图中越亮的地方即为最有可能成为障碍物出现的地方。
测量模型与波束模型类似，式中,使用熟悉的混合权值 Zt、Zrand和Zmax。图 6.9b给出了一个测量波束与其结果分布 p(z| x, m) 的例子。很容易看到这个分布结合了图 6. 9a所示的 p,以及分布Pmax和 Prand。这里所说的调节混合参数大多转移到新模型上。它们可以通过手动调节或者利用极大似然估计得到。图 6.8b所示的表示法,将障碍物检测的似然描述成全局 x-y 坐标的函数,叫作似然域。
似然域算法为：

这个算法即为GMAPPING中score函数的原理。

第4章 launch文件

4.1、文件内容：

<launch>

  <param name="use_sim_time" value="true"/>
  <arg name="scan_topic" default="sick_scan" />

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <remap from="scan" to="$(arg scan_topic)"/>

    *<!--*  *GMapping* *Wrapper参数* *-->*
    <param name="throttle_scans" value="1"/>
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="odom"/>--
    <param name="map_update_interval" value="5.0"/>                   *<!--*  *地图更新速率*  *-->*

*<!--* *Set* *maxUrange* *<* *actual* *maximum* *range* *of* *the* *Laser* *maxRange* *-->*
    *<!--* *maxUrange* *<* *actual-->*
    *<!--* *Laser* *Parameters(雷达相关参数)* *-->*
    <param name="maxRange" value="5.51" />                             *<!--* *激光最大范围* *-->*
    <param name="maxUrange" value="5.5"/>                             *<!--* *激光最大使用范围* *maxUrange* *<* *真实范围* *<* *maxRange-->*
    <param name="sigma" value="0.05"/>                                *<!--* *scan* *matching过程中的标准差(cell)* *-->*
    <param name="kernelSize" value="1"/>                              *<!--* *scan* *matching过程中的搜索窗口大小* *-->*
    <param name="lstep" value="0.05"/>                                *<!--* *scan* *matching过程中的位置增量* *-->*
    <param name="astep" value="0.05"/>                                *<!--* *scan* *matching过程中的角度增量* *-->*
    <param name="iterations" value="5"/>                              *<!--* *scan* *matching过程中的迭代优化次数* *-->*
    <param name="lsigma" value="0.075"/>                              *<!--* *scan* *matching过程中的计算似然的标准差(single* *laser* *beam)* *-->*
    <param name="ogain" value="3.0"/>                                 *<!--* *平滑似然的增益* *-->*
    <param name="lskip" value="0"/>                                   *<!--* *取每第(n+1)个激光束来计算匹配(0表示取所有的激光束)* *-->*
    <param name="minimumScore" value="50"/>                           *<!--* *scan* *matching被接受的最小阈值(不被接受，则使用里程计数据)* *-->*

    *<!--* *小车运动模型参数* *-->*
    <param name="srr" value="0.01"/>                                  *<!--* *由平移造成的平移误差-->*
    <param name="srt" value="0.005"/>                                  *<!--* *由平移造成的角度误差-->*
    <param name="str" value="0.01"/>                                          *<!--* *由旋转造成的平移误差* *-->*
    <param name="stt" value="0.005"/>                                  *<!--* *由旋转造成的角度误差* *-->*

    *<!--* *其他参数* *-->*
    <param name="linearUpdate" value="0.2"/>                            *<!--* *小车每走过0.2m,处理一次激光数据* *-->*
    <param name="angularUpdate" value="0.2"/>                           *<!--* *小车旋转0.2* *

第五章 参考帮助理解博客

博客内容：
https://blog.csdn.net/zhao_ke_xue/article/details/110702322

第六章 gmapping使用

近期为了更加熟练使用gmapping，开始做实验，但是做实验的路上，每一步都非常艰难，则在此写下gmapping使用以及研究过程中的各项问题，防止忘记。
因为小车电量有限，参数量庞大，且背电脑不方便，于是采取录bag包调试算法的方法研究。

6.1、bag包的录取

播放官方bag包时发现

我们主要需要的几个话题，一个是/scan以及/tf，/odom可以在算法中计算，也可以使用imu以及其他方法得出，可以不录取。/clock是包的时钟，会自动录取。
故录取bag包时我们需要录取到雷达数据以及tf变换的过程包。

; 6.1.1、这里在tf变换上写点心得

因为录取后用网上的方法播放建图时候出现了很多问题，我将问题全部总结下来。

出现这个问题时候先分析

rosrun rqt_tf_tree rqt_tf_tree

看tf树是否对齐，然后在分析如何更改launch文件中的tf包，

<launch>

  <param name="use_sim_time" value="true"/>

  <!-- we need a static tf transform from base to laser -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_link" args="0 0 0 0 0 0 base_footprint base_link 100" />
     <node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.15 0 0.05 0 0 1.0 6.12323399574e-17 base_footprint laser 100" />

  <!-- Gmapping -->
  <include file="/home/ningyang//buildingpic/src/gmapping/launch/slam_gmapping_pr2.launch"/>
</launch>

对于任意一个tf变换来看，第一个是包的名字，第二个是变换类型，第三个是发布tf变换的节点名称，arg里面前三个是平移xyz变换，后面几个数字是旋转变换(三个是欧拉角，四个是四元数)，后面两个是坐标系名称和子坐标系名称，最后一个是周期数，一般是100
这里出现上述问题的原因是bag包里面已经录了tf变换，比如base_link到base_footprint，但是launch里面又写了类似tf变换，launch文件中不需要对应坐标系变换。直接启用slam_gmapping_pr2.launch就好。只有当录取数据时没有用到的tf变换才需要在launch文件中加上。
还需要注意的问题，下面参数要改到对应bag包里面发布的tf变换

    <remap from="scan" to="$(arg scan_topic)"/>

    *<!--*  *GMapping* *Wrapper参数* *-->*
    <param name="throttle_scans" value="1"/>
    <param name="base_frame" value="base_link" />
    <param name="odom_frame" value="odom"/>--

由上面tf数可以看出odom坐标系用的odom_combined，base_frame可以是base_link,也可以是base_footprint。
从下面图片可以看出各个tf的发布者
![](https://teakki.top/file/image/5feefe254c8fec461fe14f68)
和gmapping有关的就是map到odom_combined，所以gmapping文件中要写odom_combined。

如果出现以下

[ WARN] [1584192934.752228918]: MessageFilter [target=odom ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_notifier] rosconsole logger to DEBUG for more information.

同样是tf树没对齐的原因

6.1.2、地图保存

rosrun map_server map_saver -f {保存地址以及名称}
rosrun map_server map_server /home/ningyang/xxx.yaml

6.2、实验就绪

有包有代码就可以开始实验啦。
我们先试着改变粒子数然后看不同粒子数对建图的影响，在本场景中数量在200时效果最好
粒子数设置为8:
建图效果:
实验发现粒子数越大效果先上升后下降
地图更新速率:
地图更新速率同样先上升后下降，在本场景中数量在0.5时效果最好

Original: https://blog.csdn.net/qq_42429516/article/details/123760171
Author: 高颜值的叮当猫
Title: Gmapping的个人理解