GMapping的重采样过程

重采样是粒子滤波器得以成功应用的一个重要环节。它的存在，是为了消除早期SIS粒子滤波器的粒子退化问题。其基本思想是对赋予权重的粒子集合进行重新采样，从中取出权重较小的粒子，增加权重较大的粒子。重采样操作之后，所有的粒子权重都会被设置为一样的。虽然，这一操作成功地解决了粒子退化问题，但它带来了一种所谓的粒子匮乏地问题，随着迭代次数的增加，粒子的多样性在下降。

为了缓解重采样的粒子退化问题，GMapping采取了一种自适应的方式进行重采样。它定义了一个指标\(N_{eff}\)来评价粒子权重的相似度，只有在\(N_{eff}\)小于一个给定的阈值的时候才进行重采样。其背后的思想是，粒子的权重差异越小，得到的粒子则更接近于对目标分布的采样。

GMapping中所用的正是我们在粒子滤波器中介绍的重采样方法。本文分析GMapping的建图引擎的函数resample和GMapping的实现方式。

1. 函数resample

函数resample中实现了两个方面的功能：根据指标\(N_{eff}\)进行重采样，更新重采样后各个粒子的位姿和地图。下面是该函数的实现片段，它有三个参数。其中，数组plainReading是激光传感器的原始读数，用于更新粒子的地图；参数adaptSize是一个配置参数，用于指定重采样的粒子数量(这也是一种削弱粒子匮乏问题的常用手段，人们一般都会在重采样的时候多采样几个粒子，以提高粒子集合的多样性)；reading则是打了时间戳的传感器读数，其中还记录了采集数据时传感器的位姿。

        inline bool GridSlamProcessor::resample(const double* plainReading,
                                                int adaptSize,
                                                const RangeReading* reading) {

我们定义一个临时变量hasResampled用于记录是否满足重采样条件并进行了重采样，它将作为函数退出时的返回值。

            bool hasResampled = false;

并用容器oldGeneration记录上代的粒子状态。其数据类型的定义为typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;，这是一个C++标准库中的vector容器。而类型GridSlamProcessor::TNode是机器人运动轨迹树的节点类型，从轨迹树的根节点开始到每个叶子节点所描述的路径，就是一个粒子所记录的运动轨迹。

            TNodeVector oldGeneration;
            for (unsigned int i=0; i < m_particles.size(); i++)
                oldGeneration.push_back(m_particles[i].node);

建图引擎的成员变量m_neff记录了当前粒子的\(N_{eff}\)，它在函数updateTreeWeights中得到了更新。当该变量小于给定的阈值时，就进行重采样。

            if (m_neff < m_resampleThreshold * m_particles.size()) {

这里使用在particlefilter.h中定义的重采样器采样，其成员函数resampleIndexes返回的是一个std::vector<unsigned int>类型的数组，记录了采样后的样本在原始粒子集合中的索引。而函数onResampleUpdate()则是一个空的虚函数，为用户提供一种重采样的回调机制，与GMapping算法的实现没有关系。

                uniform_resampler<double, double> resampler;
                m_indexes = resampler.resampleIndexes(m_weights, adaptSize);
                onResampleUpdate();

局部变量temp是一个用于记录重采样后的粒子集合，最终它将被整合到建图引擎的粒子集合m_particles中。数组deletedParticles用于记录需要去除的粒子，变量j则是用于计算deletedParticles的辅助变量。

                ParticleVector temp;
                unsigned int j=0;
                std::vector<unsigned int> deletedParticles;

接下来，遍历选择的粒子索引。首先记录下所有不在选择索引中的粒子，它们将在后序的过程中从粒子集合中删除。

                for (unsigned int i = 0; i < m_indexes.size(); i++) {
                    while(j < m_indexes[i]) {
                        deletedParticles.push_back(j);
                        j++;
                    }
                    if (j==m_indexes[i])
                        j++;

然后，为各个粒子更新最新位姿和传感器数据，并记录在轨迹树中。

                    Particle & p=m_particles[m_indexes[i]];
                    TNode *oldNode = oldGeneration[m_indexes[i]];
                    TNode *node = new TNode(p.pose, 0, oldNode, 0);
                    node->reading = reading;

最后，将选取的样本记录在临时粒子集合temp中。

                    temp.push_back(p);
                    temp.back().node=node;
                    temp.back().previousIndex=m_indexes[i];
                }

在上述的for循环中，还遗漏了部分需要删除的粒子，下面将之补全。

                while(j < m_indexes.size()){
                    deletedParticles.push_back(j);
                    j++;
                }

并释放内存。

                for (unsigned int i = 0; i < deletedParticles.size(); i++){
                    delete m_particles[deletedParticles[i]].node;
                    m_particles[deletedParticles[i]].node=0;
                }

接下来为重采样后的粒子更新地图，并将它们记录到建图引擎中。并标记已经进行了重采样。

                m_particles.clear();
                for (ParticleVector::iterator it=temp.begin(); it!=temp.end(); it++){
                    it->setWeight(0);
                    m_matcher.invalidateActiveArea();
                    m_matcher.registerScan(it->map, it->pose, plainReading);
                    m_particles.push_back(*it);
                }
                hasResampled = true;

即使我们不需要进行重采样，也需要更新各个粒子的轨迹和地图。

            } else {
                int index=0;
                TNodeVector::iterator node_it=oldGeneration.begin();
                for (ParticleVector::iterator it = m_particles.begin(); it != m_particles.end(); it++) {
                    TNode* node = new TNode(it->pose, 0.0, *node_it, 0);
                    node->reading=reading;
                    it->node=node;

                    m_matcher.invalidateActiveArea();
                    m_matcher.registerScan(it->map, it->pose, plainReading);
                    it->previousIndex=index;
                    index++; node_it++;
                }
            }
            return hasResampled;
        }

