GMapping的地图结构
对于建图引擎而言，合理的地图表达方式是影响其运行效率的一个重要因素，因为它是整个建图过程中都要处理和维护的数据对象。 在拆解GMapping的ROS封装[1][2]过程中， 我们看到在GMapping中每个粒子都维护了一个地图，并最终采用了最优粒子的地图。它们使用类GMapping::ScanMatcherMap来描述地图。本文来详细解析该类。
1. 地图数据结构
在smmap.h中通过typedef定义了类型ScanMatcherMap。
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
其原型Map是一个有三个参数的模板类，定义在map.h文件中， 其中Cell指的是地图中每个单元的数据类型，Storage则是地图的存储结构类型，isClass默认为true目前没发现有什么用。所以ScanMacherMap的地图单元类型为PointAccumulator， 存储结构为HierachicalArray2D<PointAccumulator>，也是一个模板我们稍后回来再解析它。ScanMacherMap没有定义第三个参数，就采用默认值true。
template <class Cell, class Storage, const bool isClass=true>
class Map{ /* 省略 */ };
在Map中定义了如下的一些成员变量，它们的权限都是protected。其中第四行中定义的m_storage就是用于存储地图数据的对象，其它的字段均是对地图尺度和物理世界尺度，以及两者间关系的描述。
protected:
Point m_center;
double m_worldSizeX, m_worldSizeY, m_delta;
Storage m_storage;
int m_mapSizeX, m_mapSizeY;
int m_sizeX2, m_sizeY2;
m_worldSizeX, m_worldSizeY是所描述的物理世界尺寸，m_delta则是地图的缩放比例关系。m_mapSizeX和m_mapSizeY是栅格地图的大小，它们与世界尺寸之间的关系如下面两个公式所示。 m_sizeX2和m_sizeY2是两个辅助信息，分别是m_mapSizeX和m_mapSizeY的一半。
$$ m\_worldSizeX = m\_mapSizeX * m\_delta $$ $$ m\_worldSizeY = m\_mapSizeY * m\_delta $$
template <class T>
struct point{
inline point():x(0),y(0) {}
inline point(T _x, T _y):x(_x),y(_y){}
T x, y;
};
typedef point<int> IntPoint;
typedef point<double> Point;
m_center描述的是地图的中心位置，或者说是物理世界的原点，其类型为Point， 是在point.h中具现的模板类型。 在该文件中具现了IntPoint和Point两个类型，相关代码摘录如右面所示。模板类point是用struct的形式定义的，在C++中struct和class的主要差别在于它的成员变量或者函数默认都是public的权限。 在其定义中有两个成员变量x和y分别描述了二维坐标系下的两个坐标值。
具现的IntPoint和Point两个类型，在栅格地图中分别用于描述栅格地图坐标和实际物理世界坐标。由于物理世界是一个连续的坐标空间，需要使用浮点数来描述，使用double类型来具现Point。 而栅格地图的表示则是离散的，所以使用int类型来具现IntPoint。
类Map存在的意义就是在连续的物理世界和离散的计算机世界之间建立一个映射关系。成员变量m_storage只是在计算机世界中建立了一个地图，我们还需要一些映射函数来完成这一映射关系。 所以在类Map中定义了一对函数world2map和map2world，前者是将物理世界坐标转换为栅格地图坐标，后者则反过来将栅格地图坐标转换为物理世界坐标。
template <class Cell, class Storage, const bool isClass>
IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const {
return IntPoint((int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
}
template <class Cell, class Storage, const bool isClass>
Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const {
return Point((p.x-m_sizeX2)*m_delta, (p.y-m_sizeY2)*m_delta) + m_center;
}
为了方便获取地图信息，类Map还定义了一系列的cell函数来访问地图数据。这些函数本质上都是通过栅格地图坐标来查询m_storage中对应的栅格单元中的数值。 下面摘取了两个主要的cell函数，分别根据栅格地图坐标和物理世界坐标访问地图数据。通过栅格地图坐标访问数据的时候比较简单，直接查询对应栅格单元即可。 而通过物理世界坐标来访问时，需要先将之转换为栅格地图坐标再查询。
template <class Cell, class Storage, const bool isClass>
const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
AccessibilityState s=m_storage.cellState(p);
if (s&Allocated)
return m_storage.cell(p);
return m_unknown;
}
template <class Cell, class Storage, const bool isClass>
const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
IntPoint ip=world2map(p);
AccessibilityState s=m_storage.cellState(ip);
if (s&Allocated)
return m_storage.cell(ip);
return m_unknown;
}
上面这两格函数默认会返回一个m_unkown的值，这是栅格地图的默认输出。 它在类Map中被定义为一个静态的常量。 并在类定义之外完成了对改变量的初始化工作，其初值为-1。
template <class Cell, class Storage, const bool isClass=true>
class Map{ /* 省略其它类成员 */
static const Cell m_unknown;
};
template <class Cell, class Storage, const bool isClass>
const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
很多时候，我们并不知道环境有多大，所以地图会在构建的过程中逐渐长大。为了适应这一需求，类Map中还提供了resize和grow两个函数，用于调整地图大小，扩张地图。 这两个函数的输入参数列表都是一样的，使用物理世界坐标描述了地图的左下角和右上角的位置，这样所得到的地图也就是一个二维的矩形。
template <class Cell, class Storage, const bool isClass=true>
class Map{ /* 省略其它类成员 */
void resize(double xmin, double ymin, double xmax, double ymax);
void grow(double xmin, double ymin, double xmax, double ymax);
};
2. 地图的存储
让我们再回到类型ScanMatcherMap的定义上。 在该类型的定义中，我们传递了两个参数，其中第二个参数指定了模板Map具例化时存储空间的类型。实际上地图数据是很耗内存的，对于一个100平米的室内环境，厘米级的地图精度， 单浮点数的存储单元，需要接近4M的存储空间。机器人的工作环境往往不只是100平米，有时还需要在户外工作，数据量是十分庞大的。
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
HierarchicalArray2D以一种类似金字塔的形式组织地图内存，可以有效的降低内存的消耗，只是这个金字塔只有两层。下面的代码片段描述了该类的继承关系， 它本身是一个Array2D，其存储单元是一个指向Array2D的智能指针autoptr。
template <class Cell>
class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >
{ /* 省略成员定义 */ };
2.1 地图与二维数组
很多时候，我们所谓的地图都是一个平面图，用两个坐标来标记位置。比如说为了描述一个山脉，我们可以用经纬度来指示不同的位置，在各个位置上记录海拔高度。 再比如说一个室内的地图，我们可以为之赋予一个笛卡尔坐标系，使用我们所熟悉的xy坐标来定位，对应位置上可以用空闲或者被占用来标记，如此就得到了所谓的占用栅格地图。 很自然的，我们会想到使用一个矩阵或者说是二维数组的形式来描述地图。
GMapping定义了模板类Array2D来实现二维数组。 下面的代码片段中列举了该类的所有成员变量。其中public权限的m_cells就是二维数组实际存放数据的地方，它是一个两重指针。私有变量m_xsize和m_ysize则描述了数组的大小。
template<class Cell, const bool debug=false>
class Array2D {
public:
// 省略各个成员函数
Cell ** m_cells;
protected:
int m_xsize, m_ysize;
}
我们可以如下面左边的代码所示，指定数组大小来创建二维数组。我们主要关注其中第6到第8行中的申请内存构建m_cells的过程。在此过程中首先构建了一个有m_xsize个指针的数组，用m_cells指向其起始地址。 然后创建m_xsize个大小为m_ysize的Cell数组，分别用m_cells所指的指针数组中的各个指针指引。如此，给定x-y索引，我们就可以像下面右边的代码那样通过类似m_cells[x][y]的形式访问二维数组中的元素。
|
|
2.2 智能指针
template <class X>
class autoptr{
public:
struct reference{
X* data;
unsigned int shares;
};
inline autoptr(X* p=(X*)(0));
inline autoptr(const autoptr& ap);
inline autoptr& operator=(const autoptr& ap);
inline ~autoptr();
inline operator int() const;
inline X& operator*();
inline const X& operator*() const;
reference * m_reference;
};
指针是C/C++语言中灵活访问对象的一种方式，同时也是主要的BUG源。为了提高程序的灵活性，往往需要动态的向系统申请内存， 而这申请来的内存需要程序员自己管理。使用的时候有很多注意事项，比如说，对象的生命周期结束的时候需要释放对象的内存否则会产生内存泄漏， 需要注意指针的取值防止野指针的出现导致系统产生异常的行为，当有函数或者循环嵌套使用的时候还需要注意不要重复释放。
由于在C/C++语言中通过指针可以直接访问内存，所以异常的指针行为所带来的后果，往往是灾难性的。也有可能增加系统漏洞，带来安全性问题。 所以程序员必须小心谨慎的处理指针，这个过程十分繁琐，也就成为了很多人C/C++语言道路上的拦路虎。在C++11标准以后增加了所谓的智能指针的概念， 它可以在一定程度上缓解程序员的这种压力。
GMapping是十几年前的项目了，那时C++11的标准还没有出来， 他定义了类autoptr实现了智能指针的机制。 右边的代码是其完整的定义。
在autoptr的类定义中，还嵌套定义了一个结构体reference。这个结构体有两个成员data和shares。其中data是指向实际对象的指针， shares是一个计数器记录了使用data所指对象的autoptr数量。
autoptr中只有一个成员变量m_reference，它还重载了指针运算符*，直接将reference的字段data所指的对象以引用的形式返回，这样就能以类似指针的形式来访问对象了。
template <class X>
X& autoptr<X>::operator*(){
assert(m_reference && m_reference->shares && m_reference->data);
return *(m_reference->data);
}
每当通过拷贝的形式构造新的autoptr，就意味着多了一个autoptr的指针引用目标对象，此时需要增加对象的计数shares。
template <class X>
autoptr<X>::autoptr(const autoptr<X>& ap){
m_reference=0;
reference* ref=ap.m_reference;
if (ap.m_reference){
m_reference=ref;
m_reference->shares++;
}
}
而执行赋值语句的时候，存在两层含义。其一减少了一个指针指向运算符'='的左值所指的对象，其二增加了一个指针指向运算符'='的右值所指的对象。在下面的赋值运算符的重载函数中， 首先使用一个临时指针ref记录右值的m_reference。接着检查左值和右值是否指向相同的对象，如果是的就不需要再做任何额外的操作了。然后，减少左值的指针计数，并释放左值的指引资源。 最后更新左值的指引对象为右值所指的对象，并增加该对象的计数。
template <class X>
autoptr<X>& autoptr<X>::operator=(const autoptr<X>& ap){
reference* ref = ap.m_reference;
if (m_reference == ref){
return *this;
}
if (m_reference && !(--m_reference->shares)) {
delete m_reference->data;
delete m_reference;
m_reference=0;
}
if (ref) {
m_reference=ref;
m_reference->shares++;
} else
m_reference=0;
return *this;
}
每当有一个autoptr对象的生命周期结束的时候，都意味着少了一个指向目标对象的指针，所以需要减小目标对象的计数。当计数值减小到0的时候，说明不再有指针指向目标对象，其生命周期应该结束了。
template <class X>
autoptr<X>::~autoptr(){
if (m_reference && !(--m_reference->shares)){
delete m_reference->data;
delete m_reference;
m_reference=0;
}
}
2.3 地图金字塔
现在回过头来看用于存储地图数据的类HierarchicalArray2D， 之前我们说过这是一个类似于金字塔的二维数组，它可以在一定程度上降低描述一幅地图所消耗的内存。其成员函数cell就可以充分的体现这种金字塔形式的存储方式。
template <class Cell>
Cell& HierarchicalArray2D<Cell>::cell(int x, int y){
IntPoint c=patchIndexes(x,y);
assert(this->isInside(c.x, c.y));
if (!this->m_cells[c.x][c.y]){
Array2D<Cell>* patch=createPatch(IntPoint(x,y));
this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
}
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
return (*ptr).cell(IntPoint(x-(c.x << m_patchMagnitude),y-(c.y << m_patchMagnitude)));
}
在HierarchicalArray2D中把地图分为若干个patch，其拆分的粒度由m_patchMagnitude来描述。在cell函数中首先通过成员函数patchIndexes计算给定的坐标所属的patch的索引。 在patchIndexes函数中将给定的坐标值右移了m_patchMagnitude位，这相当于除以了\(2^n\)。在计算机中除法的计算是很慢的，所以这里通过移位的方式替代除法操作， 但是这一技巧只能用于整型的数据，对于浮点数不适用。
template <class Cell>
IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const{
if (x >= 0 && y >= 0)
return IntPoint(x >> m_patchMagnitude, y >> m_patchMagnitude);
return IntPoint(-1, -1);
}
接下来在cell函数中，断言输入的参数在地图范围内。研究HierarchicalArray2D的定义时，我们发现它继承自Array2D< autoptr< Array2D<Cell >>>， 也就是说其用于保存数据的m_cells是一个智能指针的二维数组。在cell函数的第5行中，判定对应的patch是否分配过内存，若为分配过则分配之。这样可以在一定程度上减少内存的使用， 只在需要的时候才申请新的内存。
如果patch的粒度越小，patch划分就越精细，那些未知的或者不可达的区域描述就越精准，它们很大程度上就是为分配过内存的patch区域。但是精细的patch分割，将使得金字塔的上层m_cells增大， 极端情况就是m_cells中的每个元素都只对应着一个指针，这样反而浪费了空间，就没有金字塔结构的必要了。所以patch粒度的划分是需要根据实际场景来确定的。
最后在第9行和第10行中，进一步的获取金字塔底层的元素，也就是实际需要的地图数据。
3. PointAccumulator
从类ScanMatcherMap的定义中， 我们知道扫描匹配地图中各个元素的数据类型都是PointAccumulator。 这个类型主要用于累积点坐标，同时提供了均值和求熵的运算。
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
下面的代码说明了类PointAccumulator的定义形式，并列举了所有的成员变量。GMapping将之定义为struct的形式，这样其中所有的成员默认都是public的。 在该类型中还将一个point的模板定义为FloatPoint类型，并定义了成员变量acc用于累积点坐标。此外还有变量n和visits分别用于记录累积的次数和访问的次数。 如下面右边的update函数所示，通过参数value来判定是否累加。
|
|
并定义了一个unknown_ptr的静态变量， 该变量在smmap.cpp中进行了初始化，如下左边代码的最后一行所示。 静态变量主要是用于描述类的一些共有的特性，它与具体的对象没有关系，所以我们可以直接通过类型名称访问它们。这里unknown_ptr可以当作一个常量来使用，用于标记未知的指针。
该类还提供了求取均值和熵的接口函数，均值很简单就是累积量除以计数总量。所谓的求熵运算，先以计数总量n除以访问量visits，用频率近似概率。然后以二值分布的形式计算熵。
inline Point mean() const {
return 1./n*Point(acc.x, acc.y);
}
double PointAccumulator::entropy() const {
if (!visits)
return -log(.5);
if (n==visits || n==0)
return 0;
double x = (double)n * SIGHT_INC / (double)visits;
return -(x * log(x) + (1-x) * log(1-x));
}
4. 完
扫描匹配地图是整个建图过程中都要处理和维护的对象。本文中，我们详细介绍了扫描匹配地图相关的各个类型定义。