scanmatcher.cpp

Go to the documentation of this file.

#include <cstring>
#include <limits>
#include <list>
#include <iostream>

#include <gmapping/scanmatcher/scanmatcher.h>
#include "gridlinetraversal.h"
//#define GENERATE_MAPS

namespace GMapping {

using namespace std;

const double ScanMatcher::nullLikelihood=-.5;

ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
        //m_laserAngles=0;
        m_laserBeams=0;
        m_optRecursiveIterations=3;
        m_activeAreaComputed=false;

        // This  are the dafault settings for a grid map of 5 cm
        m_llsamplerange=0.01;
        m_llsamplestep=0.01;
        m_lasamplerange=0.005;
        m_lasamplestep=0.005;
        m_enlargeStep=10.;
        m_fullnessThreshold=0.1;
        m_angularOdometryReliability=0.;
        m_linearOdometryReliability=0.;
        m_freeCellRatio=sqrt(2.);
        m_initialBeamsSkip=0;
        
/*      
        // This  are the dafault settings for a grid map of 10 cm
        m_llsamplerange=0.1;
        m_llsamplestep=0.1;
        m_lasamplerange=0.02;
        m_lasamplestep=0.01;
*/      
        // This  are the dafault settings for a grid map of 20/25 cm
/*
        m_llsamplerange=0.2;
        m_llsamplestep=0.1;
        m_lasamplerange=0.02;
        m_lasamplestep=0.01;
        m_generateMap=false;
*/

   m_linePoints = new IntPoint[20000];
}

ScanMatcher::~ScanMatcher(){
        delete [] m_linePoints;
}

void ScanMatcher::invalidateActiveArea(){
        m_activeAreaComputed=false;
}

/*
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
        if (m_activeAreaComputed)
                return;
        HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
        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;
        IntPoint p0=map.world2map(lp);
        const double * angle=m_laserAngles;
        for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
                if (m_generateMap){
                        double d=*r;
                        if (d>m_laserMaxRange)
                                continue;
                        if (d>m_usableRange)
                                d=m_usableRange;
                        
                        Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
                        IntPoint p1=map.world2map(phit);
                        
                        d+=map.getDelta();
                        //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
                        //IntPoint p2=map.world2map(phit2);
                        IntPoint linePoints[20000] ;
                        GridLineTraversalLine line;
                        line.points=linePoints;
                        //GridLineTraversal::gridLine(p0, p2, &line);
                        GridLineTraversal::gridLine(p0, p1, &line);
                        for (int i=0; i<line.num_points-1; i++){
                                activeArea.insert(map.storage().patchIndexes(linePoints[i]));
                        }
                        if (d<=m_usableRange){
                                activeArea.insert(map.storage().patchIndexes(p1));
                                //activeArea.insert(map.storage().patchIndexes(p2));
                        }
                } else {
                        if (*r>m_laserMaxRange||*r>m_usableRange) continue;
                        Point phit=lp;
                        phit.x+=*r*cos(lp.theta+*angle);
                        phit.y+=*r*sin(lp.theta+*angle);
                        IntPoint p1=map.world2map(phit);
                        assert(p1.x>=0 && p1.y>=0);
                        IntPoint cp=map.storage().patchIndexes(p1);
                        assert(cp.x>=0 && cp.y>=0);
                        activeArea.insert(cp);
                        
                }
        //this allocates the unallocated cells in the active area of the map
        //cout << "activeArea::size() " << activeArea.size() << endl;
        map.storage().setActiveArea(activeArea, true);
        m_activeAreaComputed=true;
}
*/
void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
        if (m_activeAreaComputed)
                return;
        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;
        IntPoint p0=map.world2map(lp);
        
        Point min(map.map2world(0,0));
        Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
               
        if (lp.x<min.x) min.x=lp.x;
        if (lp.y<min.y) min.y=lp.y;
        if (lp.x>max.x) max.x=lp.x;
        if (lp.y>max.y) max.y=lp.y;
        
        /*determine the size of the area*/
        const double * angle=m_laserAngles+m_initialBeamsSkip;
        for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
                if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
                double d=*r>m_usableRange?m_usableRange:*r;
                Point phit=lp;
                phit.x+=d*cos(lp.theta+*angle);
                phit.y+=d*sin(lp.theta+*angle);
                if (phit.x<min.x) min.x=phit.x;
                if (phit.y<min.y) min.y=phit.y;
                if (phit.x>max.x) max.x=phit.x;
                if (phit.y>max.y) max.y=phit.y;
        }
        //min=min-Point(map.getDelta(),map.getDelta());
        //max=max+Point(map.getDelta(),map.getDelta());
        
        if ( !map.isInside(min) || !map.isInside(max)){
                Point lmin(map.map2world(0,0));
                Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
                //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
                //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
                min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
                max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
                min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
                max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
                map.resize(min.x, min.y, max.x, max.y);
                //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
        }
        
        HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
        /*allocate the active area*/
        angle=m_laserAngles+m_initialBeamsSkip;
        for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
                if (m_generateMap){
                        double d=*r;
                        if (d>m_laserMaxRange||d==0.0||isnan(d))
                                continue;
                        if (d>m_usableRange)
                                d=m_usableRange;
                        Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
                        IntPoint p0=map.world2map(lp);
                        IntPoint p1=map.world2map(phit);
                        
                        //IntPoint linePoints[20000] ;
                        GridLineTraversalLine line;
                        line.points=m_linePoints;
                        GridLineTraversal::gridLine(p0, p1, &line);
                        for (int i=0; i<line.num_points-1; i++){
                                assert(map.isInside(m_linePoints[i]));
                                activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
                                assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
                        }
                        if (d<m_usableRange){
                                IntPoint cp=map.storage().patchIndexes(p1);
                                assert(cp.x>=0 && cp.y>=0);
                                activeArea.insert(cp);
                        }
                } else {
                        if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
                        Point phit=lp;
                        phit.x+=*r*cos(lp.theta+*angle);
                        phit.y+=*r*sin(lp.theta+*angle);
                        IntPoint p1=map.world2map(phit);
                        assert(p1.x>=0 && p1.y>=0);
                        IntPoint cp=map.storage().patchIndexes(p1);
                        assert(cp.x>=0 && cp.y>=0);
                        activeArea.insert(cp);
                }
        
        //this allocates the unallocated cells in the active area of the map
        //cout << "activeArea::size() " << activeArea.size() << endl;
