Gmapping源码阅读(4)————添加激光雷达数据

C_link
392 阅读3分钟

一、绪论

添加激光雷达数据addScan()是Gmaping中一个比较重要的函数。 该函数包含三个主要函数:

  • getOdomPose
  • setPose
  • processScan

我们首先简单介绍addScan()这个函数的流程，后面再对其包含的几个函数进行逐个讲解。

二、addScan

首先我们看看该函数:

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];
    }
  }
  
  //执行RangeReading()，读取点云大小、点云扫描值、gsp_laser_(RangeSensor()返回值）、单位为秒的时间戳到reading，并设置reading的位姿；
  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);
}

输入: scan

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

函数作用: 获取gmap_pose和scan信息先处理scan信息，放到range_double里头，然后将gmap_pose和ranges_double一起放进reading里一起处理。

具体流程:

  1. 确保获取到上一次里程计的位姿、时间戳以及相同数量的激光扫描点；
  2. 判断是否需将激光扫描逆向反转；
  3. 执行RangeReading()，读取点云大小、点云扫描值、gsp_laser_(RangeSensor()返回值）、单位为秒的时间戳到reading，并设置reading的位姿；
  4. 执行processScan(reading)。

三、 getOdomPose()

该函数在前面提到过，主要就是获取对应激光雷达时间帧的激光雷达数据，然后进行坐标变换到对应的坐标系，最后输出数据。

四、 GMapping::RangeReading

该类的作用就是读取激光雷达的数据。

  GMapping::RangeReading reading(scan.ranges.size(),
                                 ranges_double,
                                 gsp_laser_,
                                 scan.header.stamp.toSec());

这一句主要的操作就是执行RangeReading()，读取点云大小、点云扫描值、gsp_laser_(RangeSensor()返回值）、单位为秒的时间戳到reading，并设置reading的位姿；

之后我们详细了解一下这个类:

class RangeReading: public SensorReading, public std::vector<double>{
	public:
		RangeReading(const RangeSensor* rs, double time=0);
		RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0);
		virtual ~RangeReading();
		inline const OrientedPoint& getPose() const {return m_pose;}
		inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
		unsigned int rawView(double* v, double density=0.) const;
		std::vector<Point> cartesianForm(double maxRange=1e6) const;
		unsigned int activeBeams(double density=0.) const;
	protected:
		OrientedPoint m_pose;
};

该类定义了两个构造函数，一个虚析构函数，一个获取位姿m_pose的函数getPose(),一个设置位姿的函数setPose(),rawView()暂时不了解它的作用，cartesianForm()这个函数将极坐标的数据转换到笛卡尔坐标系下,activeBeam()用于计算给定过滤参数下激活状态的扫描束。然后该函数的主要变量就是存储位姿的m_pose这里的位姿是传感器的位置信息。

RangeReading这个类它继承了两个类。其中std::vector是C++标准库中定义的一种线性表实现，在这里它就是用于存储距离数据的列表。 

然后我们再详细的看一下每个函数的作用。

  • 首先是两个构造函数，它们有两个相同的参数rs和time,分别描述了传感器对象和产生数据的时间，用于初始化父类SensorReading。第二个构造函数的参数相对多一点，通过该函数，我们可以指定一组扫描数据来构建RangeReading对象。
RangeReading::RangeReading(const RangeSensor* rs, double time):
	SensorReading(rs,time){}
        
        
RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time):
	SensorReading(rs,time){
	assert(n_beams==rs->beams().size());
	resize(n_beams);
	for (unsigned int i=0; i<size(); i++)
		(*this)[i]=d[i];
}
  • RangeReading有一个protected权限的成员变量m_pose，该变量记录了获取激光传感器扫描数据时的传感器的位置信息。对应的，还有一对public权限的get-set函数，用于获取和设定位置信息。
public:
    inline const OrientedPoint& getPose() const {return m_pose;}
    inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
protected:
    OrientedPoint m_pose;
  • 我们可以通过函数rawView将扫描数据拷贝到一个数组中。它有两个参数，其中v是保存数据的数组。density是一个过滤参数，如果临近的几个扫描数据所探测的点距离小于该参数， 就赋予一个很大的值，此时的扫描束被称为是抑制的(suppressed)。那些非抑制的扫描束称为激活的(actived)。缺省的情况下，density为0.0，过滤参数不起作用。 相应的还有函数activeBeams，用于计算给定过滤参数下激活状态的扫描束。
unsigned int rawView(double* v, double density=0.) const;
unsigned int activeBeams(double density=0.) const;
  • 一般情况下，激光扫描数据都是用极坐标的形式描述的，使用到圆点距离以及相对于极轴的偏转角来确定空间中的一个点。这种方式虽然能够很直观的表述激光传感器的扫描过程， 但是并不利于计算，很多时候还需要将这些数据转换到笛卡尔坐标系下。RangeReading也提供了这一坐标变换的接口，它有一个参数maxRange用于限定测量距离的最大值。
std::vector<Point> cartesianForm(double maxRange=1e6) const;

五、reading.setPose(gmap_pose);

使用对象reading调用RangeReading中的函数setPose()设置位姿。该位姿是里程计位姿，应该是先获取里程计位姿作为估计后面再优化。

reading.setPose(gmap_pose);

至此本章的内容就结束了，但是AddScan()这个函数还没结束，因为最重要processScan()函数还没解析。该函数主要就是对激光数据进行处理。就是因为重要，因此我们决定在新的一章来分析它。

avatar
文章
阅读
粉丝