gmapping算法教程（5）------scanmatcher和gridfastslam头文件

今天，将讲解最后三个头文件。话不多说，直接开始。首先是gridlinetraversal.h，主要功能是判定空闲栅格。

1、gridlinetraversal.h

namespace GMapping 
{

    typedef struct 
    {
    int     num_points;  //网格点的数量
    IntPoint*  points;    //网格点的坐标
    } GridLineTraversalLine;  //

    struct GridLineTraversal 
    {
    inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ;  //网格线的遍历
    inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; //网格线的遍历的核心函数 ，跟上面这个函数除了函数名不一样，其他都一样，后续再看看是不是有什么区别


    };

    void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line )  //根据起点和终点，生成一条网格线
    {
    int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;  //incr1：横坐标的增量，incr2：纵坐标的增量，d：横坐标和纵坐标的差值，x，y：横坐标和纵坐标的值，xend，yend：横坐标和纵坐标的终点值，xdirflag，ydirflag：横坐标和纵坐标的方向标志。 dx
    int cnt = 0;    //cnt：网格线的点数。  初始化为0

    dx = abs(end.x-start.x); dy = abs(end.y-start.y);  //计算横坐标和纵坐标的差值
    
    if (dy <= dx)    //如果纵坐标的差值小于等于横坐标的差值
    {
        d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx);  //计算增量
        if (start.x > end.x)   //如果起点的横坐标大于终点的横坐标
        {
            x = end.x;  //则横坐标的值为终点的横坐标
            y = end.y;  //则纵坐标的值为终点的纵坐标
            ydirflag = (-1);  //纵坐标的方向标志为-1
            xend = start.x;  //横坐标的终点值为起点的横坐标
        } 
        else   //如果起点的横坐标小于终点的横坐标
        {
            x = start.x;   //则横坐标的值为起点的横坐标 
            y = start.y;   //则纵坐标的值为起点的纵坐标
            ydirflag = 1;  //纵坐标的方向标志为1
            xend = end.x;  //横坐标的终点值为终点的横坐标
        }
        line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标
        line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标
        cnt++;                  //网格线的点数加1
        if (((end.y - start.y) * ydirflag) > 0)  //如果纵坐标的差值乘以纵坐标的方向标志大于0
         
        {
            while (x < xend)   //如果横坐标的值小于横坐标的终点值
            {
                x++;           //横坐标的值加1
                if (d <0)    //如果横坐标的差值小于0
                {
                 d+=incr1;   //横坐标的差值加上增量
                } 
                else        //如果横坐标的差值大于等于0
                {
                 y++;     //纵坐标的值加1
                 d+=incr2; //横坐标的差值加上增量
                }
                line->points[cnt].x=x;   //将横坐标的值赋给网格线的点的横坐标
                line->points[cnt].y=y;   //将纵坐标的值赋给网格线的点的纵坐标
                cnt++;                   //网格线的点数加1
            }
        } 
        else   //如果纵坐标的差值乘以纵坐标的方向标志小于等于0
        {
           while (x < xend)   //如果横坐标的值小于横坐标的终点值（即横坐标的最大值）
            {
                x++;        //横坐标的值加1
                if (d <0)   //如果横坐标的差值小于0
                {
                 d+=incr1;  //横坐标的差值加上横坐标的增量
                } 
                else   //如果横坐标的差值大于等于0
                {
                 y--;   //纵坐标的值减1
                 d+=incr2;  //横坐标的差值加上纵坐标增量
                }
                line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标
                line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标
                cnt++;                  //网格线的点数加1
            }
        }		
    } 
    else   //如果纵坐标的差值大于横坐标的差值
    {
        d = 2*dx - dy;   //横坐标的差值减去纵坐标的差值  为什么是2*dx-dy？原理是因为横坐标的差值是2*dx，纵坐标的差值是dy，所以横坐标的差值减去纵坐标的差值就是2*dx-dy
        incr1 = 2*dx; incr2 = 2 * (dx - dy);  //横坐标的增量为2*dx，纵坐标的增量为2*(dx-dy)
        if (start.y > end.y)   //如果起点的纵坐标大于终点的纵坐标
        {
            y = end.y; x = end.x;  //则横坐标的值为终点的横坐标，纵坐标的值为终点的纵坐标
            yend = start.y;       //纵坐标的终点值为起点的纵坐标
            xdirflag = (-1);      //横坐标的方向标志为-1
        } 
        else //如果起点的纵坐标小于终点的纵坐标
        {
            y = start.y; x = start.x;  //则横坐标的值为起点的横坐标，纵坐标的值为起点的纵坐标
            yend = end.y;     //纵坐标的终点值为终点的纵坐标
            xdirflag = 1;     //横坐标的方向标志为1
        }
        line->points[cnt].x=x;   //将横坐标的值赋给网格线的点的横坐标
        line->points[cnt].y=y;   //将纵坐标的值赋给网格线的点的纵坐标
        cnt++;                   //网格线的点数加1
        if (((end.x - start.x) * xdirflag) > 0)   //如果横坐标的差值乘以横坐标的方向标志大于0
        {
            while (y < yend)    //如果纵坐标的值小于纵坐标的终点值（即纵坐标的最大值）
            {
                y++;    //纵坐标的值加1
                if (d <0) // 如果横坐标的差值小于0
                {
                 d+=incr1;  //横坐标的差值加上横坐标的增量
                } 
                else   //如果横坐标的差值大于等于0
                {
                    x++;   //横坐标的值加1
                    d+=incr2;  //横坐标的差值加上纵坐标的增量
                }
                line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标
                line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标
                cnt++;    //网格线的点数加1
            }
        } 
        else  //如果横坐标的差值乘以横坐标的方向标志小于0
         {
            while (y < yend)   //如果纵坐标的值小于纵坐标的终点值（即纵坐标的最大值）
            {
                y++;  //纵坐标的值加1
                if (d <0) //如果纵坐标的差值小于0
                {
                  d+=incr1;  //纵坐标的差值加上横坐标的增量
                } 
                else  //如果纵坐标的差值大于等于0
                {
                 x--;  //横坐标的值减1
                 d+=incr2;  //纵坐标的差值加上横坐标的增量
                }
                line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标
                line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标
                cnt++;                  //网格线的点数加1
            }
        }
    }
    line->num_points = cnt;  //将网格线的点数赋值给网格点的数量
    }

    void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) //网格线遍历
    {
    int i,j;     //循环变量
    int half;    //目前，不知道具体含义，只看做是一个临时变量
    IntPoint v;  //网格线的点
    gridLineCore( start, end, line );  //调用网格线核心函数
    if ( start.x!=line->points[0].x ||start.y!=line->points[0].y ) //如果起点的横坐标不等于网格线的第一个点的横坐标或者起点的纵坐标不等于网格线的第一个点的纵坐标
        {
        half = line->num_points/2;  //将网格线的点数除以2赋值给half
        for (i=0,j=line->num_points - 1;i<half; i++,j--)   //循环，（注意，不是遍历）i从0到half，j从网格线的点数减1到half
        {
        v = line->points[i];  //将网格线的第i个点赋值给v
        line->points[i] = line->points[j];  //把网格线的第j个点赋值给第i个点
        line->points[j] = v; //把v 点赋值给网格线的第j个点    我的理解是相当于交换函数swap
        }
    }
    }

};