/*      
        cerr << "ActiveArea=";
        for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
                cerr << "(" << it->x <<"," << it->y << ") ";
        }
        cerr << endl;
*/              
        map.storage().setActiveArea(activeArea, true);
        m_activeAreaComputed=true;
}

double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
        if (!m_activeAreaComputed)
                computeActiveArea(map, p, readings);
                
        //this operation replicates the cells that will be changed in the registration operation
        map.storage().allocActiveArea();
        
        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;
        IntPoint p0=map.world2map(lp);
        
        
        const double * angle=m_laserAngles+m_initialBeamsSkip;
        double esum=0;
        for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
                if (m_generateMap){
                        double d=*r;
                        if (d>m_laserMaxRange||d==0.0||isnan(d))
                                continue;
                        if (d>m_usableRange)
                                d=m_usableRange;
                        Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
                        IntPoint p1=map.world2map(phit);
                        //IntPoint linePoints[20000] ;
                        GridLineTraversalLine line;
                        line.points=m_linePoints;
                        GridLineTraversal::gridLine(p0, p1, &line);
                        for (int i=0; i<line.num_points-1; i++){
                                PointAccumulator& cell=map.cell(line.points[i]);
                                double e=-cell.entropy();
                                cell.update(false, Point(0,0));
                                e+=cell.entropy();
                                esum+=e;
                        }
                        if (d<m_usableRange){
                                double e=-map.cell(p1).entropy();
                                map.cell(p1).update(true, phit);
                                e+=map.cell(p1).entropy();
                                esum+=e;
                        }
                } else {
                        if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
                        Point phit=lp;
                        phit.x+=*r*cos(lp.theta+*angle);
                        phit.y+=*r*sin(lp.theta+*angle);
                        IntPoint p1=map.world2map(phit);
                        assert(p1.x>=0 && p1.y>=0);
                        map.cell(p1).update(true,phit);
                }
        //cout  << "informationGain=" << -esum << endl;
        return esum;
}

