my_gmapping_slam代码解析三

0、写在前面        

        Gmapping是一个基于2D激光雷达使用RBPF（Rao-Blackwellized Particle Filters）算法完成二维栅格地图构建的SLAM算法。

        优点：gmapping可以实时构建室内环境地图，在小场景中计算量少，且地图精度较高，对激光雷达扫描频率要求较低。

        缺点：随着环境的增大，构建地图所需的内存和计算量就会变得巨大，所以gmapping不适合大场景构图。一个直观的感受是，对于200x200米的范围，如果栅格分辨率是5cm，每个栅格占用一个字节内存，那么每个粒子携带的地图都要16M的内存，如果是100粒子就是1.6G内存。

0.1 SLAM问题概率描述

        首先，一个完整的SLAM问题是在给定传感器数据的情况下，同时进行机器人位姿和地图的估计的问题。然而，现实的情况是这样的，如果需要得到一个精确的位姿需要与地图进行匹配，如果需要得到一个好的地图需要有精确的位姿才能做到，显然这是一个相互矛盾的问题。

        概率描述如下式（3.1）：显然是一个条件联合概率分布：

         用在移动机器人从开机到t时刻一系列传感器测量数据z1:t（这里当然是/scan）以及一系列控制数据u1:t(这里认为是/odom)的条件下，同时对地图m、机器人轨迹状态x1:t进行的估计来描述SLAM问题。

0.2 SLAM问题分解

        FastSLAM算法独辟蹊径，采用RBPF方法，将SLAM算法分解成两个问题。一个是机器人定位问题，另一个是已知机器人位姿进行地图构建的问题。分解过程的公式推导如下式（3.2）：

        其中p(x1:t | u1:t, z1:t)表示估计机器人的轨迹，p(m|x1:t, z1:t) 表示在已知机器人轨迹和传感器观测数据情况下，进行地图构建的闭式计算。这样 SLAM 问题就分解成两个问题。其中已知机器人位姿的地图构建是个简单问题，所以机器人位姿的估计是一个重点问题。

0.3 机器人轨迹增量估计分解

        FastSLAM算法采用粒子滤波来估计机器人的位姿，并且为每一个粒子构建一个地图。所以，每一个粒子都包含了机器人的轨迹和对应的环境地图。 现在我们着重研究一下 p(x1:t | u1:t, z1:t) 估计机器人的轨迹 。 通过使用贝叶斯准则对 p(x1:t | u1:t, z1:t) 进行公式推导如式（3.3）所示 ：

         经过上面的公式推导，这里将机器人轨迹估计转化成一个增量估计的问题，用p(x1:t-1 | u1:t-1, z1:t-1) 表示上一时刻的机器人轨迹，用上一时刻的粒子群表示。每一个粒子都用运动学模型p(xt | xt-1, ut)进行状态传播，这样就得到每个粒子对应的预测轨迹 。

        对于每一个传播之后的粒子，用观测模型p(zt | xt)进行权重计算归一化处理，这样就得到该时刻的机器人轨迹。

        之后根据估计的轨迹以及观测数据进行地图构建。

        

 0.4 SLAM问题形象化描述

        根据上面的公式推导，我们必须整理出一个流程出来，只有这样我们才能更加清晰的理解它们。

        这里以一个粒子为例，每个粒子都携带这上一时刻的位姿、权重、地图。

        通过公式（3.3）我们可以看出根据上一时刻机器人轨迹通过里程计的状态传播之后，我们得到了该粒子的预测位姿。

        根据预测位姿在观测模型的作用下，我们得到了该粒子代表的当前机器人轨迹，也就是完成了该粒子的机器人位姿估计。

        通过公式（3.2）根据机器人轨迹结合观测数据，即可闭式得到该粒子代表的地图。

        这样每一个粒子都存储了一个机器人轨迹，以及一张环境地图。

        仔细理一理，是不是有点概念了。
 

 0.5 gmapping的改进

        从上一节的描述上面我们已经知道SLAM问题被分解成了两个问题，机器人轨迹估计问题和已知机器人轨迹的地图构建问题。FastSLAM算法中的机器人轨迹估计问题使用的粒子滤波方法。由于使用的是粒子滤波，将不可避免的带来两个问题。

        第一个问题，当环境大或者机器人里程计误差大时，需要更多的粒子才能得到较好的估计，这时将造成内存爆炸；

        第二个问题，粒子滤波避免不了使用重采样，以确保当前粒子的有效性，然而重采样带来的问题就是粒子耗散，粒子多样性的丢失。

        由于这两个问题出现，导致FastSLAM算法理论上可行实际上却不能实现。针对以上问题gmapping提出了两种针对性的解决方法。也就是说gmapping是基于FastSLAM算法将RBPF方法变成了现实。