2、scanmatcher.h

其次，是scanmatcher.h，功能是在预测机器人位姿的时候，参考激光传感器的扫描数据，以达到改进粒子滤波器中的建议分布的目的。有两个地方用到了这个类型的对象：在建图引擎gsp_中以成员变量存在的匹配器m_matcher， 在ROS封装中更新地图的函数中以局部变量存在的匹配器matcher。

namespace GMapping
 {

    class ScanMatcher{
    public:
        ScanMatcher();  //构造函数
        ~ScanMatcher();  //析构函数

        void setLaserParameters(unsigned int beams, double* angles);  //设置雷达参数
        void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );//设置匹配参数
        double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;//优化函数
        inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;//得分函数
        inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;//得分函数和概率函数

        void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);//计算激活区域
        void registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);//注册扫描

        static const double nullLikelihood;//空值概率
    private:

    protected:

        unsigned int  m_laserBeams;		//雷达激光束数量												
        double        m_laserAngles[LASER_MAXBEAMS];  //雷达激光束的角度								    
        IntPoint*     m_linePoints;    //网格线的点
        //以下是匹配参数，不具体细将
        PARAM_SET_GET(double, laserMaxRange, protected, public, public)					
        PARAM_SET_GET(double, usableRange, protected, public, public)					
        PARAM_SET_GET(double, gaussianSigma, protected, public, public)
        PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
        PARAM_SET_GET(int,    kernelSize, protected, public, public)
        PARAM_SET_GET(double, optAngularDelta, protected, public, public)				
        PARAM_SET_GET(double, optLinearDelta, protected, public, public)				
        PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)	
        PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
        PARAM_SET_GET(bool, generateMap, protected, public, public)
        PARAM_SET_GET(double, enlargeStep, protected, public, public)
        PARAM_SET_GET(double, fullnessThreshold, protected, public, public)				
        PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)	
        PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)		
        PARAM_SET_GET(double, freeCellRatio, protected, public, public)					
        PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)		
        
    };

    inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const//计算得分
    {
        double s=0;  //初始化得分为0
        const double * angle=m_laserAngles+m_initialBeamsSkip;  //设置激光束的角度为m_laserAngles的第m_initialBeamsSkip个角度

        OrientedPoint lp=p;  //设置lp为p

        unsigned int skip=0;  //初始化跳过的激光束数量为0
        double freeDelta=map.getDelta()*m_freeCellRatio;  //设置空闲网格的大小为网格的大小的m_freeCellRatio倍  m_freeCellRatio = sqrt(2.)

        for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++) //循环，r指向readings的第m_initialBeamsSkip个点，angle指向m_laserAngles的第m_initialBeamsSkip个角度
        {
            skip++;   //跳过激光束数量加1
            skip=skip>m_likelihoodSkip?0:skip;  //如果跳过的激光束数量大于m_likelihoodSkip，则跳过激光束数量置为0
            if (skip||*r>m_usableRange||*r==0.0) continue;//如果跳过激光束数量大于0或者*r大于m_usableRange或者*r等于0.0，则跳过此次循环

            Point phit=lp;   //设置phit为lp ， phit是激光束的位置
            phit.x+=*r*cos(lp.theta+*angle); //更新phit的x坐标，等于phit的x坐标加上激光束的角度*r*cos(lp.theta+*angle)
            phit.y+=*r*sin(lp.theta+*angle);  //更新phit的y坐标，等于phit的y坐标加上激光束的角度*r*sin(lp.theta+*angle)

            IntPoint iphit=map.world2map(phit);  //设置iphit为map.world2map(phit)，iphit是激光束的位置

            Point pfree=lp;  //设置pfree为lp ， pfree是空闲网格的位置  ，临时变量
            pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);  //更新pfree的x坐标，等于pfree的x坐标加上(*r - freeDelta)*cos(lp.theta+*angle)
            pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);  //更新pfree的y坐标，等于pfree的y坐标加上(*r - freeDelta)*sin(lp.theta+*angle)

            pfree=pfree-phit;  //设置pfree为pfree-phit，pfree是空闲网格的位置  我们需要的数据
            IntPoint ipfree=map.world2map(pfree);  //设置ipfree为map.world2map(pfree)，ipfree是空闲网格的位置

            bool found=false;  //设置found为false  found:是否找到空闲网格的位置
            Point bestMu(0.,0.);  //设置bestMu为(0.,0.)  bestMu:最佳的空闲网格的位置
            for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)  //遍历x方向空闲网格的位置
                for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)  //遍历y方向空闲网格的位置
                {
                    IntPoint pr=iphit+IntPoint(xx,yy);   //设置pr为iphit+IntPoint(xx,yy)，pr是空闲网格的位置
                    IntPoint pf=pr+ipfree;               //设置pf为pr+ipfree，pf是空闲网格的位置 为什么要加上ipfree呢？？？

                    const PointAccumulator& cell=map.cell(pr);    //设置cell为map.cell(pr)，cell是空闲网格的位置  这个数据有什么用？？？
                    const PointAccumulator& fcell=map.cell(pf);   //设置fcell为map.cell(pf)，fcell是空闲网格的位置

                    if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)  //m_fullnessThreshold是空闲网格的满度阈值，默认值为0.1；如果空闲网格的满度大于m_fullnessThreshold,则找到空闲网格的位置
                    {

                        Point mu=phit-cell.mean();  //cell.mean()是空闲网格的均值，设置mu为phit-cell.mean()，mu是空闲网格的位置
                        if (!found)  //如果没有找到空闲网格的位置
                        {
                            bestMu=mu;  //设置bestMu为mu,bestMu是空闲网格的位置
                            found=true;  //设置found为true
                        }
                        else//如果找到空闲网格的位置
                        {
                            bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;  //如果mu*mu小于bestMu*bestMu，则设置bestMu为mu，否则设置bestMu为bestMu
                        }
                    }
                }
            if (found)  //如果找到空闲网格的位置
            {
                double tmp_score = exp(-1.0/m_gaussianSigma*bestMu*bestMu);  //设置tmp_score为exp(-1.0/m_gaussianSigma*bestMu*bestMu)，tmp_score是空闲网格的得分
                s += tmp_score;            //设置s为s+tmp_score，s是空闲网格的得分                                
            }
        }
        return s;  //返回s，s是空闲网格的得分
    }



 /*
 函数likelihoodAndScore完成了评价地图、传感器数据、机器人位姿的匹配度，并计算了位姿的对数似然度。
 它有5个参数，其中参数s和l是两个输出参数，分别用于返回匹配度和对数似然度。 
 参数map，p，和readings评价的对象，分别描述了待匹配的地图、机器人位姿、激光传感器的数据。
 
 */
    inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const
    {
        using namespace std;
        l=0;        //初始化对数似然度为0
        s=0;        //初始化匹配度为0
        const double * angle=m_laserAngles+m_initialBeamsSkip;  //使用指针angle获取传感器各个扫描数据所对应的转角   扫描匹配器的成员变量m_laserAngles在建图引擎的初始化过程中就已经通过函数setLaserParameters进行了设定。 
                                                                 //m_initialBeamsSkip默认情况下为0，必要时也可以使用函数setMatchingParameters进行配置。
        
       
        OrientedPoint lp=p;      //创建一个临时变量lp记录考察位姿p   

        double noHit=nullLikelihood/(m_likelihoodSigma);  //没有匹配的似然概率值

        unsigned int skip=0;                // 用于对传感器数据筛选的计数器
        unsigned int c=0;                   // 匹配计数器
        double freeDelta=map.getDelta()*m_freeCellRatio;  // freeDelta用于确定空闲节点的参数，其值为地图的像素间隔的m_freeCellRatio倍。m_freeCellRatio目前只知道是double类型，不知道其具体值。
                                                          // getDelta()函数: double getDelta() const { return m_delta;} m_delta=delta,即位姿差（目前的理解，没搞懂怎么变成了地图的像素间隔）
        
        //接下来，我们遍历传感器的读数。有时可能因为传感器的数据比较多，为了提高计算速度，我们需要每个几个数据处理一次。这里用skip作为数据跳跃计数器，当达到跳跃步长m_likelihoodSkip时， 将之清零，并进行一次匹配操作。
        //在使用超声、激光等传感器测距的时候，一般都会认为传感器的数据是不可能超过一个固定值的，即传感器的量程。所以这里也用成员变量m_usableRange对超出量程的数据进行筛选。
        for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)  
        {
            skip++;
            skip=skip>m_likelihoodSkip?0:skip;
            if (*r>m_usableRange) continue;   //目前只知道m_usableRange是double类型，激光传感器数据的可用距离（即有效数据），不知道其具体值。 如果传感器的数据超过了量程，则跳过这个数据。
            if (skip) continue;               //如果skip不为0，则跳过这个数据。
          

          //然后根据扫描数据的距离读数和角度创建一个点phit，及其在地图中的索引点，该点在地图中应当是被占用的。（为什么是被占用的，原理目前不清楚）
            Point phit=lp;
            phit.x+=*r*cos(lp.theta+*angle);
            phit.y+=*r*sin(lp.theta+*angle);
            IntPoint iphit=map.world2map(phit);

         //接着，我们沿着扫描波束所指的方向构建一个空闲点。我们知道激光传感器测量的是扫描波束所指的最近障碍的距离，那么从波束的起点到测量值所在位置中间的这段区域就应当是空闲的。
         // 由于传感器的读数存在噪声，所以我们需要减去一个合适的值作为空闲区域的判定标准。
            Point pfree=lp;  //创建一个临时变量pfree记录空闲区域的起点
            pfree.x+=(*r - freeDelta)*cos(lp.theta+*angle);  //pfree的x坐标为lp的x坐标加上（*r - freeDelta）*cos(lp.theta+*angle)，即lp的x坐标加上*r-freeDelta*cos(lp.theta+*angle)。
            pfree.y+=(*r - freeDelta)*sin(lp.theta+*angle);  //pfree的y坐标为lp的y坐标加上（*r - freeDelta）*sin(lp.theta+*angle)，即lp的y坐标加上*r-freeDelta*sin(lp.theta+*angle)。
            pfree=pfree-phit;      //pfree的x坐标为pfree的x坐标减去phit的x坐标，
            IntPoint ipfree=map.world2map(pfree);  //把空闲区域的起点和终点转换成地图坐标系中的点。

            //下面我们定义两个临时变量。
            bool found=false;  //found用于记录在phit的一个邻域内有匹配点。
            Point bestMu(0.,0.);  //bestMu是一个二维向量用于记录匹配点到考察点之间的距离向量，该向量的长度越小说明两个点越接近，匹配度就越高。

            //我们在点phit的一个邻域内搜索匹配点。点pr到pf的连线理论上与点iphit到ipfree之间的连线平行。
            for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)  //搜索点phit的一个邻域内的匹配点。
                for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++)//目前只知道m_kerneSize是int类型，具体值不知道
                {
                    IntPoint pr=iphit+IntPoint(xx,yy);         //pr是点phit的一个邻域内的一个点。
                    IntPoint pf=pr+ipfree;                     //pf是空闲区域的起点。
                    const PointAccumulator& cell=map.cell(pr);  //获取点pr所在的格子的值。
                    const PointAccumulator& fcell=map.cell(pf);  //获取点pf所在的格子的值。
                  
                    //阈值m_fullnessThreshold是用于判定栅格被占用的阈值。当我们构建的占用点所对应的cell被占用，空闲点所对应的fcell是空闲的，我们认为匹配了一个点。
                    // 然后在变量found留下匹配记录，遍历了整个邻域之后，bestMu将记录下最佳匹配点到地图栅格中心的矢量差，该矢量的长度越小匹配程度就越高。
                    if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)  //m_fullnessThreshold=0.1  邻域的点大于阈值并且空闲区域的点小于阈值
                    {
                        Point mu=phit-cell.mean();  //计算点phit到栅格中心的矢量差。
                        if (!found)                 //如果没有找到匹配点，那么我们记录下这个矢量差
                        {
                            bestMu=mu;              //记录下矢量差
                            found=true;             //记录下已经找到匹配点
                        }
                        else
                        {
                            bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;  //如果已经找到匹配点，那么我们计算出两个矢量差的平方和，如果矢量差的平方和小于之前记录的平方和，那么我们记录下这个矢量差。
                        }
                    }
                }

             //如果找到匹配点，我们就累加点的匹配度，当考察了所有的扫描数据点之后，累加变量s就记录了当前激光扫描的匹配度，c则对匹配点进行累加
                if (found)   
                
                {
                    s+=exp(-1./m_gaussianSigma*bestMu*bestMu);  //s是匹配度，bestMu是矢量差，m_gaussianSigma是高斯核的标准差。  累加匹配度。
                    c++;    //累加匹配点的数量。   c表示匹配计数器
                }

             //如果不是跳过点，我们就在一个高斯分布的假设下计算该点的对数似然度，对所有匹配点的累加值l将用于更新粒子的权重。在函数的最后返回匹配点数量。
                if (!skip)  
                
                {
                    double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);  //f是对数似然度，bestMu是矢量差，m_likelihoodSigma是对数似然度的标准差。
                    l+=(found)?f:noHit;    //如果找到匹配点，我们就累加对数似然度，如果没有找到匹配点，我们就累加noHit。
            }
        }
        return c;  //返回匹配点数量。
    }

};

