gmapping头文件详细讲解
今天，继续讲解gmapping头文件。首先是sensor_range下的sensor_range.h，功能是用于存储距离数据的列表，有一个protected权限的成员变量m_pose，该变量记录了获取激光传感器扫描数据时的传感器的位置信息。这里提示一点，对于所有的析构函数一概不讲。
1、sensor_range.h
#include <vector>
#include "../utils/point.h"
namespace GMapping{
class RangeReading
{
public:
//分两种情况构造函数：1、均匀角度的激光雷达数据
RangeReading(unsigned int n_beams, const double* d); //构造函数，n_beams为激光雷达的激光数，d为激光雷达的激光数据
//2、不均匀角度的激光雷达数据 ，比上面的函数多一个参数激光雷达的角度数据
RangeReading(unsigned int n_beams, const double* d,const double* angle); //构造函数，n_beams为激光雷达的激光数，d为激光雷达的激光数据，angle为激光雷达的角度数据
virtual ~RangeReading(); //析构函数
//得到这帧激光数据的机器人位置
inline const OrientedPoint& getPose() const {return m_pose;}
//设置这帧传感器数据的位置
inline void setPose(const OrientedPoint& pose) {m_pose=pose;}
//返回激光束的多少
inline const unsigned int getSize() const {return m_beams;}
//存储激光雷达的距离信息
std::vector<double> m_dists; //激光雷达的距离信息
unsigned int m_beams; //激光雷达的激光数
//每个激光束对应的角度
std::vector<double> m_angles; //激光雷达的角度信息
protected:
OrientedPoint m_pose; //这帧激光数据的位置 这里的位置表示的是机器人的位姿
};
};
2、sensor_range.cpp
本来是准备先把头文件讲解完再具体讲解函数实现的，但是这部分代码量很少，就一并讲解了
namespace GMapping{
using namespace std;
/*构造函数
n_beams 指定了激光束的数量
d 表示各个激光的距离
angle 每束激光的角度
*/
//均匀角度的激光雷达数据
RangeReading::RangeReading(unsigned int n_beams, const double* d) //获取激光雷达数据
{
m_beams = n_beams; //将获取到的激光数赋值到m_beams中
m_dists.resize(n_beams); //将激光数量赋值给m_dists resize()的作用是改变vector中元素的数目。这部分属于容器vector知识，还不理解的同学请自学相关知识
for (unsigned int i=0; i<n_beams; i++) //遍历激光数量
m_dists[i]=d[i]; //将激光的距离赋值给m_dists 上一个是激光数量，这里是激光距离，不要搞混了
}
//不均匀角度的激光雷达数据
RangeReading::RangeReading(unsigned int n_beams, const double* d,const double* angle) //获取激光雷达数据
{
m_beams = n_beams; //将获取到的激光数赋值到m_beams中
m_dists.resize(n_beams);//将激光数量赋值给m_dists
for (unsigned int i=0; i<n_beams; i++) //遍历激光数量
{
m_dists[i]=d[i]; //将激光的距离赋值给m_dists
m_angles[i]=angle[i]; //将激光的角度赋值给m_angles
}
}
RangeReading::~RangeReading(){} //析构函数
};
下面开始讲解grid下的头文件,首先是accesstate.h,该文件非常简单，就定义了一个枚举类型，功能是用来表示地图的访问状态。
3、accesstate.h
namespace GMapping
{
enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; //定义一个枚举类型，用来表示地图的访问状态 ；Outside表示超出边界，Inside表示在边界内。
};
4、array2d.h
其次，是array2d.h，功能是根据地图尺寸给栅格单元分配内存以及对栅格单元的内存进行管理，特别地，对在边界内的栅格进行相关处理。
namespace GMapping
{
template<class Cell, const bool debug=false>
class Array2D //模板类，cell表示栅格单元，debug表示是否调试模式
{
public:
Array2D(int xsize=0, int ysize=0); //构造函数，xsize表示x方向的栅格数量，ysize表示y方向的栅格数量
Array2D& operator=(const Array2D &); //重载赋值运算符
Array2D(const Array2D<Cell,debug> &); //重载拷贝构造函数
~Array2D(); //析构函数
void clear(); //清空数组
void resize(int xmin, int ymin, int xmax, int ymax); //重新设置数组的尺寸 这里，说明一下栅格地图坐标系，x轴向右为正方向，y轴向下为正方向。 xmin和ymin表示左上角最小点的坐标值， xmax和ymax为右下角最大点坐标值
inline bool isInside(int x, int y) const; //判断某个栅格是否在数组内
inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);} //判断某个点是否在数组内
inline Cell& cell(int x, int y); //获取某个栅格的值
inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);} //获取某个点的值(重载)
inline const Cell& cell(int x, int y) const; //获取某个栅格的值（重载)
inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);}//获取某个点的值（重载)
//获取某个栅格的访问状态
inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);} //如果在数组内，则返回Inside或者Allocated，否则返回Outside
inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);} //获取某个点的访问状态(重载)
inline int getPatchSize() const{return 0;} //获取栅格单元的大小
inline int getPatchMagnitude() const{return 0;} //获取栅格单元的大小的平方根
inline Cell** cells() {return m_cells;} //获取数组的指针
inline int getXSize() const {return m_xsize;} //获取x方向的栅格数量
inline int getYSize() const {return m_ysize;} //获取y方向的栅格数量
Cell ** m_cells; //数组的指针
protected:
int m_xsize, m_ysize; //x方向的栅格数量，y方向的栅格数量
};
template <class Cell, const bool debug>
Array2D<Cell,debug>::Array2D(int xsize, int ysize) //构造函数
{
m_xsize=xsize; //x方向的栅格数量
m_ysize=ysize; //y方向的栅格数量
if (m_xsize>0 && m_ysize>0) //如果x方向的栅格数量大于0,y方向的栅格数量大于0
{
m_cells=new Cell*[m_xsize]; //分配内存
for (int i=0; i<m_xsize; i++) //遍历x方向的栅格数量
m_cells[i]=new Cell[m_ysize]; //分配内存
}
else //如果x方向的栅格数量小于0或y方向的栅格数量小于0
{
m_xsize=m_ysize=0; //设置x方向的栅格数量为0，y方向的栅格数量为0
m_cells=0; //设置数组的指针为0
}
}
template <class Cell, const bool debug>
Array2D<Cell,debug> & Array2D<Cell,debug>::operator=(const Array2D<Cell,debug> & g) //重载赋值运算符
{
if (debug || m_xsize!=g.m_xsize || m_ysize!=g.m_ysize) //如果是调试模式或者x方向的栅格数量不等于g的x方向的栅格数量或者y方向的栅格数量不等于g的y方向的栅格数量
{
for (int i=0; i<m_xsize; i++) //遍历x方向的栅格数量
delete [] m_cells[i]; //释放内存
delete [] m_cells; //释放内存
m_xsize=g.m_xsize; //设置x方向的栅格数量为g的x方向的栅格数量
m_ysize=g.m_ysize; //设置y方向的栅格数量为g的y方向的栅格数量
m_cells=new Cell*[m_xsize]; //分配内存
for (int i=0; i<m_xsize; i++) //遍历x方向的栅格数量
m_cells[i]=new Cell[m_ysize]; //分配内存
}
for (int x=0; x<m_xsize; x++) //遍历x方向的栅格数量
for (int y=0; y<m_ysize; y++) //遍历y方向的栅格数量
m_cells[x][y]=g.m_cells[x][y]; //设置某个栅格的值为g的某个栅格的值
return *this; //返回自身
}
template <class Cell, const bool debug>
Array2D<Cell,debug>::Array2D(const Array2D<Cell,debug> & g) //拷贝构造函数
{
m_xsize=g.m_xsize; //设置x方向的栅格数量为g的x方向的栅格数量
m_ysize=g.m_ysize; //设置y方向的栅格数量为g的y方向的栅格数量
m_cells=new Cell*[m_xsize]; //分配内存
for (int x=0; x<m_xsize; x++) //遍历x方向的栅格数量
{
m_cells[x]=new Cell[m_ysize]; //分配内存
for (int y=0; y<m_ysize; y++) //遍历y方向的栅格数量
m_cells[x][y]=g.m_cells[x][y]; //设置某个栅格的值为g的某个栅格的值
}
}
template <class Cell, const bool debug>
Array2D<Cell,debug>::~Array2D() //析构函数
{
for (int i=0; i<m_xsize; i++)
{
delete [] m_cells[i];
m_cells[i]=0;
}
delete [] m_cells;
m_cells=0;
}
template <class Cell, const bool debug>
void Array2D<Cell,debug>::clear() //清空函数
{
for (int i=0; i<m_xsize; i++) //遍历x方向的栅格数量
{
delete [] m_cells[i]; //释放内存
m_cells[i]=0; //设置遍历到的栅格的值为0
}
delete [] m_cells; //释放内存
m_cells=0; //设置数组的指针为0
m_xsize=0; //设置x方向的栅格数量为0
m_ysize=0; //设置y方向的栅格数量为0
}
template <class Cell, const bool debug>
void Array2D<Cell,debug>::resize(int xmin, int ymin, int xmax, int ymax) //调整地图大小函数
{
int xsize=xmax-xmin; //设置x方向的栅格数量为xmax-xmin
int ysize=ymax-ymin; //设置y方向的栅格数量为ymax-ymin
Cell ** newcells=new Cell *[xsize]; //分配内存
for (int x=0; x<xsize; x++) //遍历x方向的栅格数量
{
newcells[x]=new Cell[ysize]; //分配内存
}
int dx= xmin < 0 ? 0 : xmin; //设置dx为xmin小于0的情况下0，否则为xmin
int dy= ymin < 0 ? 0 : ymin; //设置dy为ymin小于0的情况下0，否则为ymin
int Dx= xmax < this->m_xsize ? xmax:this->m_xsize; //设置Dx为xmax小于m_xsize的情况下m_xsize，否则为xmax
int Dy= ymax < this->m_ysize ? ymax:this->m_ysize; //设置Dy为ymax小于m_ysize的情况下m_ysize，否则为ymax
for (int x=dx; x<Dx; x++) //遍历x方向的栅格数量
{
for (int y=dy; y<Dy; y++) //遍历y方向的栅格数量
{
newcells[x-xmin][y-ymin]=this->m_cells[x][y]; //设置某个栅格的值为this的某个栅格的值
}
delete [] this->m_cells[x]; //释放内存
}
delete [] this->m_cells; //释放内存
this->m_cells=newcells; //设置数组的指针为newcells
this->m_xsize=xsize; //设置x方向的栅格数量为xsize
this->m_ysize=ysize; //设置y方向的栅格数量为ysize
}
template <class Cell, const bool debug>
inline bool Array2D<Cell,debug>::isInside(int x, int y) const //判断某个栅格是否在地图内函数
{
return x>=0 && y>=0 && x<m_xsize && y<m_ysize; //判断某个栅格是否在地图内
}
template <class Cell, const bool debug>
inline Cell& Array2D<Cell,debug>::cell(int x, int y) //获取某个栅格的值函数
{
assert(isInside(x,y)); //判断某个栅格是否在地图内
return m_cells[x][y]; //返回某个栅格的值
}
template <class Cell, const bool debug>
inline const Cell& Array2D<Cell,debug>::cell(int x, int y) const //获取某个栅格的值函数（重载）
{
assert(isInside(x,y)); //判断某个栅格是否在地图内
return m_cells[x][y]; //返回某个栅格的值
}
};
5、harry2d.h
然后，是harry2d.h，功能是把地图分为若干个patch，如果patch的粒度越小，patch划分就越精细，那些未知的或者不可达的区域描述就越精准；但是精细的patch分割，将使得金字塔的上层m_cells增大， 极端情况就是m_cells中的每个元素都只对应着一个指针，这样反而浪费了空间，就没有金字塔结构的必要了，所以patch粒度的划分是需要根据实际场景来确定的。
namespace GMapping
{
template <class Cell>
class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > > //
{
public:
typedef std::set< point<int>, pointcomparator<int> > PointSet; //定义一个点集合类型
HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5); //构造函数
HierarchicalArray2D(const HierarchicalArray2D& hg); //拷贝构造函数
HierarchicalArray2D& operator=(const HierarchicalArray2D& hg); //重载赋值运算符
virtual ~HierarchicalArray2D(){} //析构函数
void resize(int ixmin, int iymin, int ixmax, int iymax); //调整地图大小函数
inline IntPoint patchIndexes(int x, int y) const; //获取某个栅格所在的patch的坐标函数
inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);} //获取某个点所在的patch的坐标函数（重载）
inline bool isAllocated(int x, int y) const; //判断某个栅格是否已经分配函数
inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);} //判断某个点是否已经分配函数（重载）
inline Cell& cell(int x, int y); //获取某个栅格的值函数
inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); } //获取某个点的值函数（重载）
inline const Cell& cell(int x, int y) const; //获取某个栅格的值函数（重载）
inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); } //获取某个点的值函数（重载）
inline AccessibilityState cellState(int x, int y) const ; //获取某个栅格的状态函数
inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); } //获取某个点的状态函数（重载）
inline void setActiveArea(const PointSet&, bool patchCoords=false); //设置活跃区域函数
inline void allocActiveArea(); //分配活跃区域函数
inline int getPatchSize() const {return m_patchMagnitude;} //获取patch大小函数
inline int getPatchMagnitude() const {return m_patchMagnitude;} //获取patch大小函数（重载）
const PointSet& getActiveArea() const {return m_activeArea; } //获取活跃区域函数
protected:
virtual Array2D<Cell> * createPatch(const IntPoint& p) const; //创建patch函数
PointSet m_activeArea; //活跃区域点集合
int m_patchMagnitude; //patch大小 （栅格单元的大小） （默认为5）
int m_patchSize; //patch大小 和m_patchMagnitude一样，但是比m_patchMagnitude小一点 但是不能改变m_patchMagnitude的值，因为m_patchMagnitude是用来计算patch大小的，如果改变了m_patchMagnitude，那么就会导致patch大小不一致，这样就会导致栅格单元的大小不一致
};
template <class Cell>
HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) //构造函数
:Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude))
{
m_patchMagnitude = patchMagnitude; //设置patch大小
m_patchSize = 1<<m_patchMagnitude; //设置patch大小
}
template <class Cell>
HierarchicalArray2D<Cell>::HierarchicalArray2D(const HierarchicalArray2D& hg) //拷贝构造函数
:Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude))
{
this->m_xsize=hg.m_xsize; //设置地图x方向大小
this->m_ysize=hg.m_ysize; //设置地图y方向大小
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize]; //创建一个二维数组指针
for (int x=0; x<this->m_xsize; x++) //遍历x方向栅格
{
this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize]; //创建一个二维数组指针
for (int y=0; y<this->m_ysize; y++) //遍历y方向栅格
this->m_cells[x][y]=hg.m_cells[x][y]; //设置每个栅格的值
}
this->m_patchMagnitude=hg.m_patchMagnitude; //设置patch大小
this->m_patchSize=hg.m_patchSize; //设置patch大小
}
template <class Cell>
HierarchicalArray2D<Cell>& HierarchicalArray2D<Cell>::operator=(const HierarchicalArray2D& hg) //赋值运算符重载
{
if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize) //如果两个地图大小不同
{
for (int i=0; i<this->m_xsize; i++) //遍历x方向栅格
delete [] this->m_cells[i]; //删除每个x方向栅格的二维数组指针
delete [] this->m_cells; //删除二维数组指针
this->m_xsize=hg.m_xsize; //设置地图x方向大小
this->m_ysize=hg.m_ysize; //设置地图y方向大小
this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];//创建一个二维数组指针
for (int i=0; i<this->m_xsize; i++) //遍历x方向栅格
this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize]; //创建一个二维数组指针
}
for (int x=0; x<this->m_xsize; x++) //遍历x方向栅格
{
for (int y=0; y<this->m_ysize; y++) //遍历y方向栅格
{
this->m_cells[x][y]=hg.m_cells[x][y]; //设置每个栅格的值
}
}
m_activeArea.clear(); //清空活跃区域点集合
m_patchMagnitude = hg.m_patchMagnitude; //设置patch大小
m_patchSize = hg.m_patchSize; //设置patch大小
return *this; //返回自身
}
template <class Cell>
void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax) //调整地图大小函数
{
int xsize=xmax-xmin; //设置地图x方向大小
int ysize=ymax-ymin; //设置地图y方向大小
autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize]; //创建一个二维数组指针
for (int x=0; x<xsize; x++) //遍历x方向栅格数量
{
newcells[x]=new autoptr< Array2D<Cell> >[ysize]; //创建一个二维数组指针
for (int y=0; y<ysize; y++) //遍历y方向栅格数量
{
newcells[x][y]=autoptr< Array2D<Cell> >(0); //创建一个二维数组指针
}
}
int dx= xmin < 0 ? 0 : xmin; // 设置x方向起始点 dx为xmin小于原地图x方向栅格数量的情况下原地图x方向栅格数量，否则为xin
int dy= ymin < 0 ? 0 : ymin; // 设置y方向起始点 dy为xmin小于原地图y方向栅格数量的情况下原地图y方向栅格数量，否则为ymin
//即左上角栅格的坐标
int Dx=xmax<this->m_xsize?xmax:this->m_xsize; //设置x方向终止点 Dx为xmax小于原地图x方向栅格数量的情况下原地图x方向栅格数量，否则为xmax
int Dy=ymax<this->m_ysize?ymax:this->m_ysize; //设置y方向终止点 Dy为ymax小于原地图y方向栅格数量的情况下原地图y方向栅格数量，否则为ymax
//即右下角栅格的坐标
for (int x=dx; x<Dx; x++) //遍历x方向栅格
{
for (int y=dy; y<Dy; y++) //遍历y方向栅格
{
newcells[x-xmin][y-ymin]=this->m_cells[x][y]; //设置每个栅格的值
}
delete [] this->m_cells[x]; //删除每个x方向栅格的二维数组指针
}
delete [] this->m_cells; //删除二维数组指针
this->m_cells=newcells; //设置二维数组指针
this->m_xsize=xsize; //设置地图x方向大小
this->m_ysize=ysize; //设置地图y方向大小
}
template <class Cell>
IntPoint HierarchicalArray2D<Cell>::patchIndexes(int x, int y) const //获取栅格所在patch的坐标函数
{
if (x>=0 && y>=0) //如果x坐标大于等于0且y坐标大于等于0
return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude); //返回栅格所在patch的坐标 x>>m_patchMagnitude为x除以2^m_patchMagnitude，y>>m_patchMagnitude为y除以2^m_patchMagnitude >>为右移运算符
return IntPoint(-1, -1); //返回（-1，-1）
}
template <class Cell>
Array2D<Cell>* HierarchicalArray2D<Cell>::createPatch(const IntPoint& ) const //创建patch函数
{
return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude); //创建一个栅格大小为2^m_patchMagnitude的二维数组指针
}
template <class Cell>
bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const //判断栅格是否已经分配函数
{
IntPoint c=patchIndexes(x,y); //获取栅格所在patch的坐标
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y]; //获取栅格所在patch的指针,是实际需要的地图数据
return (ptr != 0); //如果指针不为空，返回true，否则返回false
}
template <class Cell>
Cell& HierarchicalArray2D<Cell>::cell(int x, int y) //获取栅格函数
{
IntPoint c = patchIndexes(x,y); //获取栅格所在patch的坐标
assert(this->isInside(c.x, c.y)); //断言栅格所在patch的坐标是否在地图范围内
if (!this->m_cells[c.x][c.y]) //如果栅格所在patch的指针为空
{
Array2D<Cell>* patch = createPatch(IntPoint(x,y)); //创建patch
this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch); //设置栅格所在patch的指针
}
autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y]; //获取栅格所在patch的指针
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude))); //返回栅格的值
}
template <class Cell>
const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const //获取栅格函数（重载）
{
assert(isAllocated(x,y)); //断言栅格是否已经分配
IntPoint c=patchIndexes(x,y); //获取栅格所在patch的坐标
const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y]; //获取栅格所在patch的指针
return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude))); //返回栅格的值
}
template <class Cell>
AccessibilityState HierarchicalArray2D<Cell>::cellState(int x, int y) const //获取栅格状态函数
{
if (this->isInside(patchIndexes(x,y))) //如果栅格所在patch的坐标在地图范围内
{
if(isAllocated(x,y)) //如果栅格已经分配
return (AccessibilityState)((int)Inside|(int)Allocated); //返回栅格状态（在地图范围内且已经分配）
else
return Inside; //返回栅格状态（在地图范围内，但未分配）
}
return Outside; //返回栅格状态（不在地图范围内）
}
template <class Cell>
void HierarchicalArray2D<Cell>::setActiveArea(const typename HierarchicalArray2D<Cell>::PointSet& aa, bool patchCoords)//设置活动区域函数
{
m_activeArea.clear(); //清空活动区域
for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) //遍历活动区域
{
IntPoint p; //定义坐标点
if (patchCoords) //如果是栅格坐标
p=*it; //设置坐标点
else
p=patchIndexes(*it); //设置坐标点（栅格坐标转换为patch坐标）
m_activeArea.insert(p); //插入坐标点
}
}
template <class Cell>
void HierarchicalArray2D<Cell>::allocActiveArea() //分配活动区域函数
{
for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it) //遍历活动区域
{
const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y]; //获取栅格所在patch的指针
Array2D<Cell>* patch=0; //定义patch指针
if (!ptr) //如果指针为空
{
patch=createPatch(*it); //创建patch
}
else //如果指针不为空
{
patch=new Array2D<Cell>(*ptr); //创建patch
}
this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch); //设置栅格所在patch的指针
}
}
};
6、map.h
接着，是map.h,功能是在连续的物理世界和离散的计算机世界之间建立一个映射关系，如实现二者之间的坐标转换。
namespace GMapping
{
template <class Cell, class Storage, const bool isClass=true>
class Map
{
public:
//map()三次重载
Map(int mapSizeX, int mapSizeY, double delta); //构造函数（重载）
Map(const Point& center, double worldSizeX, double worldSizeY, double delta); //构造函数（重载）
Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta); //构造函数（重载）
void resize(double xmin, double ymin, double xmax, double ymax); //重设地图大小函数
void grow(double xmin, double ymin, double xmax, double ymax); //地图增长函数
inline IntPoint world2map(const Point& p) const; //将world坐标系转换为map坐标系函数
inline IntPoint world2map(double x, double y) const { return world2map(Point(x,y)); } //将world坐标系转换为map坐标系函数（重载）
inline Point map2world(const IntPoint& p) const; //将map坐标系转换为world坐标系函数
inline Point map2world(int x, int y) const { return map2world(IntPoint(x,y)); } //将map坐标系转换为world坐标系函数（重载）
inline Point getCenter() const {return m_center;} //获取地图中心点函数
inline double getWorldSizeX() const {return m_worldSizeX;} //获取世界坐标宽度函数
inline double getWorldSizeY() const {return m_worldSizeY;} //获取世界高度坐标函数 目前，这两个专业术语不知道是不是这样叫
inline int getMapSizeX() const {return m_mapSizeX;} //获取地图宽度函数
inline int getMapSizeY() const {return m_mapSizeY;} //获取地图高度函数
inline double getDelta() const { return m_delta;} //获取地图分辨率函数
inline double getMapResolution() const { return m_delta;} //获取地图分辨率函数（重载）
inline double getResolution() const { return m_delta;} //获取地图分辨率函数（重载） 目前，delta是指位姿差，不知道这里怎么变成地图分辨率了 如果大家知道，请告诉我，谢谢！
inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const //获取地图大小函数
{
Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1)); //获取地图最小坐标点和最大坐标点
xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y; //设置地图大小函数
}
inline Cell& cell(const IntPoint& p); //获取指定坐标的栅格函数
inline Cell& cell(int x, int y){ return cell(IntPoint(x, y)); } //获取指定坐标的栅格函数（重载）
inline const Cell& cell(const IntPoint& p) const; //获取指定坐标的栅格函数（重载）
inline const Cell& cell(int x, int y) const{ return cell(IntPoint(x, y)); } //获取指定坐标的栅格函数（重载）
inline Cell& cell(const Point& p); //获取指定坐标的栅格函数
inline Cell& cell(double x, double y){ return cell(Point(x, y)); } ///获取指定坐标的栅格函数（重载）
inline const Cell& cell(const Point& p) const; //获取指定坐标的栅格函数（重载）
inline const Cell& cell(double x, double y) const{ return cell(Point(x, y)); } ///获取指定坐标的栅格函数（重载）
//对描述栅格单元是否在边界里进行4次函数重载
inline bool isInside(int x, int y) const{ return m_storage.cellState(IntPoint(x,y))&Inside; } //判断指定坐标的栅格单元是否在边界里函数
inline bool isInside(const IntPoint& p) const{ return m_storage.cellState(p)&Inside; } //判断指定坐标的栅格单元是否在边界里函数（重载）
inline bool isInside(double x, double y) const{ return m_storage.cellState(world2map(x,y))&Inside; }//判断指定坐标的栅格单元是否在边界里函数（重载）
inline bool isInside(const Point& p) const{ return m_storage.cellState(world2map(p))&Inside; } //判断指定坐标的栅格单元是否在边界里函数（重载）
inline Storage& storage() { return m_storage; } //获取地图存储器函数
inline const Storage& storage() const { return 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; // 用来计算栅格单元的坐标的，比如，栅格单元的坐标是(x,y)，那么，x的范围是[0,m_sizeX2]，y的范围是[0,m_sizeY2]
static const Cell m_unknown; //未知栅格单元
};
//实现地图增长的函数，其实是描述地图的长宽的变化：
template <class Cell, class Storage, const bool isClass>
const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1); //对未知栅格进行表示，-1表示未知；
template <class Cell, class Storage, const bool isClass>
Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta): //构造函数，初始化地图的大小和栅格的大小
m_storage(mapSizeX, mapSizeY)
{
m_worldSizeX=mapSizeX * delta; //设置地图的长度
m_worldSizeY=mapSizeY * delta; //设置地图的宽度
m_delta=delta; //目前，因为不知道delta到底是什么，所以这里不理解，但是可能是栅格单元的大小，比如，栅格单元的大小是delta，那么，地图的长度是mapSizeX*delta，地图的宽度是mapSizeY*delta
m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY); //设置地图的中心点
m_sizeX2=m_mapSizeX>>1; //设置地图的长度的一半，这里的>>1是指的是右移一位，也就是除以2，
m_sizeY2=m_mapSizeY>>1; //设置地图的宽度的一半，这里的>>1是指的是右移一位，也就是除以2，
}
template <class Cell, class Storage, const bool isClass>
Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta): //构造函数，初始化地图的大小和栅格的大小
m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta))
{
m_center=center; //设置地图的中心点
m_worldSizeX=worldSizeX; //设置地图的长度
m_worldSizeY=worldSizeY; //设置地图的宽度
m_delta=delta; //目前，因为不知道delta到底是什么，所以这里不理解，但是可能是栅格单元的大小，比如，栅格单元的大小是delta，那么，地图的长度是mapSizeX*delta，地图的宽度是mapSizeY*delta
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize(); //设置地图的长度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize(); //设置地图的宽度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_sizeX2=m_mapSizeX>>1; //设置地图的长度的一半，这里的>>1是指的是右移一位，也就是除以2，
m_sizeY2=m_mapSizeY>>1; //设置地图的宽度的一半，这里的>>1是指的是右移一位，也就是除以2，
}
template <class Cell, class Storage, const bool isClass>
Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta): //构造函数，初始化地图的大小和栅格的大小
m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)) //设置地图的长度和宽度，这里的ceil是指的是向上取整，也就是把小数变成整数，这里的2是指的是地图的长度的一半，也就是地图的长度的一半
{
m_center=center; //设置地图的中心点
m_worldSizeX=xmax-xmin; //设置地图的长度
m_worldSizeY=ymax-ymin; //设置地图的宽度
m_delta=delta; //目前，因为不知道delta到底是什么，所以这里不理解，但是可能是栅格单元的大小，比如，栅格单元的大小是delta，那么，地图的长度是mapSizeX*delta，地图的宽度是mapSizeY*delta
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize(); //设置地图的长度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize(); //设置地图的宽度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_sizeX2=(int)round((m_center.x-xmin)/m_delta); //设置地图的长度的一半，这里的round是指的是向下取整，也就是把小数变成整数，
m_sizeY2=(int)round((m_center.y-ymin)/m_delta); //设置地图的宽度的一半，这里的round是指的是向下取整，也就是把小数变成整数，
}
template <class Cell, class Storage, const bool isClass>
void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax) //重设地图的大小
{
IntPoint imin=world2map(xmin, ymin); //计算地图的左上角点的坐标
IntPoint imax=world2map(xmax, ymax); //计算地图的右下角点的坐标
int pxmin, pymin, pxmax, pymax; //设置栅格单元的左上角点的坐标，右下角点的坐标
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
m_storage.resize(pxmin, pymin, pxmax, pymax); //重设栅格单元的左上角点的坐标，右下角点的坐标
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize(); //设置地图的长度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize(); //设置地图的宽度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_worldSizeX=xmax-xmin; //设置世界坐标x方向大小
m_worldSizeY=ymax-ymin; //设置世界坐标y方向大小
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude()); //设置地图的长度的一半，
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude()); //设置地图的宽度的一半，
}
//适当调整地图的大小，使得地图能够包含指定的点：
template <class Cell, class Storage, const bool isClass>
void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax) //适当调整地图的大小，使得地图能够包含指定的点
{
IntPoint imin=world2map(xmin, ymin); //计算地图的左上角点的坐标
IntPoint imax=world2map(xmax, ymax); //计算地图的右下角点的坐标
if (isInside(imin) && isInside(imax)) //如果地图的左上角点和右下角点都在地图内，那么不需要调整地图的大小
return; //nothing to do
imin=min(imin, IntPoint(0,0)); //如果地图的左上角点和右下角点都在地图外，那么地图的左上角点和右下角点都设置为(0,0)
imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1)); //如果地图的左上角点和右下角点都在地图外，那么地图的左上角点和右下角点都设置为(m_mapSizeX-1,m_mapSizeY-1)
int pxmin, pymin, pxmax, pymax; //设置栅格单元的左上角点的坐标，右下角点的坐标
pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude())); //设置栅格单元的左上角点的坐标，右下角点的坐标
m_storage.resize(pxmin, pymin, pxmax, pymax); //调整地图的大小
m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize(); //设置地图的长度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize(); //设置地图的宽度的一半，这里的<<m_storage.getPatchSize()是指的是左移m_storage.getPatchSize()位，也就是乘以2，
m_worldSizeX=xmax-xmin; //设置世界坐标x方向大小
m_worldSizeY=ymax-ymin; //设置世界坐标y方向大小
m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude()); //设置地图的长度的一半，
m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude()); //设置地图的宽度的一半，
}
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>
Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) //访问地图数据
{
AccessibilityState s=m_storage.cellState(p); //获取栅格地图坐标对应的栅格单元的状态
if (! (s&Inside)) //如果栅格地图坐标对应的栅格单元不在地图内，则抛出异常
assert(0);
return m_storage.cell(p); //返回栅格地图坐标对应的栅格单元的数值
}
template <class Cell, class Storage, const bool isClass>
Cell& Map<Cell,Storage,isClass>::cell(const Point& p) //访问地图数据
{
IntPoint ip=world2map(p); //将物理世界坐标转换为栅格地图坐标，赋值给ip
AccessibilityState s=m_storage.cellState(ip); //获取栅格地图坐标对应的栅格单元的状态
if (! (s&Inside)) //如果栅格地图坐标对应的栅格单元不在地图内，则抛出异常
assert(0);
return m_storage.cell(ip); //返回栅格地图坐标对应的栅格单元的数值
}
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; //如果栅格地图坐标对应的栅格单元不在地图内，则返回m_unknown 这里的m_unknown是一个默认值，可以自定义，比如返回0
}
template <class Cell, class Storage, const bool isClass>
const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const //访问地图数据
{
IntPoint ip=world2map(p); //将物理世界坐标转换为栅格地图坐标，赋值给ip
AccessibilityState s=m_storage.cellState(ip); //获取栅格地图坐标对应的栅格单元的状态
if (s&Allocated) //如果栅格地图坐标对应的栅格单元在地图内，则返回栅格地图坐标对应的栅格单元的数值
return m_storage.cell(ip);
return m_unknown; //如果栅格地图坐标对应的栅格单元不在地图内，则返回m_unknown 这里的m_unknown是一个默认值，可以自定义，比如返回0
}
};
7、smmap.h
最后，是smamap.h。
namespace GMapping {
struct PointAccumulator
{
typedef point<float> FloatPoint;
PointAccumulator(): acc(0,0), n(0), visits(0){} //构造函数
PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);} //构造函数
inline Point mean() const {return 1./n*Point(acc.x, acc.y);} //计算平均值
inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; } //计算访问次数
inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; } //添加一个点的访问次数
inline void update(bool value, const Point& p=Point(0,0)); //更新点的访问次数
inline double entropy() const; //计算熵值
static const PointAccumulator& Unknown(); //获取未知点的访问次数
static PointAccumulator* unknown_ptr; //未知点的访问次数的指针
FloatPoint acc; //点
int n, visits; //点的访问次数 n和visits的区别是，n是点的访问次数，visits是点的访问次数的总和
};
void PointAccumulator::update(bool value, const Point& p) //更新点的访问次数
{
if (value) //如果点的访问次数大于0,则更新点的访问次数
{
acc.x+= static_cast<float>(p.x); //更新点的x坐标
acc.y+= static_cast<float>(p.y); //更新点的y坐标
n++; //更新点的访问次数
visits+=SIGHT_INC; //更新点的访问次数的总和 SIGHT_INC是一个访问次数,默认值为1
}
else //如果点的访问次数为0，则不更新点的访问次数
visits++; //更新点的访问次数的总和 SIGHT_INC是一个访问次数,默认值为1
}
double PointAccumulator::entropy() const //计算熵值 ，熵值越大，访问次数越少 熵值的作用是，计算点的访问次数的稳定性
{
if (!visits) //如果点的访问次数的总和为0
return -log(.5); //返回-log(.5)
if (n==visits || n==0) //如果点的访问次数等于点的访问次数的总和或者点的访问次数为0
return 0; //返回0
double x=(double)n*SIGHT_INC/(double)visits; //计算点的访问次数的总和除以点的访问次数
return -( x*log(x)+ (1-x)*log(1-x) ); //返回-( x*log(x)+ (1-x)*log(1-x) )
}
typedef Map< PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap; //
};
8、next
下回继续讲解particlefilter和motionmodel文件，如果觉得文章不错，欢迎大家点赞分享，嘿嘿。
版权声明：本文为qq_49589440原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。