gmapping源码解读

目前已经有很多博客分享了gmapping的源码分析，之前我也看过几遍，不过一直没有记录，在此记录一下。

从应用gmapping包来讲，ROS中的slam_gmapping包也是调用了openslam_gmapping开源算法，gmapping的代码分为两部分：

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

https://github.com/ros-perception/slam_gmapping

一、先看数据结构：

（1）粒子的数据结构：

 /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/
    struct Particle{
      /**constructs a particle, given a map
	 @param map: the particle map
      */
      Particle(const ScanMatcherMap& map);

      /** @returns the weight of a particle */
      inline operator double() const {return weight;}
      /** @returns the pose of a particle */
      inline operator OrientedPoint() const {return pose;}
      /** sets the weight of a particle
	  @param w the weight
      */
      inline void setWeight(double w) {weight=w;}
      /** The map */
      ScanMatcherMap map;
      /** The pose of the robot */
      OrientedPoint pose;

      /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */
      OrientedPoint previousPose;

      /** The weight of the particle */
      double weight;

      /** The cumulative weight of the particle */
      double weightSum;

      double gweight;

      /** The index of the previous particle in the trajectory tree */
      int previousIndex;

      /** Entry to the trajectory tree */
      TNode* node; 
    };

    typedef std::vector<Particle> ParticleVector;
    ...
    ...
    inline const ParticleVector& getParticles() const {return m_particles; }
    ...
    ...
    /**the particles*/
    ParticleVector m_particles;

 

（2）激光lidar获取的数据格式：

        RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);

        bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles)

        //这个用来准备激光lidar数据为openslam_gmapping能处理的格式
        RangeReading* reading_copy = new RangeReading(reading.size(),
                                                                                              &(reading[0]),
                                                                                              static_cast<const RangeSensor*>(reading.getSensor()),
                                                                                              reading.getTime());

class RangeSensor: public Sensor{
	friend class Configuration;
	friend class CarmenConfiguration;
	friend class CarmenWrapper;
	public:
		struct Beam{
			OrientedPoint pose;	//pose relative to the center of the sensor
			double span;	//spam=0 indicates a line-like beam
			double maxRange;	//maximum range of the sensor
			double s,c;		//sinus and cosinus of the beam (optimization);
		};	
		RangeSensor(std::string name);
		RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0);
		inline const std::vector<Beam>& beams() const {return m_beams;}
		inline std::vector<Beam>& beams() {return m_beams;}
		inline OrientedPoint getPose() const {return m_pose;}
		void updateBeamsLookup();
		bool newFormat;
	protected:
		OrientedPoint m_pose;
		std::vector<Beam> m_beams;
};

 

二、函数入口：

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

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

  return(0);
}

startLiveSlam()中，

（1）先订阅激光雷达的数据：  scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);

（2）通过boost::bind开启两个线程：laserCallback()和 publishLoop()，

在核心线程laserCallback中 ：

laserCallback()
    ——>initMapper()  //进行参数初始化
                    //gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_);
                    //g