GMapping的粒子和轨迹树
根据GMapping的粒子滤波框架， 在完成扫描匹配的操作之后，就应该更新粒子的状态和重要性权重了。 建图引擎调用了函数scanMatch之后，就通过函数updateTreeWeights来更新粒子的权重。 而粒子的运动轨迹和占用地图的更新实际是在重采样过程中完成的。
粒子是粒子滤波器的基石，它记录了系统的状态。对SLAM问题而言，系统的状态包括了机器人的位姿状态以及环境地图。重要性权重虽然不是系统状态的一部分， 但是它反映了状态存在的可能性。在算法的实现过程中，人们通常也会将之写到粒子的成员变量中。
GMapping或者说RBPF的SLAM解决方案，都是先估计的机器人运动轨迹，再根据机器人的状态估计环境地图。由于粒子滤波涉及到频繁的重采样操作，需要不断地生成新的粒子，去除旧的粒子。 而每个粒子都独立的记录了一条可能的机器人轨迹和环境地图。为了提高效率，GMapping设计了一种轨迹树的数据结构，我们可以通过从叶子节点向根节点的追溯得到机器人的运动轨迹。
本文中，我们先详细分析GMapping中所用的粒子和轨迹树的数据结构，再分析函数updateTreeWeights。
struct Particle{
Particle(const ScanMatcherMap& map);
inline operator double() const {return weight;}
inline operator OrientedPoint() const {return pose;}
inline void setWeight(double w) {weight=w;}
ScanMatcherMap map;
OrientedPoint pose;
OrientedPoint previousPose;
double weight;
double weightSum;
double gweight;
int previousIndex;
TNode* node;
};
1. 粒子
GMapping在建图引擎的定义中嵌套地定义了Particle结构体。 右边是该结构体的定义。在C++中结构体与类没有过多的区分，只是默认情况下，结构体的成员都是public的。
第2行定义了一个构造函数，它的具体实现如下所示。有一个扫描匹配地图的引用map作为参数，该构造函数无非是对各种成员变量进行了初始化，赋予了初值。
Particle::Particle(const ScanMatcherMap& m):
map(m), pose(0,0,0), weight(0),
weightSum(0), gweight(0), previousIndex(0) {
node=0;
}
第4行和第5行，两次重载了操作符“()”，它们的返回值是不同的。假设p是一个Particle类型的对象，我们可以像下面这样将之用作为赋值语句的右值。 根据左值的数据类型的不同，变量w和op分别获取了粒子的重要性权重和位姿状态。
double w = p;
OrientedPoint op = p;
第6行，定义了一个set函数，用于设置粒子的权重。在我看来实际并不怎么需要这个函数，因为Particle中的所有成员都是公开的，我们可以直接访问其成员变量weight。
第8到10行，定义了机器人位姿状态和占用栅格地图的成员对象，它们是整个粒子滤波器迭代过程中更新的数据对象。而第12到14行，则记录了与重要性权重相关的信息。 第16行定义的previousIndex是为重采样定义的辅助变量，它将记录上代粒子集合中演化出本粒子的粒子索引。 第17行中定义的TNode类型的指针，则是轨迹树的体现，它指向了轨迹树中的一个叶子节点，表示当前粒子的最新姿态。通过该指针所指的TNode对象，我们可以追述到轨迹树的根节点上， 期间所经历的节点就是当前粒子所记录的一种可能的运动轨迹。
struct TNode{
TNode(const OrientedPoint& pose, double weight,
TNode* parent=0, unsigned int childs=0);
~TNode();
OrientedPoint pose;
double weight;
double accWeight;
double gweight;
const RangeReading* reading;
TNode* parent;
unsigned int childs;
mutable unsigned int visitCounter;
mutable bool flag;
};
2. 轨迹树
右边是轨迹树节点TNode类型的定义，与Particle一样，它也是结构体形式的定义。关于轨迹树的各个函数在 源文件gridslamprocessor_tree.cpp中得到了具体的实现。
下面使其构造函数的实现，它有四个参数。其中p记录了粒子的位姿，w是粒子的重要性权重。n则是所要构造的节点的父节点，如果没有父节点，那么在传参的时候给0就可以了， 这意味着所要构造的节点是轨迹树的根节点。c则是其孩子节点的数量。在构造函数中，分别为各个成员变量赋予了必要的初值。此外在第5、6行中，需要增加父节点的子女数量。
GridSlamProcessor::TNode::TNode(const OrientedPoint& p,
double w, TNode* n, unsigned int c){
pose=p; weight=w; childs=c; parent=n;
reading=0; gweight=0; flag=0; accWeight=0;
if (n)
n->childs++;
}
在析构的时候，我们需要注意释放资源。首先需要判定父节点是否存在，若存在需要减少父节点的子女数量。接着需要判定在释放了本节点之后，父节点是否还有子女， 如果没有我们需要进一步地释放父节点的资源，因为通过粒子所记录的叶子节点将不再可能追溯到这个父节点上。
GridSlamProcessor::TNode::~TNode(){
if (parent && (--parent->childs)<=0)
delete parent;
}
轨迹树节点TNode用成员变量pose记录机器人的位姿，weight记录粒子的权重，accWeight记录粒子的累积权重，reading记录传感器的读数，parent指示父节点，childs记录子女数量。
3. updateTreeWeights
建图引擎的成员函数updateTreeWeights十分简单，如下代码片段所示，只有一个参数用于描述是否需要对粒子权重进行归一化。在函数主体中，也是先后调用了三个函数， 分别完成归一化粒子权重、重置轨迹树、更新轨迹树权重三个任务。
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){
if (!weightsAlreadyNormalized)
normalize();
resetTree();
propagateWeights();
}
下面是函数normalize的代码片段，定义了局部变量gain与lmax用于具体计算各个粒子的权重，其中lmax是粒子集合中最大的权重。
inline void GridSlamProcessor::normalize(){
double gain=1./(m_obsSigmaGain*m_particles.size());
double lmax= -std::numeric_limits::max();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)
lmax=it->weight>lmax?it->weight:lmax;
接下来更新各个粒子的权重记录，并求和。
m_weights.clear();
double wcum=0;
for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
m_weights.push_back(exp(gain*(it->weight-lmax)));
wcum+=m_weights.back();
}
最后除以粒子的权重和，得到归一化后的粒子权重，并计算权重相似度\(N_{eff}\)。
m_neff=0;
for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){
*it=*it/wcum;
double w=*it;
m_neff+=w*w;
}
m_neff=1./m_neff;
}
函数resetTree遍历了所有的粒子，并沿着各个粒子的node节点向上追溯到根节点，清除遍历过程中各个节点的权重计数和访问计数器。
void GridSlamProcessor::resetTree() {
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
TNode* n=it->node;
while (n){
n->accWeight=0;
n->visitCounter=0;
n=n->parent;
}
}
}
函数propagateWeights更新了轨迹树中各个节点的权重。其中局部变量aw记录了叶子节点权重的和，由于叶子节点实际上是各个粒子所记录的最近一次的状态， 所以该值应当接近为1。lastNodeWeight实际上是根节点的累积权重。(按，实际上我并不理解轨迹树中各个节点权重的意义在哪里，更不用说这里的lastNodeWeight了。)
double GridSlamProcessor::propagateWeights(){
double lastNodeWeight=0;
double aw=0;
std::vector::iterator w=m_weights.begin();
for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
double weight=*w;
aw+=weight;
TNode * n=it->node;
n->accWeight = weight;
lastNodeWeight += propagateWeight(n->parent,n->accWeight);
w++;
}
if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001)
assert(0);
return lastNodeWeight;
}
在propagateWeights中调用了函数propagateWeight用于从叶子节点向上追溯到根节点，更新沿途经过的各个节点的权重和累积权重。它以递归的形式实现。
double propagateWeight(GridSlamProcessor::TNode* n, double weight){
if (!n)
return weight;
double w=0;
n->visitCounter++;
n->accWeight+=weight;
if (n->visitCounter==n->childs)
w=propagateWeight(n->parent,n->accWeight);
assert(n->visitCounter <= n->childs);
return w;
}
4. 完
本文中，我们先分析了GMapping中使用的粒子和轨迹树节点的数据结构。粒子是各个粒子滤波器的基础，它描述了机器人的可能状态，占用栅格地图以及粒子的重要性权重。 轨迹树用于描述机器人可能的运动轨迹，树中的每个叶子节点对应着一个粒子，从叶子节点出发向上追溯到根节点就可以得到一种可能的机器人轨迹。
建图引擎通过函数updateTreeWeights，完成了粒子权重的归一化操作，计算了用于判定是否需要进行重采样的指标\(N_{eff}\)，还更新了轨迹树中各个节点的权重和累积权重。 但是我并不理解轨迹树的权重有什么用处。