/*
void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
        if (!m_activeAreaComputed)
                computeActiveArea(map, p, readings);
                
        //this operation replicates the cells that will be changed in the registration operation
        map.storage().allocActiveArea();
        
        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;
        IntPoint p0=map.world2map(lp);
        const double * angle=m_laserAngles;
        for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
                if (m_generateMap){     
                        double d=*r;
                        if (d>m_laserMaxRange)
                                continue;
                        if (d>m_usableRange)
                                d=m_usableRange;
                        Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
                        IntPoint p1=map.world2map(phit);
                        
                        IntPoint linePoints[20000] ;
                        GridLineTraversalLine line;
                        line.points=linePoints;
                        GridLineTraversal::gridLine(p0, p1, &line);
                        for (int i=0; i<line.num_points-1; i++){
                                IntPoint ci=map.storage().patchIndexes(line.points[i]);
                                if (map.storage().getActiveArea().find(ci)==map.storage().getActiveArea().end())
                                        cerr << "BIG ERROR" <<endl;
                                map.cell(line.points[i]).update(false, Point(0,0));
                        }
                        if (d<=m_usableRange){
                                
                                map.cell(p1).update(true,phit);
                        }
                } else {
                        if (*r>m_laserMaxRange||*r>m_usableRange) continue;
                        Point phit=lp;
                        phit.x+=*r*cos(lp.theta+*angle);
                        phit.y+=*r*sin(lp.theta+*angle);
                        map.cell(phit).update(true,phit);
                }
}

*/

double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
        double currentScore;
        double sc=score(map, init, readings);;
        OrientedPoint start=init;
        pnew=init;
        int iterations=0;
        do{
                currentScore=sc;
                sc=icpStep(pnew, map, start, readings);
                //cerr << "pstart=" << start.x << " " <<start.y << " " << start.theta << endl;
                //cerr << "pret=" << pnew.x << " " <<pnew.y << " " << pnew.theta << endl;
                start=pnew;
                iterations++;
        } while (sc>currentScore);
        cerr << "i="<< iterations << endl;
        return currentScore;
}

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};
/*      cout << __PRETTY_FUNCTION__<<  " readings: ";
        for (int i=0; i<m_laserBeams; i++){
                cout << readings[i] << " ";
        }
        cout << endl;
*/      int c_iterations=0;
        do{
                if (bestScore>=currentScore){
                        refinement++;
                        adelta*=.5;
                        ldelta*=.5;
                }
                bestScore=currentScore;
//              cout <<"score="<< currentScore << " refinement=" << refinement;
//              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
                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 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);
                        }
                        double localScore=odo_gain*score(map, localPose, readings);
                        
                        if (localScore>currentScore){
                                currentScore=localScore;
                                bestLocalPose=localPose;
                        }
                        c_iterations++;
                } while(move!=Done);
                currentPose=bestLocalPose;
//              cout << "currentScore=" << currentScore<< endl;
                //here we look for the best move;
        }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
        //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
        //cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
        pnew=currentPose;
        return bestScore;
}

struct ScoredMove{
        OrientedPoint pose;
        double score;
        double likelihood;
};

typedef std::list<ScoredMove> ScoredMoveList;

double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
        ScoredMoveList moveList;
        double bestScore=-1;
        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;
        enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
        do{
                if (bestScore>=currentScore){
                        refinement++;
                        adelta*=.5;
                        ldelta*=.5;
                }
                bestScore=currentScore;
//              cout <<"score="<< currentScore << " refinement=" << refinement;
//              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
                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++;
                        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;
                //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
                //here we look for the best move;
        }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
        //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
        //cout << __PRETTY_FUNCTION__ << "iterations=" << count<< endl;
        
        //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;
        }
        //cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
        for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
                it->likelihood=exp(it->likelihood-lmax);
                //cout << "l=" << it->likelihood << endl;
        }
        //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);
        //OrientedPoint delta=mean-currentPose;
        //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
        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;
}

void ScanMatcher::setLaserParameters
        (unsigned int beams, double* angles, const OrientedPoint& lpose){
        /*if (m_laserAngles)
                delete [] m_laserAngles;
        */
        assert(beams<LASER_MAXBEAMS);
        m_laserPose=lpose;
        m_laserBeams=beams;
        //m_laserAngles=new double[beams];
        memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);     
}
        

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);
        }
        
        //OrientedPoint delta=mean-currentPose;
        //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
        //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;
        }
        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;
        return log(lcum)+lmax;
}

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);
        }
        
        //OrientedPoint delta=mean-currentPose;
        //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
        //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;
}

void ScanMatcher::setMatchingParameters
        (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations,  double likelihoodSigma, unsigned int likelihoodSkip){      
        m_usableRange=urange;
        m_laserMaxRange=range;
        m_kernelSize=kernsize;
        m_optLinearDelta=lopt;
        m_optAngularDelta=aopt;
        m_optRecursiveIterations=iterations;
        m_gaussianSigma=sigma;
        m_likelihoodSigma=likelihoodSigma;
        m_likelihoodSkip=likelihoodSkip;
}

};