欢迎访问我的博客首页。
1.权重
粒子权重 Particle::weight 是 double 类型。从函数 GridSlamProcessor::init 中可以看出,刚创建的粒子,其权重被设置为 0。然后粒子的权重就在函数 GridSlamProcessor::processScan 中变化。我们把与粒子权重有关的代码列出如下。
bool GridSlamProcessor::processScan(const RangeReading &reading, int adaptParticles) {
scanMatch(plainReading);
updateTreeWeights(false);
resample(plainReading, adaptParticles, reading_copy);
}
函数 GridSlamProcessor::scanMatch 负责权重的来源。它把调用函数 ScanMatcher::likelihoodAndScore 计算的似然度加到粒子的权重,而这个似然度代表的是粒子所存位姿的精确度。所以我们可以知道,粒子的权重代表了粒子所存位姿的好坏,权重越高,位姿越好。
函数 GridSlamProcessor::updateTreeWeights 负责权重的传播。传播前,它先对粒子的权重归一化。粒子的权重仅有相对意义,即,用于区分粒子的相对好坏。而归一化不改变权重的相对大小。
void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) {
if (!weightsAlreadyNormalized) {
normalize();
}
resetTree();
propagateWeights();
}
double propagateWeight(GridSlamProcessor::TNode *n, double weight) {
// 如果已经到达根结点,停止传播。
if (!n)
return weight;
double w = 0;
// 已经有这么多子结点传播来 acc 权重。
n->visitCounter++;
// 父结点的 acc 权重是其所有子结点的 acc 权重之和。
n->accWeight += weight;
// 所有子结点都已传播来 acc 权重。
if (n->visitCounter == n->childs) {
w = propagateWeight(n->parent, n->accWeight);
}
assert(n->visitCounter <= n->childs);
// 一棵轨迹树根结点的 acc 权重。
return w;
}
函数 GridSlamProcessor::resetTree 重置权重传播时用到的变量,其中一个是 accWeight。accWeight 也称为权重,但它仅用于权重传播过程中。我们用下图表示函数 propagateWeight 的权重传播。图中所示是粒子总数为 n=8 时,权重从叶结点向上传播。从图中可以看出,权重传播很简单:父节点的权重是其所有子结点的权重之和。
函数 GridSlamProcessor::resample 负责权重的利用,即权重用于粒子重采样。上图中,有子结点的结点就是之前重采样时被采样到的点。可以看出,权重传播之后,每一代所有被采样的粒子的权重之和都是
w
1
+
w
2
+
.
.
.
+
w
8
=
1
w_1+w_2+...+w_8=1
w1+w2+...+w8=1。gmapping 建图结束后,会得到 n=8 条从根节点到叶结点的路径,我们把每条路径上的这些权重相加作为这条路径的权重,则权重最大的路径就是我们想要的地图(或称为轨迹),因为这个路径上的所有结点总体的位姿最好。
2.重采样
重采样的频率决定着精度和效率。gmapping 的重采样策略如下。
inline bool GridSlamProcessor::resample(const double *plainReading, int adaptSize, const RangeReading *reading) {
// 2.重采样:权重分散性 m_neff < 0.5 * 粒子数量。
if (m_neff < m_resampleThreshold * m_particles.size()) {
// 重采样并增加轨迹结点。
} else {
// 仅增加轨迹结点。
}
}
具体的重采样方法如下。
// 调用者 GridSlamProcessor::resample。
/*Implementation of the above stuff*/
template <class Particle, class Numeric>
std::vector<unsigned int> uniform_resampler<Particle, Numeric>::resampleIndexes(const std::vector<Particle> &particles, // 归一化化权重。
int nparticles) const { // 采样后的粒子数量。
// 1.计算所有粒子的 acc 权重之和 cweight(值为 1) 和粒子数量 n。
// 第一个实参是 GridSlamProcessor::m_weights。它已被 GridSlamProcessor::normalize 归一化,即元素之和为 1。
Numeric cweight = 0;
// compute the cumulative weights
unsigned int n = 0;
for (typename std::vector<Particle>::const_iterator it = particles.begin(); it != particles.end(); ++it) {
cweight += (Numeric)*it;
n++;
}
// 采样后的粒子数量 n:如果设置了采样数量,就使用设置的值。
if (nparticles > 0)
n = nparticles;
// 2.把归一化权重之和 cweight 等分成 n 段,每段长度为 interval。从第一段随机选个起点 target,每隔 interval 设置一个 target。
// 2.1 计算段长和 target。
// compute the interval
Numeric interval = cweight / n;
// compute the initial target weight
Numeric target = interval * ::drand48(); // 随机数范围 [0, 1]。
// 2.2 每隔 interval 采样一次。
// compute the resampled indexes
cweight = 0;
// 采样后的样本在原粒子集合中的下标。
std::vector<unsigned int> indexes(n);
n = 0;
unsigned int i = 0;
for (typename std::vector<Particle>::const_iterator it = particles.begin(); it != particles.end(); ++it, ++i) {
cweight += (Numeric)*it;
while (cweight > target) {
indexes[n++] = i;
target += interval;
}
}
// 3.返回采样后的样本在原粒子集合中的下标。
return indexes;
}
我们把代码分为三部分。第一部分统计所有粒子的 acc 权重之和 cweight 和粒子数量 n。这一步是多余的,因为 cweight 已被 GridSlamProcessor::normalize 归一化为 1,而粒子数量是固定的。第三部分返回采样结果,即,采样后的粒子在原粒子集合中的下标。
第二部分,我们以粒子数量 n=8 为例说明。首先把长度 cweight=1 的线段等分成 n=8 段,如下图。每段长度 interval=1/8,然后在第一段 [0, 1/8] 内随机选择一个 target 点(图中的红点)。
接下来开始重采样。target 会从它被随机确定的起始位置开始,按固定值 interval=1/8 的步长向后走。gmapping 的重采样思想是优先选择权重大的粒子。以 target=0.3/8 为例,当 n=8 个粒子的权重分别为 [0.1/8, 0.4/8, 1.9/8, 0.3/8, 2.8/8, 0.2/8, 0.5/8, 1.8/8] 时,重采样过程如下表所示。
3.保存地图
n 个粒子会创建 n 个地图。保存地图时,选择最优粒子对应的轨迹结点。地图会以话题名 map 发布出去。map_server 包中的 map_saver 结点(map_saver.cpp) 订阅这个话题并保存地图。
void SlamGMapping::updateMap(const sensor_msgs::LaserScan &scan) {
GMapping::GridSlamProcessor::Particle best = gsp_->getParticles()[gsp_->getBestParticleIndex()];
for (GMapping::GridSlamProcessor::TNode *n = best.node; n; n = n->parent) {
}
sst_.publish(map_.map); // 以话题名 map 发布。
}
4.参考
- 论文。
- 源码,github。
- ros wiki。
- GMapping漫谈,知乎专栏,王金戈,2022。
- GMapping的基本原理,无处不在的小土,高乙超。
- 粒子滤波,CSDN,2023。
- 粒子滤波,B 站,2020。