gmaping原理及代码解析（三）

（三）gmapping详细代码解析

---

A、里程计运动模型

我们先来看一下Gmapping里面实现的里程计模型代码：

/*
@desc 里程计运动模型
@p    表示粒子估计的最优位置(机器人上一个时刻的最优位置)
@pnew 表示里程计算出来的新的位置
@pold 表示里程计算出来的旧的位置(即上一个里程计的位置)
*/
OrientedPoint 
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
	double sxy=0.3*srr;

	OrientedPoint delta=absoluteDifference(pnew, pold); // 计算新位姿相对于旧位姿的偏移量
	
	/*初始化一个点*/
	OrientedPoint noisypoint(delta);
	
	/*走过的X、y、z轴方向的距离加入噪声*/
	noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
	noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
	noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
	
	/*限制角度的范围*/
	noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
	if (noisypoint.theta>M_PI)
		noisypoint.theta-=2*M_PI;
	
	/*把加入了噪声的值 加到粒子估计的最优的位置上  即得到新的位置(根据运动模型推算出来的位置)*/
	return absoluteSum(p,noisypoint);
}

• OrientPoint点的结构:

/*带方向的点  即除了位置之外，还有角度，可以理解为机器人的位姿*/
template <class T, class A>
struct orientedpoint: public point<T>
{
  inline orientedpoint() : point<T>(0,0), theta(0) {};
	inline orientedpoint(const point<T>& p);
	inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
        inline void normalize();

    inline orientedpoint<T,A> rotate(A alpha)
    {
		T s=sin(alpha), c=cos(alpha);
		A a=alpha+theta;
		a=atan2(sin(a),cos(a));
		return orientedpoint(
			c*this->x-s*this->y,
			s*this->x+c*this->y, 
			a);
	}
	A theta;
};

• absoluteDifference函数：

absoluteDifference()这个函数是将t时刻与t-1时刻里程计测量数据之差转换到t-1时刻的车辆坐标系下，计算位姿差

/*
@desc 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。
*/
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);
}

• sampleGaussian函数（这里我把源代码中的注释给删掉了）：

double sampleGaussian(double sigma, unsigned int S) {
	if (S!=0)
        {
                srand(S);
        }
	if (sigma==0)
		return 0;
	return pf_ran_gaussian (sigma);
}

sampleGaussian函数里面的pf_ran_gaussian()函数,从方差为sigma,均值为0的高斯分布中抽取随机数，

参考：https://www.taygeta.com/random/gaussian.html（没太懂）：

double pf_ran_gaussian(double sigma)
{
  double x1, x2, w;
  double r;

  do
  {
    do { r = drand48(); } while (r == 0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r == 0.0);
    x2 = 2.0 * drand48() - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);

  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

• fod(x,y)是返回x/y的余数，将角度限制到（-pi,pi）之间

gmapping的里程计采样模型采取的方法是《概率机器人》里的里程计运动模型，具体如下：

• 运动模型示意图：

伪代码：

（1）2-4行表示将t时刻数据转换到t-1时刻的车辆坐标系下

（2）5-6行给新的偏移量加入一个包含噪声的偏移值,噪声是一个高斯采样值

（3）8-9行把得到的新的偏移量转换到没有噪声的车辆坐标下，再叠加t-1时刻的位姿，得到t时刻的位姿估计

控制量ut就是里程计给出的信息，，其中

是里程计信息在t-1时刻的位姿，是里程计在t时刻位姿，因为里程计是机器人内部的传感器，它计算的是机器人一点都没噪声的理想状态时的行进距离，所以由于机器人在行进的时候的偏移和打滑，这个是不准确的，所以在5~6行我们加入一定的噪声来表示机器人实际行进的“真”值。

---

B、scan-match模型

先上代码，gmapping里的函数scanMatch()，函数位于gridslamprocessor.hxx中；

来看在scanMatch中调用第一个比较重要的函数---optimize函数，位于scanmatcher.cpp中：

double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
    double bestScore=-1;
    /*计算当前位置的得分*/
    OrientedPoint currentPose=init;
    double currentScore=score(map, currentPose, readings);
	
    /*所有时的步进增量*/
    double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
	
    /*精确搜索的次数*/
    unsigned int refinement=0;
	
    /*搜索的方向*/
    enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
    //enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
    int c_iterations=0;
    do
    {
        /*如果这一次(currentScore)算出来比上一次(bestScore)差，则有可能是走太多了，要减少搜索步长 这个策略跟LM有点像*/
        if (bestScore>=currentScore)
        {
            refinement++;
            adelta*=.5;
            ldelta*=.5;
        }
        bestScore=currentScore;
        OrientedPoint bestLocalPose=currentPose;
        OrientedPoint localPose=currentPose;

        /*把8个方向都搜索一次  得到这8个方向里面最好的一个位姿和对应的得分*/
        Move move=Front;
        do
        {
            localPose=currentPose;
            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;

            //计算当前位姿的角度和初始角度的区别 如果里程计比较可靠的话
            //那么进行匹配的时候就需要对离初始位姿比较远的位姿施加惩罚
            if (m_angularOdometryReliability>0.)
            {
                double dth=init.theta-localPose.theta; 	dth=atan2(sin(dth), cos(dth)); 	dth*=dth;
                odo_gain*=exp(-m_angularOdometryReliability*dth);
            }
            //计算线性距离的区别 线性距离也是一样
            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;
            }
            c_iterations++;
        } while(move!=Done);
		
        /* 把当前位置设置为目前最优的位置  如果8个值都被差了的话，那么这个值不会更新*/
        currentPose=bestLocalPose;
    }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
	
    /*返回最优位置和得分*/
    pnew=currentPose;
    return bestScore;
}