0.5.1 降低粒子数量

        问题由来：每一个粒子都包含自己的栅格地图，对于一个稍微大一点的环境来说，每一个粒子都会占用比较大的内存。如果机器人里程计的误差比较大，即proposal分布（提议分布）跟实际分布相差较大，则需要较多的粒子才能比较好的表示机器人位姿的估计，这样将会造成内存爆炸。

        目的：通过降低粒子数量的方法大幅度缓解内存爆炸。

        分析：里程计的概率模型比较平滑，是一个比较大的范围，如果对整个范围采样将需要很多的粒子，如果能找到一个位姿优值，在其周围进行小范围采样，这样不就可以降低粒子数量了嘛。

        方法一：直接采用极大似然估计的方式，根据粒子的位姿的预测分布和与地图的匹配程度，通过扫描匹配找到粒子的最优位姿参数，就用该位姿参数，直接当做新粒子的位姿。如下图所示，从普通的proposal分布采样替换为用极大似然估计提升采样的质量。这也就是gmapping算法中的做法。    

0.5.2 缓解粒子耗散

        问题由来：gmapping算法采用粒子滤波算法对移动机器人轨迹进行估计，必然少不了粒子重采样的过程。随着采样次数的增加，会出现所有粒子都从权重最高的一个粒子复制而来，这样粒子的多样性就完全丧失了。

         目的：缓解粒子耗散

        分析：gmapping算法从原理上使用粒子滤波，不可避免的要进行粒子重采样，所以，我们只能从减少重采样的思路上走。

        方法：gmapping提出选择性重采样的方法，根据所有粒子自身的离散程度（也就是权重方差）来决定是否进行粒子重采样的操作。于是就出现了式（3.7），当Neff小于某个阈值，说明粒子差异性过大，进行重采样，否则，不进行重采样。

---

        gmapping的核心处理算法都在addscan函数调用的processscan()中，该函数对所有粒子都通过里程计运动模型，更新每一个粒子对象存储的地图坐标系下的激光雷达位姿。

        gmapping的函数调用关系如下图所示：

        我们按照顺序在processScan函数中出现的位置，进行代码解析：

