上一讲梳理了ExplorationPlanner文件夹下的planner文件夹下的所有头文件，这一讲我们梳理sensor文件夹下的所有头文件。

ExplorationPlanner

从上面的截图可以看出，ExplorationPlanner分为这几部分:grid, kinematics, planner, sensor, trajectory, utils。

4.sensor部分

1>Bresenham2D.h

看这个头文件的名字，我们猜到使用Bresenham画线算法。

"Bresenham直线算法是用来绘制由两点所决定的直线的算法，它会算出一条线段在n维光栅上最接近的点。这个算法只会用到较为快速的整数加法、减法和位元移位，常用于绘制电脑画面的直线。是计算机图形学中最先发展出来的算法。"--引自wiki：

Bresenham画线算法绘制直线过程:

Bresenham画线算法伪码:

关于Bresenham画线算法原理参考:

Bresenham’s Line Generation Algorithm - GeeksforGeeks​www.geeksforgeeks.org/bresenhams-line-generation-algorithm/

Bresenham画线算法C++代码实现:

void Line( float x1, float y1, float x2, float y2, const Color& color )
{
 // Bresenham's line algorithm
  const bool steep = (fabs(y2 - y1) > fabs(x2 - x1));
  if(steep)
  {
    std::swap(x1, y1);
    std::swap(x2, y2);
  }
 
  if(x1 > x2)
  {
    std::swap(x1, x2);
    std::swap(y1, y2);
  }
 
  const float dx = x2 - x1;
  const float dy = fabs(y2 - y1);
 
  float error = dx / 2.0f;
  const int ystep = (y1 < y2) ? 1 : -1;
  int y = (int)y1;
 
  const int maxX = (int)x2;
 
  for(int x=(int)x1; x<=maxX; x++)
  {
    if(steep)
    {
        SetPixel(y,x, color);
    }
    else
    {
        SetPixel(x,y, color);
    }
 
    error -= dy;
    if(error < 0)
    {
        y += ystep;
        error += dx;
    }
  }
}

2>Raytracer2D.h

照例先看头文件，这里引用grid目录下的PlanarGridIndex.h，这个头文件下的PlanarGridIndex类主要元素是x,y，主要功能是设置x,y,获取x,y以及两个PlanarGridIndex类实例化的对象ma,mb。

#include "ExplorationPlanner/grid/PlanarGridIndex.h"

以及使用boost库的unordered_map容器:

#include <boost/unordered_map.hpp>

关于boost库的unordered_map相关参考:

我们再回到这个Raytracer2D类本身，看它的私有成员,RayTracePt:

private:
    typedef std::pair<int, int> RayTracePt;  

这里用到std::pair，具体语法特性参考:

私有成员 RayTracePtVec,是RayTracePt类型的元素的vector集合:

private:
   typedef std::vector<RayTracePt> RayTracePtVec;

关于std::vector，具体语法特性参考:

私有成员RayTracePtMap,是盛放PlanarGridIndex类类型的元素(x,y),RayTracePtVec类类型元素(RayTracePt类型的元素的vector集合)，再加上boost库的hash函数，其类型输入为PlanarGridIndex类型(x,y),综合这些组成一个boost库unorderd_map容器表示成的一个集合。

private:
  typedef boost::unordered_map<PlanarGridIndex, RayTracePtVec, boost::hash<PlanarGridIndex> > RayTracePtMap;

boost库unorderd_map容器参考:

boost库hash函数参考:

再声明RayTracePtVec的迭代器const_iterator:

private:
   RayTracePtVec::const_iterator ipt_; // points to current pt in raytrace
   RayTracePtVec::const_iterator ipt_end_; // points to end pt of current raytrace

3>PlanarCellObservation.h

在这个头文件里我们专注于分析PlanarCellObservation这个类。

先看与PlanarCellObservation类同名的构造函数:

PlanarCellObservation()
        : o_(false), inverse_sensor_model_(0.0)
    {
    }

PlanarCellObservation(bool o, double ism)
        : o_(o), inverse_sensor_model_(ism)
    {
    }

要想要知道构造函数传入的参数o_和inverse_sensor_model_来自于哪里，我们在类的私有成员里找到:

private:
    bool o_;
    double inverse_sensor_model_;

关于inverse_sensor_model_，直译是反演测量模型：

补课链接,见Ⅷ.占据栅格地图部分:

关于PlanarCellObservation类实现的功能,我们来看它的类内函数,首先是获取observations，然后再返回一个真值，然后是获取反演测量模型的值：

//获取observations
bool getObservation() const {
        return o_;
    }
//获取反演测量模型的值
double getISMValue() const {
        return inverse_sensor_model_;
}

4>PlanarObservation.h

先看引用的头文件，有两个来自grid目录下的PlanarGridContainer.h和PlanarGridIndex.h;

//用于管理整个地图的栅格单元的容器，以及找到栅格单元里xmin, xmax, ymax, ymin
#include "ExplorationPlanner/grid/PlanarGridContainer.h"
//用于管理单个栅格的x,y，以及比较两个栅格是否一样
#include "ExplorationPlanner/grid/PlanarGridIndex.h