其optomize,中心思想就是给一个初始位姿结合激光数据和地图求出最优位姿；但是这个初始位姿可能不够准确，但是我们要怎么找倒较准确的位姿（最优位姿）呢？-----方法就是在初始位姿的基础上，我们给他的x,y,thelta方向上依次给一个增量，给一次我们就计算一次位姿得分（得分表示在该位姿下激光束和地图的匹配程度），看看是不是比不给增量之前得分高，得分高意味着位姿更准确了；就这样迭代求出最优位姿。代码流程如下：

optimize里面有一个很重要的函数---score()函数，位于scanmatcher.h文件中：

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;
}

这个score函数采样的方法是《Probabilistic Robotics》的Perception部分，基本原理如下：

score函数的程序代码流程如下：

其中cell的坐标计算如下，n为被击中的次数，acc.x和acc.y是击中的累加值：

每个cell的状态更新如下，其中value表示是否被击中，p为被击中的世界坐标：

---

scanMatch函数经过optimize函数优化求得最优位姿后，optimize输出的得分，我们用得分来判定是否匹配成功，得分大于最小得分就认为匹配成功，就更新粒子的位姿；如果不成功，就用里程计的数据更新粒子的位姿。之后用likelihoodAndScore函数来计算粒子的似然和来表示粒子的权重，并更新：

---

likelihoodAndScore函数如下，计算原理和score函数是一样的，只不过函数里的位姿是匹配成功的最优位姿或者是（匹配不成功时）的里程计位姿；除了该粒子的得分外，还计算了该粒子的所有似然值的和，这个似然和是把没有击中障碍物的激光束也算进去了，这个似然和用来表示该粒子的权重（是当前时刻的权重，不是累计的权重哦）------------注意，计算得分时，我们只计算击中障碍物的激光束，：

