SLAM GMapping（7）粒子和轨迹
1. 粒子
在 《SLAM GMapping（4）SLAM处理器》 中粒子滤波更新的每个粒子都独立的记录了一条可能的机器人轨迹和环境地图
为了提高效率，GMapping设计了一种轨迹树的数据结构
可以通过从叶子节点向根节点的追溯得到机器人的运动轨迹
在openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h中的Particle结构体
/* 该类定义了过滤器的粒子。 每个粒子都有一个映射、一个位姿、一个权重，并在轨迹树中保留当前节点 */
struct Particle
{
/* 构造一个粒子，给定一个映射，参数map：粒子的地图 */
Particle(const ScanMatcherMap &map);
/* 返回粒子的权重 */
inline operator double() const { return weight; }
/* 返回粒子的位姿 */
inline operator OrientedPoint() const { return pose; }
/* 设置粒子的权重，参数w：权重 */
inline void setWeight(double w) { weight = w; }
/* 地图 */
ScanMatcherMap map;
/* 位姿 */
OrientedPoint pose;
/* 机器人在前一时间段的位姿(用于计算里程计位移) */
OrientedPoint previousPose;
/* 权重 */
double weight;
/* 累积权重 */
double weightSum;
double gweight;
/* 轨迹树中前一个粒子的索引 */
int previousIndex;
/* 轨迹树的指针入口 */
TNode *node;
};
构造函数对各种成员变量进行了初始化，赋予了初值
/**
* @brief 粒子构建函数
*
* @param m 激光地图对象
*/
GridSlamProcessor::Particle::Particle(const ScanMatcherMap &m) : map(m), pose(0, 0, 0), weight(0), weightSum(0), gweight(0), previousIndex(0)
{
node = 0;
}
通过该指针所指的TNode对象，可以追述到轨迹树的根节点上
期间所经历的节点就是当前粒子所记录的一种可能的运动轨迹
2. 轨迹
轨迹树节点TNode类型的定义，与Particle一样，它也是结构体形式的定义
struct TNode
{
/* 构造轨迹树的节点 */
TNode(const OrientedPoint &pose, double weight, TNode *parent = 0, unsigned int childs = 0);
/* 销毁树节点，并持续更新树。 如果父节点只有一个子节点被删除，那么父节点也会被删除。这是因为父结点在轨迹树中不再存在。 */
~TNode();
/* 位姿 */
OrientedPoint pose;
/* 权重 */
double weight;
/* 所有粒子权重的总和是轨迹的前一节点的权重累加 */
/* 在轨迹的某一节点的粒子权重总和 */
double accWeight;
double gweight;
/* 父节点 */
TNode *parent;
/* 此节点关联的读取范围，即记录激光传感器数据 */
const RangeReading *reading;
/* 子节点数量 */
unsigned int childs;
/* 访问节点的计数器（内部使用） */
mutable unsigned int visitCounter;
/* 访问标志（内部使用） */
mutable bool flag;
};
构造函数对各种成员变量进行了初始化，赋予了初值
/**
* @brief 粒子运动轨迹节点构建函数
*
* @param p 粒子位姿
* @param w 粒子权重
* @param n 所要构造的节点的父节点
* @param c 该节点的子节点的数量
*/
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;
/* 增加父节点的子节点数量 */
if (n)
{
n->childs++;
}
flag = 0; /* 访问标志初始化 */
accWeight = 0; /* 权重总和初始化 */
}
在析构的时候，需要注意释放资源
/**
* @brief 粒子运动轨迹节点析构函数
*/
GridSlamProcessor::TNode::~TNode()
{
/* 判定父节点是否存在，并减少父节点的子节点数量，释放资源 */
if (parent && (--parent->childs) <= 0)
delete parent;
assert(!childs);
}
3. 更新轨迹权重
粒子滤波中，粒子的权重已在第二步扫描匹配中初步更新
第三步，更新粒子轨迹权重 updateTreeWeights
参数weightsAlreadyNormalized 用于描述粒子权重是否已归一化
函数调用了三个函数， 分别完成归一化粒子权重、重置轨迹树、更新轨迹树权重三个任务
/**
* @brief 更新轨迹权重函数
*
* @param weightsAlreadyNormalized 权重已归一化
*/
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized)
{
/* 确保所有权重归一化 */
if (!weightsAlreadyNormalized)
{
normalize();
}
/* 重置所有轨迹树 */
resetTree();
/* 更新所有轨迹树权重 */
propagateWeights();
}
3.1. 粒子权重归一化
在openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx中定义normalize函数
除了归一化所有粒子权重，还计算权重相似度neff
/**
* @brief 粒子权重归一化函数
*
* 归一化所有粒子权重，计算权重相似度
*/
inline void GridSlamProcessor::normalize()
{
/* 计算粒子的权重增益 = 1 /（似然度的平滑因子 * size） */
double gain = 1. / (m_obsSigmaGain * m_particles.size());
/* 计算粒子集合中最大的权重 */
double lmax = -std::numeric_limits<double>::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; /* 累积权重初始化 */
m_neff = 0; /* 相似性初始化 */
/* 遍历粒子群，更新所有粒子权重范围 */
for (std::vector<Particle>::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
m_weights.push_back(exp(gain * (it->weight - lmax))); /* 更新权重在范围[0,1] */
wcum += m_weights.back(); /* 累积求和权重 */
}
m_neff = 0; /* 相似性初始化 */
/* 遍历粒子权重，更新所有粒子权重归一化 */
for (std::vector<double>::iterator it = m_weights.begin(); it != m_weights.end(); it++)
{
*it = *it / wcum; /* 权重除以权重和，得到归一化后的粒子权重 */
/* 累加权重相似度倒数 */
double w = *it;
m_neff += w * w; /* m_neff会一直小于1，若所有w都比较接近，则m_neff会较小，倒数后就会较大，就说明权重差距较小 */
}
m_neff = 1. / m_neff; /* 计算权重相似度 */
}
3.2. 重置轨迹树
在openslam_gmapping/gridfastslam/gridslamprocessor_tree.cpp中定义resetTree函数
也就是清除各个节点的累积权重和访问计数
/**
* @brief 重置所有轨迹树函数
*
* 遍历了所有的粒子，各个轨迹节点的累积权重和访问计数
*/
void GridSlamProcessor::resetTree()
{
// don't calls this function directly, use updateTreeWeights(..) !
/* 不要直接调用这个函数，使用 updateTreeWeights */
/* 遍历了所有的粒子 */
for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++)
{
/* 沿着粒子的node节点向上追溯到根节点，清除遍历过程中各个节点的累积权重和访问计数 */
TNode *n = it->node;
while (n)
{
n->accWeight = 0;
n->visitCounter = 0;
n = n->parent;
}
}
}
3.3. 更新轨迹树权重
在openslam_gmapping/gridfastslam/gridslamprocessor_tree.cpp中定义propagateWeights函数
/**
* @brief 更新所有轨迹树权重函数
*
* @return lastNodeWeight 粒子群的运动轨迹根节点的累积权重
*/
double GridSlamProcessor::propagateWeights()
{
double lastNodeWeight = 0; /* 粒子群的运动轨迹根节点的累积权重 */
double aw = 0; /* 粒子群的运动轨迹叶子节点权重的和 */
/* 定义所有（粒子群的最新位姿）权重的叶子节点 */
std::vector<double>::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++; /* 叶子节点（粒子群的最新位姿）权重入口随遍历粒子群变化 */
}
/* 所有粒子的轨迹的叶子和根节点的权重和都应该为1，因都属于某次数据更新的权重总和 */
if (fabs(aw - 1.0) > 0.0001 || fabs(lastNodeWeight - 1.0) > 0.0001)
{
cerr << "ERROR: ";
cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl;
assert(0);
}
/* 返回粒子群的运动轨迹根节点的累积权重 */
return lastNodeWeight;
}
其中调用了propagateWeight函数
用于从叶子节点寻轨迹的根节点，更新轨迹上节点的访问计数和累积权重，并获取根节点权重
/**
* @brief 获取单轨迹树根节点权重函数
*
* 累积当前轨迹树的节点的访问计数和的权重，返回当前轨迹树的根节点权重
*
* @param n 轨迹树的节点
* @param weight 当前轨迹树的累积权重
* @return w 当前轨迹树的根节点权重
*/
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;
}
谢谢