还有一个是来自sensor本地目录下的PlanarCellObservation.h;

//用于获取observations，然后再返回一个真值，以及获取反演测量模型的值
#include "ExplorationPlanner/sensor/PlanarCellObservation.h"

另外还用到boost库的unordered_map：

#include <boost/unordered_map.hpp

关于boost库的unoreded_map参考:

接下来我们专注这个PlanarObservation类本身:

class PlanarObservation : virtual public PlanarGridContainer<PlanarCellObservation>
{
};

我们可以看到这个PlanarObservation类继承了包含PlanarCellObservation类型元素的PlanarGridContainer容器。至于PlanarCellObservation类表示单个栅格单元用于获取observations，然后再返回一个真值，以及获取反演测量模型的值。而PlanarGridContainer是用来管理整个地图的栅格单元的容器，以及找到栅格单元里xmin, xmax, ymax, ymin。

综合来看，这个PlanarObservation类就是一个unordered_map容器用来管理整个栅格地图的所有栅格单元获取observations和获取反演测量模型inverse_sensor_model的值。

5>LaserScanner2D.h

看引用的头文件，引用了一大堆，就知道这个LaserScanner2D类不平凡。

首先是来自sensor目录本地的Raytracer2D.h和PlanarObservation.h。

Raytracer2D类里的内容有：关于PlanarGridIndex类型元素(x,y)，PlanarGridIndex类型的hash函数，<int,int>类型的pair:RayTracePtr,再是由RayTracePtr类型组成的vector: RayTracePtVec。

最后是由PlanarGridIndex, RayTracePtVec, boost::hash<PlanarGridIndex> 组成的unordered_map类型的RayTracePtMap容器。这个Raytrace2D类还会返回这个RayTracePtMap的size。总的来说，作用就是管理栅格单元的index。

至于PlanarObservation.h,如上面我们分析过的，也是一个unorderedmap类型的容器，它用来管理整个栅格地图里的所有栅格单元获取observations和获取反演测量模型inverse_sensor_model的值。

#include "Raytracer2D.h"
#include "PlanarObservation.h"

再看到有三个来自grid目录下的头文件,如果看到这些头文件还陌生的话，还是跳转到这个链接补补课比较好:

#include "ExplorationPlanner/grid/PlanarGridOccupancyMap.h"
#include "ExplorationPlanner/grid/PlanarGridBinaryMap.h"
#include "ExplorationPlanner/grid/PlanarGridIndex.h"

PlanarGridIndex.h用来用于管理单个栅格的x,y，以及比较两个栅格是否一样；

PlanarGridBinaryMap.h主要声明继承了能对栅格地图元素进行内容管理以及能在栅格地图中栅格单元遍历的管理工具；另外再加了自己一个独有的功能，由函数dilate()实现。

PlanarGridOccupancyMap.h这里强调的是栅格单元的更新，整个栅格地图的更新，至于更新的影响因子，来源于sensor文件夹下的PlanarCellObservation.h 和PlanarObservation.h，这两个文件夹是由激光传感器模拟产生的observations。由它们影响着栅格单元值的更新、栅格地图的更新。

接下来是来自kinematics目录下的PlanarPose.h，PlanarPose类表示x,y,theta；可以用来表示机器人的位姿或者地图的原点origin。

#include "ExplorationPlanner/kinematics/PlanarPose.h"

紧接着是来自utils目录下的RNG.h,这个头文件表示的是用于生成随机数的类。

#include "ExplorationPlanner/utils/RNG.h"

最后一个比较重要的是boost库里均匀分布uniform_int_distribution的头文件:

#include <boost/random/uniform_int_distribution.hpp>

关于boost库的uniform_int_distribution参考：

关于boost库的随机数生成参考:

这个boost/random/uniform_int_distribution的引用，是用于定义栅格的occupancies和observations的采样分布UniDist。

private: 
// For sampling cell occupancies and observations
 typedef boost::random::uniform_int_distribution<int8_t> UniDist;

好了，重要的引用的头文件分析完了，让我们回到LaserScanner2D类本身。

先看与类同名的构造函数:

LaserScanner2D(double start_angle_rads, double stop_angle_rads,
                   double angle_step_rads, double max_distance_meters,
                   double p_false_pos, double p_false_neg);

这部分的参数从类的私有成员定义里可以看到:

private:
    double starting_angle_rads_;
    double stopping_angle_rads_;
    double angle_step_rads_;
    double max_distance_meters_;
    
    int8_t prob_false_positive_;
    int8_t prob_false_negative_;
    
    // store incidence angles
    std::vector<double> av_

而这些参数的相关解释，均与激光有关，激光的incidence angle,激光的起始角度，末端角度，两束激光射线之间的角度间隔，激光可测量到的最大距离，以及因为激光传感器有测量误差，我们假定两个概率:

