gmapping代码走读

Gmapping　代码走读
主要流程

1. １.1、laserCallback()

功能描述

• 接受到激光雷达数据的回调函数 在这里面调用addScan()函数
• 如果addScan()函数调用成功，也就是说激光数据被成功的插入到地图中后，
• 如果到了地图更新的时间，则对地图进行更新，通过调用updateMap()函数来进行相应的操作。
• laserCallback() ->addScan() ->gmapping::processScan() ->updateMap()

####1.2、addScan()
功能描述

• 加入一个激光雷达的数据 主要函数 这里面会调用processScan()函数
• 这个函数被laserCallback()函数调用
  
  ####1.3、updateMap()
  功能描述
  更新地图:这里的更新地图，只是为了可视化。因为真正的地图都存储在粒子里面。
  这里会拿一个权值最大的粒子的地图发布出来.
  得到权值最大的粒子，然后遍历这个粒子的整个轨迹，根据轨迹上记录的信息来进行建图
  然后把得到的地图发布出去
  这个函数被laserCallback()调用，每次addScan()成功了，就会调用这个函数来生成地图，并发布出去

2.1 processScan()

功能描述
　在scanmatcherprocessor里面也有一个这样的函数 但是那个函数是没有使用的
@desc 处理一帧激光数据 这里是gmapping算法的主要函数。
在这个函数里面调用其他的函数，包括里程计预测、激光测量更新、粒子采样等等步骤。
主要步骤：
利用运动模型更新里程计分布
利用最近的一次观测来提高proposal分布。(scan-match)
利用proposal分布+激光雷达数据来确定各个粒子的权重
对粒子进行重采样

####2.2 drawFromMotion()
功能描述
里程计运动模型。把加入了噪声的值，加入到粒子估计的最优位置上，得到新的位置（根据运动模型推算出来的位置）。
####2.3　scanMatch()
功能描述
为每一个粒子都计算扫描匹配。扫描匹配即为在里程计的基础上，通过优化求得位姿
这个函数是最终被使用的函数，这个函数不但对每个位姿进行scan-match来优化，再优化之后，还会计算每个粒子的权重
这里的权重用似然表示。
主要步骤：
１、对每个粒子迭代计算最有位姿，返回粒子得分。采用optimize()计算
２、如果矫正成功则更新位姿
　　如果匹配不上，则采用历程数据。使用里程计数据无需进行更新，因为再扫描匹配前已经更新完成。
３、重新计算粒子的权重，相当于粒子滤波器中的观测步骤，计算p(z|x,m))，粒子的权重由粒子的似然来表示。采用likelihoodAndScore()计算
４、计算出来最优的位姿之后，进行地图的扩充。采用computeActiveArea计算

#####2.3.1 optimize()
功能描述
根据地图、激光数据、位姿迭代求解一个最优的新的位姿出来
主要步骤
１、计算当前位置的得分，调用score函数
２、do-while循环
2.1　如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像
2.2　把8个方向都搜索一次 得到这8个方向里面最好的一个位姿和对应的得分
2.3　返回最优位置和得分
#####2.3.2score()
功能描述
根据地图、机器人位置、激光雷达数据，计算出一个得分：原理为likelihood_field_range_finder_model
主要步骤

inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
{
    double s=0;
    const double * angle=m_laserAngles+m_initialBeamsSkip;
    OrientedPoint lp=p;
    /*
    把激光雷达的坐标转换到世界坐标系
    先旋转到机器人坐标系，然后再转换到世界坐标系
    p表示base_link在map中的坐标
    m_laserPose 表示base_laser在base_link坐标系中的坐标
    */
    lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
    lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
    lp.theta+=m_laserPose.theta;

    /*
     * map.getDelta表示地图分辨率 m_freeCellRatio = sqrt(2)
     * 意思是如果激光hit了某个点 那么沿着激光方向的freeDelta距离的地方要是空闲才可以
    */
    unsigned int skip=0;
    double freeDelta=map.getDelta()*m_freeCellRatio;


    //枚举所有的激光束
    for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
    {
        skip++;
        skip=skip>m_likelihoodSkip?0:skip;
        if (skip||*r>m_usableRange||*r==0.0) continue;

        /*被激光雷达击中的点 在地图坐标系中的坐标*/
        Point phit=lp;
        phit.x+=*r*cos(lp.theta+*angle);
        phit.y+=*r*sin(lp.theta+*angle);
        IntPoint iphit=map.world2map(phit);

        /*该束激光的最后一个空闲坐标，即紧贴hitCell的freeCell 原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的*/
        Point pfree=lp;
        //理论上来说 这个应该是一个bug。修改成下面的之后，改善不大。
        //		pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
        //		pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
        pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);
        pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);

        /*phit 和 freeCell的距离*/
        pfree=pfree-phit;
        IntPoint ipfree=map.world2map(pfree);

        /*在kernelSize大小的窗口中搜索出最优最可能被这个激光束击中的点 这个kernelSize在大小为1*/
        bool found=false;
        Point bestMu(0.,0.);
        for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
            for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)
            {
                /*根据已开始的关系 搜索phit的时候，也计算出来pfree*/
                IntPoint pr=iphit+IntPoint(xx,yy);
                IntPoint pf=pr+ipfree;

                /*得到各自对应的Cell*/
                const PointAccumulator& cell=map.cell(pr);
                const PointAccumulator& fcell=map.cell(pf);
                /*
            (double)cell返回的是该cell被占用的概率
            这束激光要合法必须要满足cell是被占用的，而fcell是空闲的
            原理为：假设phit是被激光击中的点，这样的话沿着激光方向的前面一个点必定的空闲的
            */
                if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
                {
                    /*计算出被击中的点与对应的cell的currentScore距离*/
                    Point mu=phit-cell.mean();
                    if (!found)
                    {
                        bestMu=mu;
                        found=true;
                    }
                    else
                    {
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
                    }
                }
            }
        /*socre的计算公式exp(-d^2 / sigma)) 这里的sigma表示方差 不是标准差*/
        if (found)
        {
            double tmp_score = exp(-1.0/m_gaussianSigma*bestMu*bestMu);
            s += tmp_score;
            //只在周围的9个栅格里面寻找，因此平方的意义不大。
            //s += tmp_score*tmp_score;
        }
    }
        return s;
}

#####2.3.3 likelihoodAndScore()
功能描述
根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model
计算出来的似然即为粒子的权重,这个函数被scanmatcher.cpp里面的optimize()调用
主要步骤

#####2.3.4 computeActiveArea()
功能描述
计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了。(这里只是计算出来栅格的位置，然后插入地图中,并不对数据进行更新)
这里计算的有效区域的坐标都是patch坐标，不是cell坐标
这个函数在正常的进行SLAM算法的过程中，使用了m_generateMap = false。这个时候不会为了空闲区域分配内存。当要生成可视化的地图的时候,m_generateMap = true。这个时候就会为空闲区域分配内存
主要步骤

####2.4 updateTreeWeights()
功能描述
更新权重，调用*normalize()*把粒子的权重归一化
主要功能为归一化粒子的权重，同时计算出neff

主要步骤

####2.5 resample()
功能描述
粒子滤波器重采样。
主要步骤　　　　　　
1.需要重采样，则所有保留下来的粒子的轨迹都加上一个新的节点，然后进行地图更新。
2.不需要冲采样，则所有的粒子的轨迹都加上一个新的节点，然后进行地图的更新
在重采样完毕之后，会调用registerScan函数来更新地图