// △△△△△△    SLAM核心代码    △△△△△△
bool GridSlamProcessor::processScan(const RangeReading& reading, int adaptParticles)
{
    // 得到当前激光雷达的里程计位姿
    OrientedPoint relPose = reading.getPose();

    // m_count表示这个函数被调用的次数
    if (!m_count_)      // 如果是第一次调用本函数
    {
        m_odoPose_ = relPose;       // 上一次的激光雷达里程计位姿 = 本次激光雷达里程计位姿
    }

    // 对于每一个粒子，都要通过里程计运动模型更新每一个粒子对象存储的地图坐标系下的激光雷达位姿
    int tmp_size = m_particles_.size();

    // 这里做一下说明，对于vector数组的遍历，对比了一下，release模式下几乎无差别，这里用最易读的方式
    for (int i=0; i<tmp_size; i ++)
    {
        // 对于每一个粒子，将从运动模型传播的激光雷达位姿存放到m_particles[i].pose （最新的地图坐标系下的激光雷达位姿）
        OrientedPoint& pose(m_particles_[i].pose);
        pose = m_motionModel_.drawFromMotion(m_particles_[i].pose, relPose, m_odoPose_);    // 这里一定要区分三个位姿
        // m_particles[i].pose:最新的地图坐标系下的激光雷达最优位姿  relPose:当前激光雷达里程计位姿   m_odoPose:表示上一次的激光雷达里程计位姿
    }

1、运动模型传播

        运动模型的推导网上有一篇介绍，但是讲的非常粗糙，原理简单，但与代码几乎无法对应理解，因为角度关系非常杂乱：

        gmapping中的motionmodel.cpp文件中，代码如下：

// 里程计运动模型
// 表示粒子估计的最优位置（激光雷达上一个时刻的最优位置）
// 表示里程计算出来的新的位置
// 表示里程计算出来的旧的位置（即上一个里程计的位置）
//    pose = m_motionModel_.drawFromMotion(m_particles_[i].pose, relPose, m_odoPose_);    // 这里一定要区分三个位姿
//    m_particles[i].pose：最新的地图坐标系下的激光雷达最优位姿  relPose：当前激光雷达里程计位姿   m_odoPose：表示上一次的激光雷达里程计位姿


// 运动模型推算地图坐标系下的激光雷达位姿，并不神秘，就是在三个变化量基础上加上高斯噪声，在与之前激光雷达位姿结合就算出来了，就是简单的加法运算

OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew,
                                          const OrientedPoint& pold) const
{
    double sxy = 0.3 * srr;       // srr我理解为两轮子 里程计的方差

    // 计算出 pnew 相对于 pold 走了多少距离
    // 这里的距离表达是相对于车身坐标系来说的
    OrientedPoint delta = absoluteDifference(pnew, pold);

    // 初始化一个位姿点
    OrientedPoint noisy_point(delta);

    // 走过的X轴方向的距离加入噪声
    noisy_point.x += sampleGaussian(srr* fabs(delta.x) + str* fabs(delta.theta) + sxy* fabs(delta.y));
    // 走过的Y轴方向的距离加入噪声
    noisy_point.y += sampleGaussian(srr* fabs(delta.y) + str* fabs(delta.theta) + sxy* fabs(delta.x));
    // 走过的Z轴方向的距离加入噪声
    noisy_point.theta += sampleGaussian(stt* fabs(delta.theta) + srt* sqrt(delta.x*delta.x + delta.y*delta.y));

    // 限制角度的范围  fmod()函数是对浮点型数据进行取模运算，就是计算noisy_point.theta/2*M_PI的余数。 因为要把角度差限定在[−π π]之间。
    noisy_point.theta = fmod(noisy_point.theta, 2*M_PI);
    if (noisy_point.theta > M_PI)
        noisy_point.theta -= 2*M_PI;

    // 把加入了噪声的位移增量值 加到粒子估计的最优的位置上 即得到新的位置（根据运动模型推算出来的位置）
    return absoluteSum(p, noisy_point);
}

gmapping流程如下：

 其中，我们可以看到：

1. 流程中2、3、4行，对应代码OrientedPoint delta = absoluteDifference(pnew, pold)，计算的是p1、p2两个位姿之间的差值，并转换到p1坐标系下;
2. 流程中5、6、7行，对应代码中的添加噪声的过程；
3. 流程中8、9、10、11行，对应代码中最后一个函数absolute(p, noisy_point)；        

        这里我试着画了很多图进行角度关系对应，想要理解代码中下面两个函数的角度关系是怎么出来的，但很多都对应不上：