laser_p_false_pos:probability of false positive (假阳性)reading for the laser(free cell observed as occupied：空闲的栅格被视作占据的)；

laser_p_false_neg:probability of false negative（假阴性） reading for the laser(occupied cell observed as free:占据的栅格被视作空闲的)；

如果还没弄明白，让我们回到这个链接补课，见->2.关键参数说明，包括cfg文件夹下的参数:Simulated laser scanner parameters

以及与反演测量模型inverse sensor model有关的:

// Alternative representation via log odds for inverse sensor model
    double ism_free_;
    double ism_occupied_;

关于反演测量模型inverse sensor model，这里再强调一遍其算法伪码:

我们可以看到这个反演测量模型的输出是返回  或者  。这也对应了这个LaserScanner2D类里的私有成员:ism_free_(double型)，ism_occupied_(double型)。

以及还有概率转对数的函数:

private:
 static double prob2lo(int8_t p);

为啥好好的概率要转对数呢?看下面占据栅格地图算法的实现:

现在我们来看测量模型measurement model。在LaserScanner2D类里这样实现:

单个栅格单元的测量采样功能实现，返回类型为bool型；

private:
 bool sampleCellMeasurement(const PlanarGridIndex& idx,
                               const PlanarGridOccupancyMap& map_sample,
                               const PlanarGridOccupancyMap &map_prior,
                               const PlanarGridBinaryMap& map_ground_truth,
                               RNG &rng) const; 

整个栅格地图的栅格单元的测量采样功能实现，返回类型是PlanarObservation类型；与上面单个栅格元的测量采样功能不一样的是传入的PlanarGridIndex类型的index(x,y)，改成了PlanarPose类型的pose(x,y,theta)。

public:
 PlanarObservation sampleMeasurement(const PlanarPose& pose,
                                        const PlanarGridOccupancyMap& map_sample,
                                        const PlanarGridOccupancyMap &map_prior,
                                        const PlanarGridBinaryMap& map_ground_truth,
                                        RNG &rng);

两个测量模型它们的共同输入是PlanarGridOccupancyMap类型的map_sample(map采样), map_prior(map先验)，PlanarGridBinaryMap类型的map_ground_truth(map真值)。

当然咯，声明一个Raytracer2D类型的raytracer_是必须的，这个表示激光射线的索引index以及对应index下的内容管理。

小结:

这个LaserScanner2D类实例化后的对象可以说是我们真正构造的激光传感器了，我们使用这个传感器产生的激光来构建栅格地图。具体使用的算法有Bresenham画线算法，反演测量模型inverse model sensor,测量采样sample measurement，采样模型采用uniform_int_distribution,占据栅格地图算法。其中还有一些基本的激光参数相关的声明定义以及关于激光index索引的管理。

总结:

这个sensor文件夹主要涉及到SLAM专业相关的激光传感器领域。讲到这里，相信大家从grid文件夹,kinematics文件夹,sensor文件夹的头文件源码分析中逐渐浮现出SLAM建图的概念。

再回顾一遍gmapping建图的算法原理，它是基于FastSLAM的占据栅格地图实现，算法伪码如下所示:

关于FastSLAM补课链接:

问题与思考:

接下来有一些问题，稍微考考大家:

1.上面提到贴出的获得占据栅格地图的FastSLAM算法的伪码里，这里的sample_motion_model(运动模型采样)对应哪个文件夹呢?

答案:是kinematics目录下的速度采样。

kinematics目录下的头文件源码分析:

2.这里的measurement_model_map(测量模型)又对应哪个文件夹呢?

答案:是sensor目录下的测量采样。

sensor目录下的头文件源码解析：

3.updated_occupancy_grid(更新占据栅格)对应的是哪个文件夹呢?

答案是grid文件夹。

grid目录下的头文件源码分析:

4.那再问问大家，跟强化学习有什么关系?

答案：在planner文件夹。在这里SMCPlanner是主角。与FastSLAM建图算法不一样的是，在这里用粒子来携带轨迹，而不是携带地图。在这里粒子经历粒子滤波算法的一切。

planner目录下的头文件源码分析:

关于粒子滤波参考:

算法原理:

以及更通俗易懂的解释版本参考,这是一个基于粒子滤波的定位算法:蒙特卡洛定位蒙特卡罗算法，可以参考感受一下粒子忙碌的工作流程:

5.最后一个问题，这里频繁用到boost库的随机数采样，用到了哪些采样模型呢?

boost库的随机数生成模型介绍:

答:

a.sensor目录下用到uniform_int_distribution(在整数域上的均匀分布)；boost官网uniform_int_distribution参考链接:

b.kinematics目录下用到uniform_real_distribution(在区间[min,max]上的实数连续均匀分布)；boost官网uniform_real_distribution参考链接:

c.kinematics目录下用到的normal_distribution(正态分布)；boost官网normal_distribution参考链接:

相信大家都学得很不错了，那下一讲再见吧！

Bye bye!

下一讲见啦！