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：横坐标和纵坐标的方向标志。 dxint cnt = 0;    //cnt：网格线的点数。  初始化为0dx = 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);  //纵坐标的方向标志为-1xend = start.x;  //横坐标的终点值为起点的横坐标} else   //如果起点的横坐标小于终点的横坐标{x = start.x;   //则横坐标的值为起点的横坐标 y = start.y;   //则纵坐标的值为起点的纵坐标ydirflag = 1;  //纵坐标的方向标志为1xend = end.x;  //横坐标的终点值为终点的横坐标}line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标cnt++;                  //网格线的点数加1if (((end.y - start.y) * ydirflag) > 0)  //如果纵坐标的差值乘以纵坐标的方向标志大于0{while (x < xend)   //如果横坐标的值小于横坐标的终点值{x++;           //横坐标的值加1if (d <0)    //如果横坐标的差值小于0{d+=incr1;   //横坐标的差值加上增量} else        //如果横坐标的差值大于等于0{y++;     //纵坐标的值加1d+=incr2; //横坐标的差值加上增量}line->points[cnt].x=x;   //将横坐标的值赋给网格线的点的横坐标line->points[cnt].y=y;   //将纵坐标的值赋给网格线的点的纵坐标cnt++;                   //网格线的点数加1}} else   //如果纵坐标的差值乘以纵坐标的方向标志小于等于0{while (x < xend)   //如果横坐标的值小于横坐标的终点值（即横坐标的最大值）{x++;        //横坐标的值加1if (d <0)   //如果横坐标的差值小于0{d+=incr1;  //横坐标的差值加上横坐标的增量} else   //如果横坐标的差值大于等于0{y--;   //纵坐标的值减1d+=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-dyincr1 = 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++;                   //网格线的点数加1if (((end.x - start.x) * xdirflag) > 0)   //如果横坐标的差值乘以横坐标的方向标志大于0{while (y < yend)    //如果纵坐标的值小于纵坐标的终点值（即纵坐标的最大值）{y++;    //纵坐标的值加1if (d <0) // 如果横坐标的差值小于0{d+=incr1;  //横坐标的差值加上横坐标的增量} else   //如果横坐标的差值大于等于0{x++;   //横坐标的值加1d+=incr2;  //横坐标的差值加上纵坐标的增量}line->points[cnt].x=x;  //将横坐标的值赋给网格线的点的横坐标line->points[cnt].y=y;  //将纵坐标的值赋给网格线的点的纵坐标cnt++;    //网格线的点数加1}} else  //如果横坐标的差值乘以横坐标的方向标志小于0{while (y < yend)   //如果纵坐标的值小于纵坐标的终点值（即纵坐标的最大值）{y++;  //纵坐标的值加1if (d <0) //如果纵坐标的差值小于0{d+=incr1;  //纵坐标的差值加上横坐标的增量} else  //如果纵坐标的差值大于等于0{x--;  //横坐标的值减1d+=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赋值给halffor (i=0,j=line->num_points - 1;i<half; i++,j--)   //循环，（注意，不是遍历）i从0到half，j从网格线的点数减1到half{v = line->points[i];  //将网格线的第i个点赋值给vline->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;  //初始化得分为0const double * angle=m_laserAngles+m_initialBeamsSkip;  //设置激光束的角度为m_laserAngles的第m_initialBeamsSkip个角度OrientedPoint lp=p;  //设置lp为punsigned int skip=0;  //初始化跳过的激光束数量为0double 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++;   //跳过激光束数量加1skip=skip>m_likelihoodSkip?0:skip;  //如果跳过的激光束数量大于m_likelihoodSkip，则跳过激光束数量置为0if (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;        //初始化对数似然度为0s=0;        //初始化匹配度为0const 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;    // 定义栅格地图的成员对象mapOrientedPoint 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，返回处理后的scanbool 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_matcherMEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public);  //定义一个参数，用来设置扫描的最大距离，默认值为10mMEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public);    //定义一个参数，用来设置扫描的可用距离，默认值为5mMEMBER_PARAM_SET_GET(m_matcher, double, gaussianSigma, protected, public, public);  //定义一个参数，用来设置扫描的高斯分布的标准差，默认值为0.1mMEMBER_PARAM_SET_GET(m_matcher, double, likelihoodSigma, protected, public, public);  //定义一个参数，用来设置扫描的概率分布的标准差，默认值为1mMEMBER_PARAM_SET_GET(m_matcher, int,    kernelSize, protected, public, public);     //定义一个参数，用来设置扫描的高斯核的大小，默认值为5MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public);  //定义一个参数，用来设置扫描的最大角度，默认值为0.1radMEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public);   //定义一个参数，用来设置扫描的最大距离，默认值为0.1mMEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public);  //定义一个参数，用来设置扫描的递归迭代次数，默认值为10MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public);  //定义一个参数，用来设置扫描的概率分布的跳过次数，默认值为0MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public);    //定义一个参数，用来设置是否生成地图，默认值为falseMEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public);    //定义一个参数，用来设置是否放大步长，默认值为falseSTRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public);     //定义一个参数，用来设置运动模型的srr，默认值为0.1STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public);     //定义一个参数，用来设置运动模型的srt，默认值为0.1STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public);      //定义一个参数，用来设置运动模型的str，默认值为0.1STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public);       //定义一个参数，用来设置运动模型的stt，默认值为0.1PARAM_SET_GET(double, minimumScore, protected, public, public);     //定义一个参数，用来设置最小得分，默认值为0.1protected: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是一个权重累加值，用于计算粒子的权重  初始化为0for (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是一个节点，用于存储采样的粒子的节点  ，初始化为0TNode* 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的权重设置为0m_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是一个节点，用于存储粒子的节点  ，初始化为0node = 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

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