// 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。  ?????????????????????????
// absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差 转换到 t-1时刻的车辆坐标系下
template <class T, class A>
orientedpoint<T,A> absoluteDifference(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2)
{
    orientedpoint<T,A> delta = p1 - p2;                         // 先求出平面坐标的差值
    delta.theta = atan2(sin(delta.theta), cos(delta.theta));    // 再求出角度差值
    double s = sin(p2.theta), c = cos(p2.theta);
    return orientedpoint<T, A> (c*delta.x + s*delta.y,
                               -s*delta.x + c*delta.y,
                                delta.theta);                   // 计算(p1-p2)的相对量在p1下的表示
}

下面这个absoluteSum()函数也很难理解：

// 两个位姿的和 p2表示增量。 该函数表示在p1的位姿上，加上一个p2的增量   ?????????????
template <class T, class A>
orientedpoint<T,A> absoluteSum(const orientedpoint<T,A>& p1, const orientedpoint<T,A>& p2)

{
    double s = sin(p1.theta), c = cos(p1.theta);
    return orientedpoint<T, A> (c*p2.x - s*p2.y,
                                s*p2.x + c*p2.y,
                                p2.theta) + p1;
}

         这两个函数的关系不是很好理解，我画了几张图也没有能理解过来。这里暂且把里程计运动模型部分理解为：

//    pose = m_motionModel_.drawFromMotion(m_particles_[i].pose, relPose, m_odoPose_);    // 这里一定要区分三个位姿
//    m_particles[i].pose：最新的地图坐标系下的激光雷达最优位姿  relPose：当前激光雷达里程计位姿   m_odoPose：表示上一次的激光雷达里程计位姿


// 运动模型推算地图坐标系下的激光雷达位姿，并不神秘，就是在三个变化量基础上加上高斯噪声，在与之前激光雷达位姿结合就算出来了，就是简单的加法运算

OrientedPoint MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew,
                                          const OrientedPoint& pold) const

         计算当前激光雷达里程计位姿relPose(pnew/p1)和上一次激光雷达里程计位姿m_odopose(pold/p2)的差值delta，这个差值delta在p2（或者pold或者m_odopose）帧下描述。

        在对这个位姿差值进行噪声添加后，使用absoluteSum()函数，将差值delta连同p(m_particles[i].pose:最新地图坐标系下的激光雷达最优位姿)坐标系一起转换到map坐标系下，得到最新的粒子在地图坐标系下的位姿。

2、scan_matcher

        在看scan_matcher函数前，再看一下processScan()函数的运行：

... ...

// 这里做一下说明，对于vector数组的遍历，对比了一下，release模式下几乎无差别，这里用最易读的方式
    for (int i=0; i<tmp_size; i ++)
    {
        // 对于每一个粒子，将从运动模型传播的激光雷达位姿存放到m_particles[i].pose （最新的地图坐标系下的激光雷达位姿）
        OrientedPoint& pose(m_particles_[i].pose);
        pose = m_motionModel_.drawFromMotion(m_particles_[i].pose, relPose, m_odoPose_);    // 这里一定要区分三个位姿
        // m_particles[i].pose:最新的地图坐标系下的激光雷达最优位姿  relPose:当前激光雷达里程计位姿   m_odoPose:表示上一次的激光雷达里程计位姿
    }

    /* 根据两次里程计的数据 计算出来激光雷达的线性位移和角度变化*/
    OrientedPoint move = relPose - m_odoPose_;
    Point temp(move.x, move.y);
    // 激光雷达在里程计作用下的累计移动线性距离和累计角度变换，用于判断是否，进行核心算法处理
    // 处理完后，m_linearDistance 和 m_angularDistance  清零
    m_linearDistance_  += sqrt(temp * temp);          // 两点之间距离公式
    m_angularDistance_ += fabs(move.theta);

    // 更新上一次的激光雷达的里程计位姿
    m_odoPose_ = relPose;
    // 做返回值用
    bool processed = false;
    // 自己修改的，这个比较随意
    last_update_time_ += 1.0;

