gmapping

欢迎访问我的博客首页。

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$。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.参考

1. 论文。
2. 源码，github。
3. ros wiki。
4. GMapping漫谈，知乎专栏，王金戈，2022。
5. GMapping的基本原理，无处不在的小土，高乙超。
6. 粒子滤波，CSDN，2023。
7. 粒子滤波，B 站，2020。