GMapping的传感器

GMapping以激光扫描的距离数据和里程计作为输入，所以ROS的demo才会费半天劲，把双目的深度图信息转换成激光的扫描数据。在GMapping中用类RangeReading来记录激光扫描数据，使用RangeSensor对激光雷达建模。里程计是SLAM技术中常用的运动模型，类OdometryReading用于记录里程计数据，OdometrySensor则对里程计建模。

1. 传感器基类

在GMapping中有一个目录sensor，其中放置了关于传感器的代码。下面分别是源文件和头文件下的sensor目录树，有三个子目录。其中sensor_base中实现了传感器数据和传感器模型的基类SensorReading和Sensor。激光扫描数据、激光传感器、里程计数据、里程计都继承自这两个基类。

        $ roscd openslam_gmapping
        $ tree sensor
        sensor
        ├── CMakeLists.txt
        ├── Makefile
        ├── sensor_base
        │   ├── CMakeLists.txt
        │   ├── sensor.cpp
        │   └── sensorreading.cpp
        ├── sensor_odometry
        │   ├── CMakeLists.txt
        │   ├── odometryreading.cpp
        │   └── odometrysensor.cpp
        └── sensor_range
            ├── CMakeLists.txt
            ├── rangereading.cpp
            └── rangesensor.cpp

        $ roscd openslam_gmapping/include/gmapping/
        $ tree sensor/
        sensor/
        ├── sensor_base
        │   ├── sensor.h
        │   └── sensorreading.h
        ├── sensor_odometry
        │   ├── odometryreading.h
        │   └── odometrysensor.h
        └── sensor_range
            ├── rangereading.h
            └── rangesensor.h

下面左边代码是类SensorReading的定义，并没有什么过多的内容，无非是定义了两个成员变量m_time和m_sensor分别记录了数据产生时间和传感器对象。再就是一些get-set函数用于获取和设定这两个变量。右边的代码则是类Sensor，除了描述传感器的名称之外，没有任何其他作用。

        class SensorReading {
            public:
                SensorReading(const Sensor* s=0, double time=0);
                virtual ~SensorReading();
                inline double getTime() const;
                inline void setTime(double t);
                inline const Sensor* getSensor() const;
            protected:
                double m_time;
                const Sensor* m_sensor;
        };

        class Sensor{
            public:
                Sensor(const std::string& name="");
                virtual ~Sensor();
                inline std::string getName() const;
                inline void setName(const std::string& name);
            protected:
                std::string m_name;
        };

2. 激光雷达

在文件rangereading.h中声明了RangeReading，它继承了两个类。其中std::vector<double>是C++标准库中定义的一种线性表实现，在这里它就是用于存储距离数据的列表。 SensorReading则是GMapping中传感器的基类。

        class RangeReading: public SensorReading, public std::vector<double>

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，用于计算给定过滤参数下激活状态的扫描束。

        public:
            unsigned int rawView(double* v, double density=0.) const;
            unsigned int activeBeams(double density=0.) const;

一般情况下，激光扫描数据都是用极坐标的形式描述的，使用到圆点距离以及相对于极轴的偏转角来确定空间中的一个点。这种方式虽然能够很直观的表述激光传感器的扫描过程，但是并不利于计算，很多时候还需要将这些数据转换到笛卡尔坐标系下。RangeReading也提供了这一坐标变换的接口，它有一个参数maxRange用于限定测量距离的最大值。

        public:
            std::vector<Point> cartesianForm(double maxRange=1e6) const;

下面是两个构造函数，它们有两个相同的参数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];
        }

RangeSensor是激光传感器的封装，它描述了扫描光束的的各种物理特性，继承自类Sensor。

        class RangeSensor: public Sensor

在RangeSensor内部嵌套定义了一个结构体Beam，描述了扫描光束的物理特性，包括相对于传感器坐标系的位置、量程、光束角的正余弦值。

        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定义了两个成员变量，其中m_pose用于记录传感器的位姿，m_beams则记录了扫描光束的信息。同时提供了set-get函数用于访问这些成员变量。

        protected:
            OrientedPoint m_pose;
            std::vector m_beams;
        public:
            inline const std::vector& beams() const {return m_beams;}
            inline std::vector& beams() {return m_beams;}
            inline OrientedPoint getPose() const {return m_pose;}

此外，还有一个成员函数updateBeamsLookup，用于更新扫描光束属性，其实现如下，只是根据光束的位置和方位角计算了正余弦值。

        void RangeSensor::updateBeamsLookup() {
            for (unsigned int i = 0; i < m_beams.size(); i++){
                RangeSensor::Beam& beam(m_beams[i]);
                beam.s=sin(m_beams[i].pose.theta);
                beam.c=cos(m_beams[i].pose.theta);
            }
        }

3. 里程计

里程计的定义和实现相对简单很多，头文件odometryreading.h中定义了里程计读数的数据结构，它用三个成员变量m_pose, m_speed和m_acceleration记录了机器人的位姿、速度和加速度，并分别提供了set-get函数来访问这些变量。同样的，该类也继承自SensorReading。

        class OdometryReading: public SensorReading{
            public:
                OdometryReading(const OdometrySensor* odo, double time=0);
                inline const OrientedPoint& getPose() const {return m_pose;}
                inline const OrientedPoint& getSpeed() const {return m_speed;}
                inline const OrientedPoint& getAcceleration() const {return m_acceleration;}
                inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
                inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;}
                inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;}
            protected:
                OrientedPoint m_pose;
                OrientedPoint m_speed;
                OrientedPoint m_acceleration;
        };

下面是里程计模型OdometrySensor的定义，只有一个成员变量m_ideal用于标记里程计是否为理想情况下的传感器。

        class OdometrySensor: public Sensor{
            public:
                OdometrySensor(const std::string& name, bool ideal=false);
                inline bool isIdeal() const { return m_ideal; }
            protected:
                bool m_ideal;   
        };

4. 完

为了方便管理传感器以及传感器数据，GMapping将传感器模型和传感器数据分开实现，可以在一定程度上提高系统的可扩展性。并且为传感器定义了两个基类，SensorReading和Sensor，提供一些基础的属性，比如传感器名称、传感器数据产生时间等等。

目前GMapping中只有两种传感器，激光雷达和里程计。分别使用RangeReading-RangeSensor和OdometryReading-OdometrySensor实现，它们都继承自SensorReading和Sensor两个基类。此外，RangeReading还继承自标准库vector，因此我们可以以数组的形式访问激光的扫描数据。