地图变量区分
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_center描述的是地图的中心位置，或者说是物理世界的原点，其类型为Point，
是在point.h中具现的模板类型。
在该文件中具现了IntPoint和Point两个类型，相关代码摘录如右面所示。模板类point是用struct的形式定义的，在C++中struct和class的主要差别在于它的成员变量或者函数默认都是public的权限。
在其定义中有两个成员变量x和y分别描述了二维坐标系下的两个坐标值。具现的IntPoint和Point两个类型，在栅格地图中分别用于描述栅格地图坐标和实际物理世界坐标。由于物理世界是一个连续的坐标空间，需要使用浮点数来描述，使用double类型来具现Point。
而栅格地图的表示则是离散的，所以使用int类型来具现IntPoint。
首先将空间点转化为地图坐标,存储到activeArea,再将根据坐标点(int),转化为patch的局部cell坐标指针,存储到m_activeArea全局变量,根据m_activeArea分配patch空间,
描述栅格状态
另外更新地图map的cell.n和cell.visits----
最后是不断拓展node节点
发布地图就是通过读取权重最大的粒子,然后通过子节点往上读取,直到根节点结束,根节点是一张空白地图
!!!------**
-这里需要注意,所谓的发布/map地图并不是粒子存储的地图,而是粒子每个节点存储点云信息,意思是每次都读取每个节点的点来构建地图,填充地图
粒子的地图只拿来做定位,不做地图发布信息;
粒子每个节点的存储点云用构建地图并发布地图
**-----
!!!
获取地图
matcher.invalidateActiveArea();//计算激活区域,标记位
matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));//计算激活区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。
//上面应该重复了computeActiveArea
matcher.registerScan(smap, n->pose, &((*n->reading)[0]));//计算激光束上所有点的熵,如果不更新地图,默认熵为0,只更新hit,返回熵值
更新粒子节点 过程
if (! m_count //第一次
|| m_linearDistance>=m_linearThresholdDistance
//超过阈值距离
|| m_angularDistance>=m_angularThresholdDistance
//超过阈值角度
|| (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
//与上一桢时间相隔超过设定周期
last_update_time_ = reading.getTime();
//更新上一刻时钟
----------------------------------------------------------------------------------
/*GridSlamProcessor::scanMatch函数
(void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings))函数*/
IntPoint p1=map.world2map(phit);
//点云坐标由世界坐标转栅格坐标
assert(p1.x>=0 && p1.y>=0);//合法值判断
IntPoint cp=map.storage().patchIndexes(p1);//返回p1坐标对应的序号索引
assert(cp.x>=0 && cp.y>=0);
activeArea.insert(cp);
/*//这应该可以调用函数
map.storage().setActiveArea(activeArea, true)函数
*/
for (PointSet::const_iterator it= activeArea.begin(); it!=activeArea.end(); it++){
//按照栅格序号读取每一个栅格
IntPoint p;
if (patchCoords)
p=*it;//直接获取栅格坐标
else
p=patchIndexes(*it);//定义在patch的坐标
m_activeArea.insert(p);
}
---------------------------------------------------------------------------------
/**这应该可以调用函数
map.storage().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];//将ptr栅格坐标对象引用到m_cells对应的地图容器网格
Array2D<Cell>* patch=0;//创建一个C++指针
if (!ptr){
//如果引用的对象为空内存
patch=createPatch(*it);//对应cell会创建一个新的空间
} else{
patch=new Array2D<Cell>(*ptr);//根据m_patchMagnitude定义patch Array2D<Cell>大小
}
this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);//patch表示为空间,将活动区域存到网格去
}
---------------------------------------------------------------------
/*
ScanMatcher::registerScan
*/
IntPoint p1=map.world2map(phit);
map.cell(p1).update(true, phit);//更新地图的n和visit
else{
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
m_matcher.invalidateActiveArea();//设为需要进行计算激活区域
m_matcher.computeActiveArea(it->map, it->pose, plainReading);//计算激活区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。
//因为computeActiveArea在registerScan重复计算了一次,估计上面的computeActiveArea多余了
m_matcher.registerScan(it->map, it->pose, plainReading);//计算激光束上所有点的熵,如果不更新地图,默认熵为0,只更新hit
// cyr: not needed anymore, particles refer to the root in the beginning!
TNode* node=new TNode(it->pose, 0., it->node, 0);
//node->reading=0;
node->reading = reading_copy;
it->node=node;
}
}
}