/*函数跟score是非常像的，不同的是这个函数除了计算的得分之外，还计算的似然likelihood
这个函数的计算出来的似然在gmapping中用来表示粒子的权重
@param s			得分
@param l			似然  粒子的权重
@param map  		对应的地图
@param p    		机器人对应的初始位置
@param 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;
    s=0;
    const double * angle=m_laserAngles+m_initialBeamsSkip;

    /*
    把激光雷达的坐标转换到世界坐标系
    先旋转到机器人坐标系，然后再转换到世界坐标系
    */
    OrientedPoint lp=p;
    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;

    //如果没有击中的时候的似然值 nullLikehood = -0.5
    double noHit=nullLikelihood/(m_likelihoodSigma);


    unsigned int skip=0;
    unsigned int c=0;
    double freeDelta=map.getDelta()*m_freeCellRatio;

    for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
    {
        /*每隔m_likelihoodSkip个激光束 就跳过一个激光束*/
        skip++;
        skip=skip>m_likelihoodSkip?0:skip;
        if (*r>m_usableRange) continue;
        if (skip) continue;

        /*被激光击中的点*/
        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.x+=(*r-freeDelta)*cos(lp.theta+*angle);
        pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
        pfree=pfree-phit;
        IntPoint ipfree=map.world2map(pfree);

        /*在对应的kernerSize中搜索*/
        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++)
            {
                IntPoint pr=iphit+IntPoint(xx,yy);
                IntPoint pf=pr+ipfree;
                const PointAccumulator& cell=map.cell(pr);
                const PointAccumulator& fcell=map.cell(pf);

                /*如果cell(pr)被占用 而cell(pf)没有被占用 则说明找到了一个合法的点*/
                if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold)
                {
                    Point mu=phit-cell.mean();
                    if (!found)
                    {
                        bestMu=mu;
                        found=true;
                    }
                    else
                    {
                        bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
                    }
                }
            }
        /*计算得分 得分是只计有用的激光束*/
        if (found)
        {
            s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
            c++;
        }

        /*计算似然 似然是计算所有的激光束 如果某一个激光束打中了空地 那也需要计算进去*/
        if (!skip)
        {
            //似然不是指数 似然只是指数的上标
            double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
            l+=(found)?f:noHit;
        }
    }
    return c;
}

---

粒子的权重是估计值和后验值的比值，似然和怎么就可以表示权重呢？----我的理解是：我们把地图当成是后验值，然后在当前时刻估计的位姿上把激光投射在地图上去，总有一些激光点和地图是不匹配的，那这个匹配程度就可以表示在当前粒子的权重。

------个人理解，欢迎指正。

---

 

---

其实到这里，是不是觉得gmapping里面实际程序用的方法和论文里面的方法不太一样，记得论文里scan-match成功后还计算了Gaussian proposal分布（参考gmaping原理及代码解析（二）），而实际上，程序里scan-match成功后，就用scan-match得到的最优位姿来计算粒子t时刻的权重了。

但是开源的gmapping里面，也是准备了论文中的方法的函数代码：

optimize函数，和前面的optimize函数参数是不一样的：

//用来存储在scan-match的过程中 机器人的优化的路径的节点
//主要在下面的optimize()函数里面被调用
typedef std::list<ScoredMove> ScoredMoveList;