3、gridslamprocessor.h

最后，是我们最重要的gridslamprocessor.h。

namespace GMapping
 {

    class GridSlamProcessor
    {
        public:


        //轨迹树结构体
        struct TNode
        {
            TNode(const OrientedPoint& pose, TNode* parent=0);//构造轨迹树的node,参数包括机器人坐标pose，以及父节点parent
            ~TNode();  //析构函数，释放父节点parent,如果一个有父节点被删除了，并且这个父母只有这一个节点，那么父母节点也删除，因为在策略树种们不会再有任何能抵达父母的可能性。
                

            OrientedPoint pose;   //机器人的坐标
            const RangeReading* reading;  // 读取的距离数据  这个是一个指针，指向一个距离数据  
            TNode* parent;        //父节点

        };
        
        typedef std::vector<GridSlamProcessor::TNode*> TNodeVector;//在粒子滤波结构体和节点结构体中定义一个新的类型TNodeVector，用来存储节点的指针


        //粒子结构体    定义滤波器的粒子，每一个粒子有一个地图，一个位姿，一个权重，一个节点，一个权重总和
        struct Particle
        {
            Particle(const ScanMatcherMap& map); //构造一个粒子，以扫描匹配地图的引用map作为参数

            inline operator double() const {return weight;}  //重载操作符double()，返回值为权重
            inline operator OrientedPoint() const {return pose;}  //重载操作符，返回值为粒子的位姿

            inline void setWeight(double w) {weight = w;}  //设置粒子权重，目前不懂这个函数的作用     现在懂了，在采样粒子后，需要设置粒子的权重为0

            //下面是结构体的具体参数
            //第一部分：
            ScanMatcherMap map;    // 定义栅格地图的成员对象map
            OrientedPoint pose;    //定义粒子的位姿，这个位姿是机器人的位姿，而不是地图的位姿
                                //上面两个参数是整个粒子滤波迭代器过程中更新的数据对象
            //第二部分：
            double weight;       //粒子的权重
            double weightSum;    //粒子的权重总和

            TNode* node; //指向轨迹树中的一个叶子节点，表示当前粒子的最新的姿态
                        //通过该指针所指的TNode对象，我们可以追述到轨迹树的根节点上， 
                        //期间所经历的节点就是当前粒子所记录的一种可能的运动轨迹。
        };

        typedef std::vector<Particle> ParticleVector;

        GridSlamProcessor();  //构造函数，初始化使用

        virtual ~GridSlamProcessor();
        //设置匹配参数
        void setMatchingParameters(double urange, double range, double sigma, int kernsize, 
                                    double lopt, double aopt, int iterations, double likelihoodSigma=1, 
                                    double likelihoodGain=1, unsigned int likelihoodSkip=0);
        //设置运动模型参数
        void setMotionModelParameters(double srr, double srt, double str, double stt);
        //设置更新距离
        void setUpdateDistances(double linear, double angular, double resampleThreshold);
        //设置更新周期
        void setUpdatePeriod(double p) {period_=p;}
        //初始化
        void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, 
                    OrientedPoint initialPose=OrientedPoint(0,0,0));
        //处理processedScan，返回处理后的scan
        bool processScan(const RangeReading & reading, int adaptParticles=0);

        inline const ParticleVector& getParticles() const {return m_particles; }  //返回粒子的集合

        inline const std::vector<unsigned int>& getIndexes() const{return m_indexes; }  //返回粒子的索引

        int getBestParticleIndex() const;  //返回最好的粒子的索引



        //scanmatcher算法的具体实现
        ScanMatcher m_matcher;  //定义一个扫描匹配类的对象m_matcher

        MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);  //定义一个参数，用来设置扫描的最大距离，默认值为10m
        MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);    //定义一个参数，用来设置扫描的可用距离，默认值为5m
        MEMBER_PARAM_SET_GET(m_matcher, double, gaussianSigma, protected, public, public);  //定义一个参数，用来设置扫描的高斯分布的标准差，默认值为0.1m
        MEMBER_PARAM_SET_GET(m_matcher, double, likelihoodSigma, protected, public, public);  //定义一个参数，用来设置扫描的概率分布的标准差，默认值为1m
        MEMBER_PARAM_SET_GET(m_matcher, int,    kernelSize, protected, public, public);     //定义一个参数，用来设置扫描的高斯核的大小，默认值为5
        MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);  //定义一个参数，用来设置扫描的最大角度，默认值为0.1rad
        MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);   //定义一个参数，用来设置扫描的最大距离，默认值为0.1m
        MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);  //定义一个参数，用来设置扫描的递归迭代次数，默认值为10
        MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);  //定义一个参数，用来设置扫描的概率分布的跳过次数，默认值为0
        MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);    //定义一个参数，用来设置是否生成地图，默认值为false
        MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);    //定义一个参数，用来设置是否放大步长，默认值为false

        STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);     //定义一个参数，用来设置运动模型的srr，默认值为0.1
        STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);     //定义一个参数，用来设置运动模型的srt，默认值为0.1
        STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);      //定义一个参数，用来设置运动模型的str，默认值为0.1
        STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);       //定义一个参数，用来设置运动模型的stt，默认值为0.1

        PARAM_SET_GET(double, minimumScore, protected, public, public);     //定义一个参数，用来设置最小得分，默认值为0.1

        protected:
        double                    last_update_time_;   //上一次更新的时间
        double                    period_;             //更新周期
        unsigned int              m_beams;              //激光雷达的激光数量

        ParticleVector            m_particles;         //粒子的集合

        std::vector<unsigned int> m_indexes;            //粒子的索引

        std::vector<double>       m_weights;           //粒子的权重

        MotionModel               m_motionModel;        //运动模型参数
        
        OrientedPoint             m_odoPose;            //里程计的位姿

        OrientedPoint             m_pose;              //机器人的位姿

        int                       m_count;            //计数

        double                    m_linearDistance;    //平移距离

        double                    m_angularDistance;   //旋转角度  

        PARAM_GET(double, xmin, protected, public);    //x最小值
        PARAM_GET(double, ymin, protected, public);    //y最小值
        PARAM_GET(double, xmax, protected, public);    //x最大值
        PARAM_GET(double, ymax, protected, public);    //y最大值
        PARAM_GET(double, delta, protected, public);    //网格大小

        PARAM_GET(double, neff, protected, public);     //有效粒子数量

        PARAM_SET_GET(double, linearThresholdDistance, protected, public, public);  //平移距离阈值
        PARAM_SET_GET(double, angularThresholdDistance, protected, public, public);  //旋转角度阈值
        
        PARAM_SET_GET(double, obsSigmaGain, protected, public, public);    //观测噪声的增益
        PARAM_SET_GET(double, resampleThreshold, protected, public, public);  //重采样阈值

        private:

        inline void scanMatch(const double *plainReading);  

        inline void normalize();

        inline bool resample(const double* plainReading, int adaptParticles, const RangeReading* rr=0);

    };


    //scanMatch()函数采用NDT算法对当前坐标的激光束和每个粒子各自维护的map进行匹配，对粒子坐标进行前后左右左转右转微调，再给出匹配得分
    //以传感器的数据作为输入，在函数体中遍历建图引擎的粒子集合
    inline void GridSlamProcessor::scanMatch(const double* plainReading)  //
    {
        int particle_number = m_particles.size();  //粒子数量
        for (int i = 0; i < particle_number;i++)   //遍历粒子集合
        {
            OrientedPoint corrected;  //存储粒子坐标的前后左右左转右转微调后的坐标
            double score, l, s;       //存储匹配得分和左右左转右转微调后的坐标

            /*
            匹配器的函数optimize是一种爬山算法,用于寻找x^(i)t=argmaxxp(x|m(i)t−1,zt,x′(i)t)，完成对建议分布的采样
            其中corrected是一个引用方式的传参，该函数最终退出之后，corrected所记录的则是x^(i)t。 
            此外，该函数返回的是一个匹配度，记录在局部变量score中。建图引擎的成员变量m_minimumScore是一个匹配度阈值，当超过该阈值时，我们认为匹配成功，使用x^(i)t作为更新样本。 
            否则，我们仍然使用传统的运动模型下的预测状态作为更新样本

            */
            score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);  //这里的pose是粒子的位姿，map是粒子的地图，plainReading是传感器的数据

            if (score > m_minimumScore)        //如果匹配度大于阈值，则更新粒子的位姿
            {
            m_particles[i].pose = corrected;     //更新粒子的位姿
            }
         //完成了对建议分布的采样之后，我们需要计算各个样本的权重。函数likelihoodAndScore计算了更新样本状态的对数似然度，可以通过累加的形式完成样本权重的更新。
            m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);  //这里的pose是粒子的位姿，map是粒子的地图，plainReading是传感器的数据
            m_particles[i].weight+=l;       //更新粒子的权重
            m_particles[i].weightSum+=l;    //更新粒子的权重和
        }
    }


    //normalize()函数用于对粒子的权重进行归一化，使得权重之和为1      -----------------normalize()函数
    inline void GridSlamProcessor::normalize()   
    {
     //gain是一个归一化的系数，用于计算粒子的权重   这里的1.表示1.0,虽然只少了一个0,但是对于新手来说很不友好，可能会陷入./是个什么东西的弥天大谎中，得不偿失啊！
    double gain=1./(m_obsSigmaGain*m_particles.size());  // m_obsSigmaGain是传感器的误差，默认值为1；m_particles.size()是粒子数量
    

    //  lmax是一个最大的对数似然度，用于计算粒子的权重
    /*
    numeric_limits<double>::max ()是函数，返回编译器允许的 double 型数 最大值。
    类似的 numeric_limits<int>::max () 返回 编译器允许的 int 型数 最大值。
    需包含头文件 #include <limits>
    
    */
    double lmax= -std::numeric_limits<double>::max(); //-std::numeric_limits<double>::max()是一个最大的对数似然度，用于计算粒子的权重

    for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++)   //遍历粒子集合
    {
        //计算最大的对数似然度
        lmax=it->weight>lmax?it->weight:lmax;   //   功能：如果粒子的权重大于lmax，则更新lmax
    }

    m_weights.clear();    //清空权重集合   为下面权重累加做准备
    double wcum=0;        //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();                               //累加粒子的权重
    }

    m_neff=0;     //neff是一个权重的有效数量，用于计算粒子的权重  ，这是只是初始化为0，后面会被更新
    for (std::vector<double>::iterator it=m_weights.begin(); it!=m_weights.end(); it++)  //遍历权重集合
    {
        *it=*it/wcum;    //计算粒子的权重
        double w=*it;    //用w记录上一步计算的权重
        m_neff+=w*w;     //累加权重的平方
    }
    m_neff=1./m_neff;   // m_neff是一个权重的有效数量，用于计算粒子的权重相似度  1.表示1.0,虽然只少了一个0,但是对于新手来说很不友好，可能会陷入./是个什么东西的弥天大谎中，得不偿失啊！
    }






 //resample()函数用于重新分配粒子的位置，使得粒子的权重和为1，并且更新粒子的位置。  --------------------resample()函数 
 /*
 函数resample中实现了两个方面的功能：根据指标Neff进行重采样，更新重采样后各个粒子的位姿和地图。下面是该函数的实现片段，它有三个参数。其中， 数组plainReading是激光传感器的原始读数，用于更新粒子的地图；
 参数adaptSize是一个配置参数，用于指定重采样的粒子数量(这也是一种削弱粒子匮乏问题的常用手段， 人们一般都会在重采样的时候多采样几个粒子，以提高粒子集合的多样性)；
 reading则是打了时间戳的传感器读数，其中还记录了采集数据时传感器的位姿。
 */
    inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading)  
   {
        bool hasResampled = false;   //hasResampled是一个标志，用于标记是否采样成功
        
        TNodeVector oldGeneration;   //oldGeneration是一个粒子集合，用于存储旧的粒子
        for (unsigned int i=0; i<m_particles.size(); i++)  //遍历粒子集合
            {
                oldGeneration.push_back(m_particles[i].node);  //将粒子的节点存储到oldGeneration中
            }
        
        if (m_neff < m_resampleThreshold*m_particles.size())   //如果权重的有效数量小于采样阈值的值，则采样
                {	
                    /*
                    
                    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;  // 
                    };
                    
                    
                    */
                    
                    uniform_resampler<double, double> resampler;      //创建一个均匀采样器  ,采样器模板类uniform_resampler，采样器类型double，权重类型double  （模板如上）

                    //std::vector<unsigned int> m_indexes;
                    m_indexes=resampler.resampleIndexes(m_weights, adaptSize);   //计算采样的索引   resampleIndexes()函数用于计算采样的索引，参数是权重集合和采样的数量；是一个模板函数，返回值是一个索引集合

                    ParticleVector temp;      //创建一个粒子集合，用于存储采样的粒子

                    for (unsigned int i=0; i<m_indexes.size(); i++)   //遍历采样的粒子
                    {
                        Particle & p=m_particles[m_indexes[i]];    //获取采样的粒子

                        TNode* node=0;       //node是一个节点，用于存储采样的粒子的节点  ，初始化为0
                        TNode* oldNode=oldGeneration[m_indexes[i]];   //oldNode是一个节点，用于存储旧的粒子的节点 

                        node=new TNode(p.pose, oldNode);   //创建一个新的节点，并将粒子的位姿和旧的节点传入
                        node->reading=reading;     //将读取的距离传入节点

                        temp.push_back(p);    //将采样的粒子存储到temp中
                        temp.back().node=node;  //  temp.back().node是一个节点，用于存储采样的粒子的节点
                    }

                    m_particles.clear();   //清空粒子集合

                    int tmp_size = temp.size();  //tmp_size是一个整数，用于存储temp的大小
                    for(int i = 0; i<tmp_size;i++)  //遍历采样的粒子，即存储到temp中的粒子
                    {
                        temp[i].setWeight(0);  //将temp的权重设置为0

                        m_matcher.computeActiveArea(temp[i].map,temp[i].pose,plainReading);  //计算激活区域  computeActiveArea()函数用于计算激活区域，参数是地图，位姿，读取的距离
                        m_matcher.registerScan(temp[i].map,temp[i].pose,plainReading);   //注册扫描   registerScan()函数用于注册扫描，参数是地图，位姿，读取的距离

                        m_particles.push_back(temp[i]);     //将采样的粒子存储到粒子集合中
                    }

                    hasResampled = true;   //标记采样成功  ，  最开始这个标志是false，这里被设置为true
                } 
        
        else //如果权重的有效数量大于采样阈值，则不采样

        {
            int particle_size = m_particles.size();  //particle_size是一个整数，用于存储粒子的大小
        
            for(int i = 0; i < particle_size;i++)    //遍历粒子集合（没有采样前的粒子）
            {
            TNode* node = 0;         //node是一个节点，用于存储粒子的节点  ，初始化为0
            node = new TNode(m_particles[i].pose,oldGeneration[i]);  //创建一个新的节点，并将粒子的位姿和旧的节点传入

            node->reading = reading;    //将读取的距离传入节点
            m_particles[i].node = node;  //将节点储存到粒子中
            m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);  //计算激活区域
            m_matcher.registerScan(m_particles[i].map, m_particles[i].pose, plainReading);   //注册扫描
            }

        }
        return hasResampled;  //返回采样是否成功
    }
};

4、NEXT

接下来，将讲解具体的函数实现及涉及的原理知识，希望你认真看完前面的内容讲解，使你更加轻松理解后面的内容，尤其是对一些变量的理解。好啦，今天的内容就是这样啦。下期再会。