上一讲，梳理了这个包的ExplorationPlannerROS文件夹下的头文件部分，这一讲开始梳理 ExplorationPlanner文件夹。
ExplorationPlanner
从上面的截图可以看出，ExplorationPlanner分为这几部分:grid, kinematics, planner, sensor, trajectory, utils。
1.grid部分
1>PlanarGridIndex.h
先从PlanarGridIndex.h开始讲起。
#include <boost/functional/hash.hpp>头文件里的引用用到了boost库的hash函数，这一点实现在下面的这个函数，使用友元friend让外部的函数可以访问类内成员PlanarGridIndex对象实例化的m；
friend std::size_t hash_value(const PlanarGridIndex& m)
{
std::size_t seed = 0;
boost::hash_combine(seed, m.x_);
boost::hash_combine(seed, m.y_);
return seed;
}boost库的hash 函数补充:
从构造函数和私有成员变量可以看出，这个头文件跟栅格的x,y有关，类内定义的函数也是用于获取x,y或者设置x,y;
// Constructors 构造函数
PlanarGridIndex(int x, int y) : x_(x), y_(y) { }
// Private attributes
int x_;
int y_;最后用到运算符operator==来比较两个栅格ma,mb对应的x,y是否一致；用到inline标志符，也就是内联函数，它应用场景是这种短小精悍的函数计算；
inline bool operator==(const PlanarGridIndex& ma, const PlanarGridIndex& mb)
{
return ( (ma.getX() == mb.getX()) && (ma.getY() == mb.getY()) );
}同样的，使用运算符operator!=来比较两个栅格ma,mb，注意它的返回是!(ma==mb),实际上是调用上面的内联函数operator==比较结果，再加一个！表示相反的结果；
inline bool operator!=(const PlanarGridIndex& ma, const PlanarGridIndex& mb)
{
//ma==mb的比较进入上一个内联函数inline bool operator==(const PlanarGridIndex& ma,const PlanarGridIndex& mb)
return !( ma == mb);
}小结：这个头文件用于管理单个栅格的x,y，以及比较两个栅格是否一样。
2>PlanarGridContainer.h
从这个头文件的名字可以看出，这个头文件是用于管理grid的容器。
从头文件的引用可以看出，第一个引用"PlanarGridIndex.h"，也就是我们上面讲到的管理栅格单元的x,y的头文件，第二个是使用了boost库的unordered_map容器。
#include "PlanarGridIndex.h"
#include <boost/unordered_map.hpp>boost库的unordered_map容器补充:
这里将这个管理栅格地图的容器作为一个可以表示任意类型的T；
// A sparse grid map container for arbitrary data type T.
template<class T>在类内定义声明该容器以及定义它所使用的迭代器iterator，迭代器可以理解为在容器里的一个遍历工具；
//T 指的是地图数据的类型
//定义容器内的数据类型
typedef boost::unordered_map<PlanarGridIndex, T, boost::hash<PlanarGridIndex> > Container;
typedef typename Container::const_iterator ContainerConstIter;//const_iterator迭代器
typedef typename Container::iterator ContainerIter;//iterator普通的迭代器我们将该容器实例化一个对象M_,用于指代容器：
// Private attributes
Container M_;关于获取容器里栅格单元的index有严格的const符号限定,以及用到了at帮助返回指定index下对应的gridValue；
const T& getGridValue(const PlanarGridIndex& idx) const
{
// Throws out_of_range if idx does not match any key in M_:
return M_.at(idx);
}关于unordered_map容器的at使用补充:
如何设置指定index下的girdValue也有讲究，使用下标运算符[] 帮助查找容器的指定的index对应的栅格单元，从而进行对该栅格单元的值进行修改;
void setGridValue(const PlanarGridIndex& idx, const T& v)
{
M_[idx] = v;
}关于unordered_map容器的下标运算符[]使用补充:
验证要查找的index对应的栅格是否在容器内:
bool isInContainer(const PlanarGridIndex& idx) const
{
return ( M_.find(idx) != M_.end() );
}关于unorderd_map容器的find函数使用补充:
返回容器的size以及返回是否为空的真值:
unsigned int size() const
{
return M_.size();
}
bool empty() const
{
return M_.empty();
}使用const_iterator类型的迭代器，那么查找指定index对应的栅格单元的函数格式会不一样 ;
ContainerConstIter find(const PlanarGridIndex& idx) const
{
return M_.find(idx);
}以及表示begin和end也都不一样:
ContainerConstIter begin() const
{
return M_.begin();
}
ContainerConstIter end() const
{
return M_.end();
}关于unordered_map的beigin和end补充:
既然已经是一个管理栅格地图的所有栅格单元的容器，那么找出这个栅格地图里栅格单元的x,y 最大值和最小值，是没什么难度的，看下面这个getMapExtents函数；
// Gets minimum and maximum values for the maps indices
bool getMapExtents(int& x_min, int& x_max, int& y_min, int& y_max) const
{
if (M_.empty())
return false;
x_min = M_.cbegin()->first.getX();
x_max = M_.cbegin()->first.getX();
y_min = M_.cbegin()->first.getY();
y_max = M_.cbegin()->first.getY();
for (ContainerConstIter it = M_.cbegin(), iend = M_.cend(); it != iend; ++it)
{
x_min = std::min(x_min, it->first.getX());
x_max = std::max(x_max, it->first.getX());
y_min = std::min(y_min, it->first.getY());
y_max = std::max(y_max, it->first.getY());
}
return true;
}将迭代器ContainerConstIter实例化为it对象,前缀自增++it，用于逐个遍历容器中的元素，迭代器it完成一轮遍历，目的是找到栅格单元里x的最小值，x的最大值， y的最小值，y的最大值。
小结:这个头文件用于管理整个地图的栅格单元，以及找到栅格单元里xmin, xmax, ymax, ymin。
3>IPlanarGridMap.h
和上面一样，先看引用的头文件:
#include "PlanarGridIndex.h"
#include "ExplorationPlanner/kinematics/PlanarPose.h"根据上面的介绍，PlanarGridIndex.h是管理单个栅格单元的头文件，作用是对带index标签的单个栅格单元进行x,y的赋值以及获取x,y的值，还有做两个栅格单元之间的比较。
PlanarPose.h是kinematics文件夹下的一个头文件，主要元素是x,y,theta，函数的功能是获取x,y,theta的值；
回到我们现在要讲解的头文件IPlanarGridMap.h，我们是需要用PlanarPose类型的对象来定义我们地图的原点origin，而地图的分辨率resolution用一个double类型就可以了；
//private attributes
PlanarPose origin_; // world coordinate for the corner of cell (0,0)
double meters_per_cell_; // resolution既然origin和resolution都是私有成员，那么获取它们的函数是需要的；
//获取原点origin
const PlanarPose& getOrigin() const {
return origin_;
}
//获取地图的分辨率resolution
double getMetersPerCell() const {
return meters_per_cell_;
}比较关键的函数还是world坐标系转换到map坐标系，map坐标系转换world坐标系。
为什么呢，因为要将 world坐标系下的x坐标转换成map坐标系下的index索引，下面这个函数，传入的形参是world坐标系下的x,y，而索引的表示采用了引用&，表示最终返回的是索引:PlanarGridIndex& idx;
如何转换呢?关于索引的x,y值设置:
void worldToMap(double wx, double wy, PlanarGridIndex& idx) const
{
idx.setX( static_cast<int> ((wx - origin_.getX()) / meters_per_cell_) );
idx.setY( static_cast<int> ((wy - origin_.getY()) / meters_per_cell_) );
}那么反过来，如何将map坐标系下的索引转换成world坐标系的坐标呢?
这里:
代码实现:
传入形参是索引:PlanarGridIndex idx,而返回wx,wy均用引用&表示，所以函数类型为void;
void mapToWorld(PlanarGridIndex idx, double& wx, double& wy) const
{
wx = origin_.getX() + (idx.getX() + 0.5) * meters_per_cell_;
wy = origin_.getY() + (idx.getY() + 0.5) * meters_per_cell_;
}小结:这个头文件主要是设置地图的原点和分辨率，以及实现map坐标系下的栅格索引index到世界坐标系的x,y坐标的相互转换。
4>PlanarGridBinaryMap.h
照例看它引用的头文件:
#include "IPlanarGridMap.h"
#include "PlanarGridContainer.h"这个就是我们上面讲的两个头文件，第一个头文件IPlanarGridMap.h是用来设置地图的原点和分辨率，以及实现栅格单元的索引与坐标转换，第二个头文件PlanarGridContainer.h是管理地图里所有栅格单元的容器，它的作用就是可以遍历所有栅格单元，以及对指定栅格单元进行值的修改，还可以找出栅格单元里索引对应的x,y的最小值和最大值。
既然有引用这两个重量级的头文件，不继承一下这两个类那岂不是太浪费代码量了么，和我们猜测得没错,果不其然，这里继承了IPlanarGridMap,PlanarGridContainer这两个大类，而且我们发现在继承层次结构中使用关键字virtual。
class PlanarGridBinaryMap : virtual public IPlanarGridMap, virtual public PlanarGridContainer<bool>
{
public:
PlanarGridBinaryMap(PlanarPose origin, double meters_per_cell)
: IPlanarGridMap(origin, meters_per_cell),
PlanarGridContainer<bool>()
{
}“在继承层次结构中，继承多个从同一个类派生而来的基类时，如果这些基类没有采用虚继承，将导致二义性。在函数声明中，vritual意味着当基类指针指向派生对象时，通过它可调用派生类的相应函数。从Base类派生出Derived1和Derived2类时，如果使用了关键字virtual,则意味着再从Derived1 和Derived2派生出Derived3时，每个Derived3实例只包含一个Base实例，也就是说，关键字virtual被用于实现两个不同的概念。”---引用自《Sams Teach Yourself C++ in One Hour a Day》。
我们可以看到IPlanarGridMap类有使用PlanarGridIndex类实例化的对象，而且我们也看到了PlanarGridContainer有使用PlanarGridIndex类实例化的对象，其实二者都隐藏继承了PlanarGridIndex这个基类。而PlanarGridBinarMap又同时继承了这两个重量级的类，不使用虚继承怎么行呢？计算机不知道是哪个PlanarGridIndex对象，这就导致二义性，出现菱形问题(Diamond Problem)。因此在继承层次结构中使用关键字vritual，将基类PlanarGridIndex的实例个数限定为1。
还没理解的话，就看看这个吧：
小结:这个类主要是声明继承了能对栅格地图元素进行内容管理以及能在栅格地图中栅格单元遍历的管理工具；另外再加了自己一个独有的功能，由函数dilate()实现；
通过在每个栅格单元格周围标记一个像素值"true"来增加"安全余量":
// increase "safety margin" by marking one pixel around each cell with value "true"
void dilate();5>PlanarGridOccupancyMap.h
搓搓小手，我们已经来到了grid文件夹下面最后一个头文件,看名字直译就是占据栅格地图。
看引用的其它头文件,有两个是grid文件夹本地的：
#include "ExplorationPlanner/grid/IPlanarGridMap.h"
#include "ExplorationPlanner/grid/PlanarGridContainer.h"还有一个是sensor文件夹下的,这个文件夹主要是用传感器模拟出observations，我们这里使用只是为了传入这个observations，来更新我们的栅格单元；
#include "ExplorationPlanner/sensor/PlanarObservation.h"其它的来自工具文件夹utils下的，Entropy意思是熵，Log字面意思我们猜测是要进行对数转换，毕竟占据栅格地图算法是用到对数滴。我们这里用到这两个头文件也就是为了获取熵值和Log_odds值；
#include "ExplorationPlanner/utils/EntropyLUT.h"
#include "ExplorationPlanner/utils/LogOddsLUT.h"比如代码中：
//获取普普通通的栅格单元cell的熵值
double getEntropy() const;
//获取sensors模拟observations产生的cells的熵值
double getEntropyInCells(const PlanarObservation& cells) const;占据栅格地图补课链接如下:找到->Ⅷ.占据栅格地图
好了，回到正题，我们来看看这个PlanarGridOccupancyMap类做了什么，同时继承了两个来自grid文件夹下的类，依旧添加virtual关键字：
class PlanarGridOccupancyMap : virtual public IPlanarGridMap, virtual public PlanarGridContainer<int8_t>那么，除了循规蹈矩地定义地图的原点，分辨率，管理栅格单元，还做了什么呢？
首先是新加入了对栅格地图的更新功能：
void update(const PlanarObservation& cells);其次是能把由observations新加入的cells加到地图里进来：
void copyCellsFromOtherMap(const PlanarGridOccupancyMap& other_map, const PlanarObservation& cells);值得一提的是，在类内私有成员里:
private:
//更新单个栅格单元cell，我们接受的传入是栅格单元的索引index,
//以及由sensor模拟产生的单个cell的obserations
void updateCell(const PlanarGridIndex& idx, const PlanarCellObservation& obs);
//熵值
EntropyLUT entropy_lut_;
//对数值
LogOddsLUT logodds_lut_;
//传入log对数输出概率值
static int8_t lo2prob(double l);小结:这个happy 占据栅格地图类就到这里结束了，我们可以看到这里强调的是栅格单元的更新，整个栅格地图的更新，至于更新的影响因子，来源于sensor文件夹下的PlanarCellObservation.h 和PlanarObservation.h，这两个文件夹是由激光传感器模拟产生的observations。由它们影响着栅格单元值的更新、栅格地图的更新。
总结:至此，grid文件夹下的所有头文件已梳理完了，是不是对栅格地图GridMap、栅格地图plus版(占据栅格地图OccupancyGridMap)有了更深的理解呢？我想应该会的。
¡Hasta luego!
下一讲见：
评论（0）
您还未登录，请登录后发表或查看评论