/*
@desc	根据地图、激光数据、位姿迭代求解一个最优位姿来。
这个函数与上面的函数不同的是，这个函数求出来的位姿不是一个特定的最优的cell，
而是假设在scan-match的过程中经过的所有的点服从高斯分布，然后根据经过的所有位姿的似然来求解得到一个加权位姿和位姿的方差。
这里认为机器人在scan-match过程中经过的路径点合起来是服从高斯分布的，因此机器人的位姿和方差最终由所有经过的点的加权和来计算得到

理论上来说这是一个更合理的选择。因为GMapping的论文里面也是选择了这样的方法


@param	pnew		新的最优位置
@param  map			地图
@param	init		初始位置
@param  readings	激光数据
*/
double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const
{
	ScoredMoveList moveList;
	double bestScore=-1;
	
	/*计算当前位置的得分score和似然likehood*/
	OrientedPoint currentPose=init;
	ScoredMove sm={currentPose,0,0};
	unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
	double currentScore=sm.score;
	moveList.push_back(sm);
	
	/*角度和线性递增*/
	double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
	
	unsigned int refinement=0;
	int count=0;
	
	/*8个方向迭代优化，找到一个最优的位置*/
	enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
	do{
        if (bestScore>=currentScore)
        {
			refinement++;
			adelta*=.5;
			ldelta*=.5;
		}
		bestScore=currentScore;
		OrientedPoint bestLocalPose=currentPose;
		OrientedPoint localPose=currentPose;

		Move move=Front;
		do {
			localPose=currentPose;
            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 localScore, localLikelihood;
			
            //计算似然的增益

            //如果里程计很可靠的话，那么现在的位姿如果离里程计位姿比较远的话，就需要加入惩罚
			double odo_gain=1;
            if (m_angularOdometryReliability>0.)
            {
                double dth=init.theta-localPose.theta;
                dth=atan2(sin(dth), cos(dth));
                dth*=dth;
				odo_gain*=exp(-m_angularOdometryReliability*dth);
			}
            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);
			}

            //这个分数根本就没有用。。。不知道搞什么鬼
			localScore=odo_gain*score(map, localPose, readings);
			//update the score
			count++;
			
            /*重新计算score 和 localLikelihood 也就是说上面计算得到的score是没有用的*/
            //这里面调用的是scanmatcher.h这个文件家里面的likelihoodAndScore函数
			matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);

            //如果大于当前得分 则需要进行更新
            if (localScore>currentScore)
            {
				currentScore=localScore;
				bestLocalPose=localPose;
			}
			
            //记录下来所有经过的点 以及对应的似然
			sm.score=localScore;
			sm.likelihood=localLikelihood;//+log(odo_gain);
			sm.pose=localPose;
			moveList.push_back(sm);

			//update the move list
		} while(move!=Done);
		currentPose=bestLocalPose;
	}while (currentScore>bestScore || refinement<m_optRecursiveIterations);
	
	//normalize the likelihood
	/*归一化似然*/
	double lmin=1e9;
	double lmax=-1e9;
	/*求取最大最小似然*/
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		lmin=it->likelihood<lmin?it->likelihood:lmin;
		lmax=it->likelihood>lmax?it->likelihood:lmax;
	}
	
    /*执行归一化操作 把似然通过指数函数计算成为得分 也就是概率*/
    for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		it->likelihood=exp(it->likelihood-lmax);
	}
	
	//compute the mean
	/*计算均值 和 方差*/
	OrientedPoint mean(0,0,0);
	double lacc=0;
	
	/*计算均值*/
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		mean=mean+it->pose*it->likelihood;
		lacc+=it->likelihood;
	}
	mean=mean*(1./lacc);
	
	/*计算方差*/
	CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		OrientedPoint delta=it->pose-mean;
		delta.theta=atan2(sin(delta.theta), cos(delta.theta));

		cov.xx+=delta.x*delta.x*it->likelihood;
		cov.yy+=delta.y*delta.y*it->likelihood;
		cov.tt+=delta.theta*delta.theta*it->likelihood;
		cov.xy+=delta.x*delta.y*it->likelihood;
		cov.xt+=delta.x*delta.theta*it->likelihood;
		cov.yt+=delta.y*delta.theta*it->likelihood;
	}
	cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
	
	_mean=currentPose;
	_cov=cov;
	return bestScore;
}

likelihood函数：