gmapping算法教程（5）------scanmatcher和gridfastslam头文件相关推荐

macOS使用C/C++万能头文件保姆级教程
macOS使用C/C++万能头文件保姆级教程 Windows上面用万能头文件为我们省去了不少记头文件的麻烦,切换到macOS上来发现C/C++环境中没有自带这个头文件.不行,今天无论如何都要用到万能头 ...
C++：include：理解 C++ 中的头文件和源文件的作用
关于头文件和源文件我们主要围绕: C++编译模式, 声明和定义区别, 符号只能被定义一次, 符号被定义在多个源文件,但是一个源文件只能定义一次这四个方面来分析论述 1:C++ 编译模式在一个C++ ...
C++库文件和头文件编写教程
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达「本文介绍了在Linux系统下生成库文件,以及编写头文件来使用该库 ...
一起自学SLAM算法：8.1 Gmapping算法
连载文章,长期更新,欢迎关注: 写在前面第1章-ROS入门必备知识第2章-C++编程范式第3章-OpenCV图像处理第4章-机器人传感器第5章-机器人主机第6章-机器人底盘第7章-SLA ...
gmapping 算法流程整理
slam_gmapping 是gmapping的基于ros的壳,我们先从壳入手 *主要是学习曾书格老师注释的代码,如果需要的可以自取.2022年05月30日更新链接链接: https://pan. ...
VS2019中在源文件中如何使用自己写的头文件（保姆级教程）
VS2019中在源文件中如何使用自己写的头文件前言开始教程建立一个头文件建立头文件对应的.cpp文件在其余源文件中使用可能出现的错误前言一个完整的头文件应该分为:.h文件与.cpp文件 ...
c malloc 头文件_C/C++笔试题：主要考察C/C++语言基础概念算法及编程，附参考答案...
1.编写my_strcpy函数,实现与库函数strcpy类似的功能,不能使用任何库函数: 答:char *strcpy(char *strDest, const char *strSrc) { if ...
《Python算法教程》——1.6 如果您感兴趣
本节书摘来自异步社区<Python算法教程>一书中的第1章,第1.6节,作者[挪威]Magnus Lie Hetland(赫特兰), 凌杰译,更多章节内容可以访问云栖社区"异步 ...
压网线教程图解（做水晶头）
压网线教程图解(做水晶头) 参考网址:http://www.zizhouzx.com/zz/770.html

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

scanmatcher和gridfastslam头文件

1、gridlinetraversal.h

2、scanmatcher.h

3、gridslamprocessor.h

4、NEXT

gmapping算法教程（5）------scanmatcher和gridfastslam头文件相关推荐

最新文章

热门文章