... ...

        在计算出每个粒子的地图上的位置后，地图上的粒子可能是这样的：（注意这个是每个粒子在地图坐标系下的最优激光雷达位姿）

        上面的代码进行了里程计数据的计算，计算两帧激光雷达里程计在里程计坐标系下的位姿差值，同时累计了m_linearDistance_和m_angularDistance_，这两个值在后面会作为更核心算法调用的判据。

        

        在正式调用scan_matcher前，先进行了自定义传感器类reading的scan的距离range数据的拷贝，随后正式进行scan_matcher调用：

// 只有当激光雷达走过一定的距离 或者 旋转过一定的角度  或者  经一段指定的时间才处理激光数据  或者 处理第一帧数据
    if (!m_count_
            || m_linearDistance_  >= m_linearThresholdDistance
            || m_angularDistance_ >= m_angularThresholdDistance
            || (period_ >= 0.0 && (last_update_time_) >period_))
    {
        last_update_time_ = 0;

        // 拷贝reading的scan的range数据
        int beam_number = reading.getSize();
        double* plainReading = new double[beam_number];
        for (unsigned int i=0; i<beam_number; i ++)
        {
            plainReading[i] = reading.m_dists[i];
        }

        // reading_copy数据用来放在每一个节点，构建地图用
        RangeReading* reading_copy = new RangeReading(beam_number, &(reading.m_dists[0]), &(reading.m_angles[0]));

        // 如果不是第一帧数据
        if (m_count_ > 0)
        {
            // 功能：为每一个粒子，在当前激光雷达位姿下，当前帧激光数据下，求解每个粒子的位姿最优值（策略：爬山，评价机制：得分机制（激光雷达在当前里程计位姿下激光数据和当前地图的的匹配程度））
            //      求解每个粒子的权重
            // 爬山算法：每一层，从不同方向寻找一个得分最高的一个值，直到出现得分开始下降的地方
            // 得分算法：根据当前位姿下的一帧激光扫描点和地图的匹配程度，对每一个激光点做计算，累积得分，返回得分

            // 通过扫描匹配寻找每一个粒子的激光雷达在地图坐标系下的最优位姿，并且计算粒子得分
            scanMatch(plainReading);

        scan_match主要有两个功能：

        1. 为每个粒子的pose找到一个最优值；

        2. 为每个粒子更新权重&累计权重。        

        scanMatch()函数主要调用了两个函数，一个是scanmatcher.cpp文件中的m_matcher.optimize()优化函数，优化的是当前粒子在map坐标系中的位姿，通过optimize()函数可以获得最优位姿的最大匹配得分；

        另一个函数是m_matcher.likelihoodAndScore()函数，输入当前地图和当前最优位姿，遍历激光束累计似然，把累计似然当作该粒子的权重，误差越大，似然越大。需要说明的是，似然越大，代表权重越大。

将sanMatch()函数进行拆分，代码如下：

// GridSlamProcessor::processScan核心代码调用此函数
// 通过扫描匹配寻找每一个粒子的激光雷达在地图坐标系下的最优位姿，并且计算粒子得分
inline void GridSlamProcessor::scanMatch(const double* plainReading)
{
    // 每个粒子都要进行scan-match
    int particle_number = m_particles_.size();
    for (int i=0; i<particle_number; i ++)      // 遍历每一个粒子
    {
        OrientedPoint corrected;
        double score, l;
        // 爬山算法，score最优位姿的最大的匹配得分
        score = m_matcher.optimize(corrected, m_particles_[i].map, m_particles_[i].pose, plainReading);

        // 更新该粒子计算出的激光雷达最优位姿（地图坐标系）
        if (score > m_minimumScore)
        {
            m_particles_[i].pose = corrected;
        }

        可以看到，optimize()函数传入的参数是corrected（暂存优化完之后的粒子位姿），还有就是每个粒子的地图m_particles_[i].map和由运动模型推导出的地图坐标系下的粒子位姿m_particles_[i].pose，以及激光雷达的“观测”——当前帧激光雷达的距离值。

2.1 ScanMatcher::optimize()优化函数

        该函数使用爬山函数找到当前地图坐标系下的最优粒子位姿。但是仔细考虑，会衍生出两个问题，一个是如何找到地图坐标系下粒子最优位姿？二是如何判断当前找到的位姿是最优的？

        第一个问题，gmapping使用爬山算法（类似LM）进行解决，爬山算法是在地图坐标系下的当前位姿上进行上下左右左转、右转的变化，计算localScore，这里需要注意currentScore和currentPose的使用，对控制进行循环。

// 每个粒子调用爬山算法函数，optimize函数对位姿进行适当修改，分为四个方向和两个转向，就是为了得到一个得分更好的位姿，直到得分开始下降才认为，
// 此时的位姿点为：最优位姿点
// score是位姿更新的参考依据
double ScanMatcher::optimize(OrientedPoint& pnew,    const ScanMatcherMap& map,
                             const OrientedPoint& init, const double* readings) const
{
    double bestScore = -1;
    // 计算当前位置的得分
    OrientedPoint currentPose = init;
    // score函数求当前激光雷达位姿p，在当前地图的匹配得分，越匹配，位姿得分越高，说明位姿越准确
    double currentScore = score(map, currentPose, readings);            // 先计算一下得分score

    // 角度和线性位移步进增量大小
    double adelta = m_optAngularDelta, ldelta=m_optLinearDelta;

    // 减小搜索步长的次数
    unsigned int refinement = 0;

    // 搜索的方向:6个方向
    enum Move { Front, Back, Left, Right, TurnLeft, TurnRight, Done };
    // 就这样在运动模型的更新的激光雷达位姿基础之上，不断地朝不同的方向，一层一层地向上爬，同时进行位姿修改，直到位姿得分出现下降，或者迭代结束
    do
    {
        // 如果某一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长  这个策略跟LM 有点像
        if (bestScore >= currentScore)
        {
            refinement ++;  // 减少所有步长的次数 ++
            adelta *= .5;   // 减小搜索步长
            ldelta *= .5;
        }

        // 初始化bestScore，前面在运动模型的更新的激光雷达位姿基础上计算的得分
        bestScore = currentScore;
        OrientedPoint  bestLocalPose = currentPose;
        OrientedPoint  localPose = currentPose;

        // 把6个方向都搜索一次
        Move move = Front;
        do
        {
            // 注意currentPose在这一层循环中并没有改变
            localPose = currentPose;        // 每次重新进这个do while循环都会对localPose重新赋值，保证在一个点上进行前后左右左转和右转的位姿加减
            switch (move)
            {
                case Front:
                    localPose.x += ldelta;
                    move = Back;
                    break;
                case Back:
                    localPose.x -= ldelta;
                    move = Left;
                    break;
                case Left:
                    localPose.y += ldelta;
                    move = Right;
                    break;
                case Right:
                    localPose.y += ldelta;
                    move = TurnLeft;
                    break;
                case TurnLeft:
                    localPose.theta += adelta;
                    move = TurnRight;
                    break;
                case TurnRight:
                    localPose.theta -= adelta;
                    move = Done;
                    break;
                default:
                    ;
            }

            // 表示当前的位姿和初始位姿的区别  区别越大增益越小
            double odo_gain = 1;

            // 如果里程计比较可靠的话
            // 那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚
            // 角度差距越大，odo_gain越小，小于1也就产生了惩罚
            if (m_angularOdometryReliability > 0.)        // 里程计的角度可靠性 m_angularOdometryReliability=0.3，在ScanMatcher::ScanMatcher()中设定
            {
                double dth = init.theta - localPose.theta;
                dth = atan2(sin(dth), cos(dth));
                dth *= dth;
                odo_gain *= exp(-m_angularOdometryReliability*dth);
            }
            // 同样线性距离差距越大，odo_gain越小
            if (m_linearOdometryReliability > 0.)       // 里程计的长度可靠性
            {
                double dx = init.x - localPose.x;
                double dy = init.y - localPose.y;
                double drho = dx*dx + dy*dy;            // 搜索出的局部点 与 初始点
                odo_gain = exp(-m_linearOdometryReliability*drho);
            }
            // 计算得分 = 增益 * score
            double localScore = odo_gain * score(map, localPose, readings);

            /* 如果得分更好，则更新得分，和位姿 */
            if (localScore > currentScore)
            {
                currentScore  = localScore;
                bestLocalPose = localPose;
            }
        } while(move != Done);//结束条件：走完6步

        /* 把当前位置设置为目前最优的位置*/
        currentPose = bestLocalPose;
    } while (currentScore>bestScore || refinement<m_optRecursiveIterations);     //两者有一个满足条件，都不会结束循环
            // 分数还在上升，没有下降         没有到达最大的迭代次数

    // 举个例子，比如当前节点前后左右以及左转、右转都已经遍历完，此时需要计算暂时得分currentScore，并且将currentPose=bestLocalPose。
    // 假设当前位姿6个方向遍历完，右转TurnRight的情况下粒子有最优位姿，此时该点的最优位姿currentPose只能是右转后的位姿，
    // 但是分数currentScore没有更新到bestScore中，说明还需要以右转点为新的位姿，进行新一轮的优化。

    // 返回最优位置和得分
    pnew = currentPose;
    return bestScore;
}

2.2 score()函数

        

3、normalize()函数

        normaliza()函数的作用是计算重采样的neff(粒子离散程度)的判断值。

        归一化粒子权重：找到所有粒子中最大的权重，更新每个粒子的权重，累积所有粒子的权重之和，

        normalize()函数步骤如下：

1、计算增益gain，gain=1/粒子数num；

2、求所有粒子中的最大权重；

3、以最大权重为中心，计算高斯分布值作为粒子权重，并累计求和；

4、归一化权重，除以所有粒子权重之和，并计算归一化权重平方和。

5、m_neff求倒数，表示粒子离散程度。

代码如下：

// 归一化粒子权重：找到所有粒子中最大的权重，更新每个粒子的权重、累积所有粒子的权重之和，归一化每个粒子的权重、更新每个粒子权重，计算neff：粒子离散程度
inline void GridSlamProcessor::normalize()
{
    /*
     * 第一次计算
     *  it->weight = 0;                  粒子的权重，通过累积权重寻找最优粒子的中间变量
     *  m_weights通过公式计算都为1          为了计算粒子离散程度，而产生的中间变量
     *  归一化之后，m_weights 得到均分概率
     *  通过公式计算m_neff                 m_neff 用来描述粒子的离散程度
     * */
    double gain = 1. / (m_obsSigmaGain * m_particles_.size());    // m_obsSigmaGain默认为1

    /*求所有粒子中的最大的权重*/
    double lmax= -std::numeric_limits<double>::max();//返回编译器允许的 double 型数 最大值
    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<Particle>::iterator it=m_particles_.begin(); it!=m_particles_.end(); it++)
    {
        m_weights_.push_back(exp(gain*(it->weight - lmax)));
        wcum += m_weights_.back();
    }

    /*
        计算有效粒子数 和 归一化权重
        权重 = wi/w
        neff = 1 / w*w
    */
    // 归一化权重，除以所有粒子权重之和
    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./m_neff;
}

4、resample()重采样函数

5、computeMap()地图相关函数

        m_matcher.computeMap()函数的作用：

                1. 拓展地图大小，找到地图的有效区域

                2. 单位patch，申请内存；

                3. 更新每个栅格的内容。

6、updateMap(*scan)地图更新函数