2. GMapping的重采样实现

在上述对函数resample的分析，我们看到GMapping定义了一个模板类uniform_resampler<double, double>来实际进行重采样。实际上，GMapping企图提供一种通用的粒子滤波框架，但是并没有完全用起来，只有这个重采样操作得到了应用。

下面是该重采样器的定义，它定义了三个成员函数，并没有成员变量。其中resampleIndexes就是resample函数中所用的重采样函数，它返回的是采样的粒子索引；函数resample所实现的功能与resampleIndexes一样，只是返回的是重采样后的粒子；而neff则是用于计算粒子集合的权重相似度\(N_{eff}\)的。

template <class Particle, class Numeric>
struct uniform_resampler{
    std::vector<unsigned int> resampleIndexes(const std::vector<Particle> &particles,
                                              int nparticles=0) const;
    std::vector<Particle> resample(const std::vector<Particle> & particles,
                                   int nparticles=0) const;
    Numeric neff(const std::vector<Particle> & particles) const;
};

关于函数resampleIndexes和resample的重采样过程如右边的伪代码所示，这里不再详细解释源码。下面是neff的计算过程，它忠实的实现了公式\(N_{eff} = \frac{1}{\sum_{i = 1}^N \left(\tilde{w}^{(i)} \right)^2}\)。虽然有函数neff的存在，但是GMapping的源码也没有使用它，而实在更新粒子轨迹权重时得到了计算。

        template <class Particle, class Numeric>
        Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
            double cum=0;
            double sum=0;
            for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
                Numeric w=(Numeric)*it;
                cum+=w*w;
                sum+=w;
            }
            return sum*sum/cum;
        }

3. 完

本文中，流水账式的记录了GMapping的重采样过程。建图引擎的resample函数，不仅仅完成了根据粒子权重相似度进行重采样的操作，还更新了重采样后各个粒子的运动轨迹和地图。重采样操作有较为通用的重采样器uniform_resampler搞定，而粒子权重相似度的计算在更新粒子权重时完成了。