gmapping原理及代码解析（一）

此次记录Gmapping学习的过程，笔者能力尚缺，欢迎大家一起交流啊～

一、gmapping代码解析

我们先来看看Gmapping里的main.cpp

#include <ros/ros.h>

#include "slam_gmapping.h"

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

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

  return(0);
}

里面涉及到startLiveSlam()函数，转到此函数，此函数在slam_gmapping.h中定义，在slam_gmapping.cpp中实现：

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

可以看出，此函数注册了相关的回调函数和发布者，我们进入雷达数据的回调函数SlamGMapping::laserCallback，也是在slam_gmapping.cpp中：

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

  //第一帧激光数据时不初始化mapper
  // We can't initialize the mapper until we've got the first scan
  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");
}

在此函数中，突出重要的是addscan()函数,这个函数需要scan（激光数据）和odom_pose（里程计位姿）两个参数：

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()];   //dynamic array
  // 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  ?过滤掉短读数,i he i-1这样关系是因为雷达reverse
      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());

//但是它在rangereading构造函数中深度复制它们，因此我们不需要保留数组。
  // ...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);   //inline void setPose(const OrientedPoint& pose) {m_pose=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);
}

addscan()函数只是将获取到的scan消息作一下处理，过滤掉无效值，将处理过的数据传入processScan()函数,这个函数如果在ros上安装了gmapping包，可以在ros的安装路径下找到，如笔者的就在/opt/ros/kinetic/include/gmapping/gridfastslam中找到此函数的相关的头文件，或者可以之间去openslam下载源代码。;

 bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)
  {
    /**retireve the position from the reading, and compute the odometry*/
    /*得到当前的里程计的位置*/
	OrientedPoint relPose=reading.getPose();
    //relPose.y = m_odoPose.y;
    
	/*m_count表示这个函数被调用的次数 如果是第0次调用,则所有的位姿都是一样的*/
	if (!m_count)
	{
      m_lastPartPose=m_odoPose=relPose;
    }
    
    //write the state of the reading and update all the particles using the motion model
	/*对于每一个粒子，都从里程计运动模型中采样，得到车子的初步估计位置  这一步对应于   里程计的更新 */
    int tmp_size = m_particles.size();

//这个for循环显然可以用OpenMP进行并行化
//#pragma omp parallel for
    for(int i = 0; i < tmp_size;i++)
    {
        OrientedPoint& pose(m_particles[i].pose);
        pose = m_motionModel.drawFromMotion(m_particles[i],relPose,m_odoPose);
    }

    //invoke the callback
    /*回调函数  实际上什么都没做*/
    onOdometryUpdate();
    
    // accumulate the robot translation and rotation
    /*根据两次里程计的数据 计算出来机器人的线性位移和角度位移的累积值 m_odoPose表示上一次的里程计位姿  relPose表示新的里程计的位姿*/
    OrientedPoint move=relPose-m_odoPose;
    move.theta=atan2(sin(move.theta), cos(move.theta));

    //统计机器人在进行激光雷达更新之前 走了多远的距离 以及　平移了多少的角度
    m_linearDistance+=sqrt(move*move);
    m_angularDistance+=fabs(move.theta);

//    cerr <<"linear Distance:"<<m_linearDistance<<endl;
//    cerr <<"angular Distance:"<<m_angularDistance<<endl;
    
    // if the robot jumps throw a warning
    /*
     * 如果机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新
     * 则需要进行报警。这个误差很可能是里程计或者激光雷达的BUG造成的。
     * 例如里程计数据出错 或者 激光雷达很久没有数据等等
     * 每次进行激光雷达的更新之后 m_linearDistance这个参数就会清零
     */
    if (m_linearDistance>m_distanceThresholdCheck)
    {
      cerr << "***********************************************************************" << endl;
      cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
      cer