/*
计算某一个机器人的位置的似然，这个似然是在机器人位置的一个范围内(-mllsamplerange~mllsamplerange  -mllsamplerange~mllsamplerange -mlasamplerange m_lasamplerange)的似然
并且通过这个范围内的似然分布，来求出机器人的位置的期望值和方差

这个函数的真实意图应该是计算机器人位姿p的似然值。
这个也是Cyrill Stachniss在论文里面提出的方法。
这个方法应该是和上面的optimize(_mean,_cov,...)方法配套使用的。
用optimize来计算位姿 然后用这个函数来计算似然(或者说对应的位姿的权重)
可以认为这个函数的返回值就是对应的粒子的权重

这个函数的意思是：
假设机器人在p附近的(-mllsamplerange~mllsamplerange  -mllsamplerange~mllsamplerange -mlasamplerange m_lasamplerange)窗口是服从高斯分布的。
求机器人的真实位姿以及机器人的方差
*/
double ScanMatcher::likelihood
    (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings)
{
	ScoredMoveList moveList;
	
	/*计算这个区域内的每个点的似然 和 得分*/
	for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
	for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
	for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep)
	{
		
		OrientedPoint rp=p;
		rp.x+=xx;
		rp.y+=yy;
		rp.theta+=tt;
		
		ScoredMove sm;
		sm.pose=rp;
		
		likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
		moveList.push_back(sm);
	}
	
	//normalize the likelihood
    /*归一化似然*/
    double lmax=-1e9; //最大的似然值
    double lcum=0;    //累计的似然值
	
    /*求取最大的似然值*/
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		lmax=it->likelihood>lmax?it->likelihood:lmax;
	}
	
	/*计算lcum 和 每个点的概率p(x)    归一化似然*/
    for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		//it->likelihood=exp(it->likelihood-lmax);
		lcum+=exp(it->likelihood-lmax);
		it->likelihood=exp(it->likelihood-lmax);
		//cout << "l=" << it->likelihood << endl;
	}
	//到了这里这个区域内的似然都已经归一化了
	
	/*计算均值  机器人位置的均值  E(x) = \sum_{x*p(x)}*/
	OrientedPoint mean(0,0,0);
	double s=0,c=0;
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		mean=mean+it->pose*it->likelihood;
		s+=it->likelihood*sin(it->pose.theta);
		c+=it->likelihood*cos(it->pose.theta);
	}
	mean=mean*(1./lcum);

    //计算机器人的位姿
    s/=lcum;
	c/=lcum;
	mean.theta=atan2(s,c);
	
	/*计算方差 机器人位置的方差*/
	CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
	for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
	{
		
		/*计算每个点和mean的距离差和角度差*/
		OrientedPoint delta=it->pose-mean;
		delta.theta=atan2(sin(delta.theta), cos(delta.theta));
		
		/*计算协防差矩阵*/
		cov.xx+=delta.x*delta.x*it->likelihood;
		cov.yy+=delta.y*delta.y*it->likelihood;
		cov.tt+=delta.theta*delta.theta*it->likelihood;
		cov.xy+=delta.x*delta.y*it->likelihood;
		cov.xt+=delta.x*delta.theta*it->likelihood;
		cov.yt+=delta.y*delta.theta*it->likelihood;
	}
    //协方差矩阵进行归一化
	cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
	
    //把值传递出去
	_mean=mean;
	_cov=cov;
	_lmax=lmax;

    //返回极大似然值和累计似然值 意思就是不单单是取似然值最大的那个 也不是取累计似然值最大的那个
	return log(lcum)+lmax;
}

likelihood函数，考虑了里程计似然的函数：

/*跟上面的函数基本上是一个功能，不同的是这个函数考虑了里程计的似然*/
double ScanMatcher::likelihood
	(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p,
    Gaussian3& odometry, const double* readings, double gain)
{
	ScoredMoveList moveList;
	
	
	for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
	for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
	for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep)
	{
		
		OrientedPoint rp=p;
		rp.x+=xx;
		rp.y+=yy;
		rp.theta+=tt;
		
		ScoredMove sm;
		sm.pose=rp;
		
		
		likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
		sm.likelihood+=odometry.eval(rp)/gain;
		assert(!isnan(sm.likelihood));
		moveList.push_back(sm);
	}
	
   //normalize the likelihood	归一化似然
  double lmax=-std::numeric_limits<double>::max();
	double lcum=0;
	for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
	{
		lmax=it->likelihood>lmax?it->likelihood:lmax;
	}
	for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++)
	{
		//it->likelihood=exp(it->likelihood-lmax);
		lcum+=exp(it->likelihood-lmax);
		it->likelihood=exp(it->likelihood-lmax);
		//cout << "l=" << it->likelihood << endl;
	}
	//似然归一化完毕
	
	//计算均值
	OrientedPoint mean(0,0,0);
	double s=0,c=0;
    for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++)
    {
		mean=mean+it->pose*it->likelihood;
		s+=it->likelihood*sin(it->pose.theta);
		c+=it->likelihood*cos(it->pose.theta);
	}
	mean=mean*(1./lcum);
	s/=lcum;
	c/=lcum;
	mean.theta=atan2(s,c);
	
	//计算方差
	CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
	for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
		OrientedPoint delta=it->pose-mean;
		delta.theta=atan2(sin(delta.theta), cos(delta.theta));
		cov.xx+=delta.x*delta.x*it->likelihood;
		cov.yy+=delta.y*delta.y*it->likelihood;
		cov.tt+=delta.theta*delta.theta*it->likelihood;
		cov.xy+=delta.x*delta.y*it->likelihood;
		cov.xt+=delta.x*delta.theta*it->likelihood;
		cov.yt+=delta.y*delta.theta*it->likelihood;
	}
	cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
	
	_mean=mean;
	_cov=cov;
	_lmax=lmax;
	double v=log(lcum)+lmax;
	assert(!isnan(v));
	return v;
}

函数和论文的中的方法一致，不做过多解释了。