scanmatcher.new.cpp
Go to the documentation of this file.
00001 #include <list>
00002 #include <iostream>
00003 
00004 #include <gmapping/scanmatcher/scanmatcher.h>
00005 #include "gridlinetraversal.h"
00006 //#define GENERATE_MAPS
00007 
00008 using namespace std;
00009 namespace GMapping {
00010 
00011 const double ScanMatcher::nullLikelihood=-1.;
00012 
00013 ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
00014         m_laserAngles=0;
00015         m_laserBeams=0;
00016         m_optRecursiveIterations=3;
00017         m_activeAreaComputed=false;
00018 
00019         // This  are the dafault settings for a grid map of 5 cm
00020         m_llsamplerange=0.01;
00021         m_llsamplestep=0.01;
00022         m_lasamplerange=0.005;
00023         m_lasamplestep=0.005;
00024 /*      
00025         // This  are the dafault settings for a grid map of 10 cm
00026         m_llsamplerange=0.1;
00027         m_llsamplestep=0.1;
00028         m_lasamplerange=0.02;
00029         m_lasamplestep=0.01;
00030 */      
00031         // This  are the dafault settings for a grid map of 20/25 cm
00032 /*
00033         m_llsamplerange=0.2;
00034         m_llsamplestep=0.1;
00035         m_lasamplerange=0.02;
00036         m_lasamplestep=0.01;
00037         m_generateMap=false;
00038 */
00039 }
00040 
00041 ScanMatcher::~ScanMatcher(){
00042         if (m_laserAngles)
00043                 delete [] m_laserAngles;
00044 }
00045 
00046 void ScanMatcher::invalidateActiveArea(){
00047         m_activeAreaComputed=false;
00048 }
00049 
00050 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00051         if (m_activeAreaComputed)
00052                 return;
00053         HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
00054         OrientedPoint lp=p;
00055         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00056         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00057         lp.theta+=m_laserPose.theta;
00058         IntPoint p0=map.world2map(lp);
00059         const double * angle=m_laserAngles;
00060         for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
00061                 if (m_generateMap){
00062                         double d=*r;
00063                         if (d>m_laserMaxRange)
00064                                 continue;
00065                         if (d>m_usableRange)
00066                                 d=m_usableRange;
00067                         
00068                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00069                         IntPoint p1=map.world2map(phit);
00070                         
00071                         d+=map.getDelta();
00072                         //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00073                         //IntPoint p2=map.world2map(phit2);
00074                         IntPoint linePoints[20000] ;
00075                         GridLineTraversalLine line;
00076                         line.points=linePoints;
00077                         //GridLineTraversal::gridLine(p0, p2, &line);
00078                         GridLineTraversal::gridLine(p0, p1, &line);
00079                         for (int i=0; i<line.num_points-1; i++){
00080                                 activeArea.insert(map.storage().patchIndexes(linePoints[i]));
00081                         }
00082                         if (d<=m_usableRange){
00083                                 activeArea.insert(map.storage().patchIndexes(p1));
00084                                 //activeArea.insert(map.storage().patchIndexes(p2));
00085                         }
00086                 } else {
00087                         if (*r>m_laserMaxRange||*r>m_usableRange) continue;
00088                         Point phit=lp;
00089                         phit.x+=*r*cos(lp.theta+*angle);
00090                         phit.y+=*r*sin(lp.theta+*angle);
00091                         IntPoint p1=map.world2map(phit);
00092                         assert(p1.x>=0 && p1.y>=0);
00093                         IntPoint cp=map.storage().patchIndexes(p1);
00094                         assert(cp.x>=0 && cp.y>=0);
00095                         activeArea.insert(cp);
00096                         
00097                 }
00098         //this allocates the unallocated cells in the active area of the map
00099         //cout << "activeArea::size() " << activeArea.size() << endl;
00100         map.storage().setActiveArea(activeArea, true);
00101         m_activeAreaComputed=true;
00102 }
00103 
00104 void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00105         if (!m_activeAreaComputed)
00106                 computeActiveArea(map, p, readings);
00107                 
00108         //this operation replicates the cells that will be changed in the registration operation
00109         map.storage().allocActiveArea();
00110         
00111         OrientedPoint lp=p;
00112         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00113         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00114         lp.theta+=m_laserPose.theta;
00115         IntPoint p0=map.world2map(lp);
00116         const double * angle=m_laserAngles;
00117         for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
00118                 if (m_generateMap){     
00119                         double d=*r;
00120                         if (d>m_laserMaxRange)
00121                                 continue;
00122                         if (d>m_usableRange)
00123                                 d=m_usableRange;
00124                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00125                         IntPoint p1=map.world2map(phit);
00126                         
00127                         d+=map.getDelta();
00128                         //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00129                         //IntPoint p2=map.world2map(phit2);
00130                         IntPoint linePoints[20000] ;
00131                         GridLineTraversalLine line;
00132                         line.points=linePoints;
00133                         //GridLineTraversal::gridLine(p0, p2, &line);
00134                         GridLineTraversal::gridLine(p0, p1, &line);
00135                         for (int i=0; i<line.num_points-1; i++){
00136                                 map.cell(line.points[i]).update(false, Point(0,0));
00137                         }
00138                         if (d<=m_usableRange){
00139                                 map.cell(p1).update(true,phit);
00140                         //      map.cell(p2).update(true,phit);
00141                         }
00142                 } else {
00143                         if (*r>m_laserMaxRange||*r>m_usableRange) continue;
00144                         Point phit=lp;
00145                         phit.x+=*r*cos(lp.theta+*angle);
00146                         phit.y+=*r*sin(lp.theta+*angle);
00147                         map.cell(phit).update(true,phit);
00148                 }
00149 }
00150 
00151 
00152 
00153 double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
00154         double bestScore=-1;
00155         OrientedPoint currentPose=init;
00156         double currentScore=score(map, currentPose, readings);
00157         double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
00158         unsigned int refinement=0;
00159         enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
00160         int c_iterations=0;
00161         do{
00162                 if (bestScore>=currentScore){
00163                         refinement++;
00164                         adelta*=.5;
00165                         ldelta*=.5;
00166                 }
00167                 bestScore=currentScore;
00168 //              cout <<"score="<< currentScore << " refinement=" << refinement;
00169 //              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
00170                 OrientedPoint bestLocalPose=currentPose;
00171                 OrientedPoint localPose=currentPose;
00172 
00173                 Move move=Front;
00174                 do {
00175                         localPose=currentPose;
00176                         switch(move){
00177                                 case Front:
00178                                         localPose.x+=ldelta;
00179                                         move=Back;
00180                                         break;
00181                                 case Back:
00182                                         localPose.x-=ldelta;
00183                                         move=Left;
00184                                         break;
00185                                 case Left:
00186                                         localPose.y-=ldelta;
00187                                         move=Right;
00188                                         break;
00189                                 case Right:
00190                                         localPose.y+=ldelta;
00191                                         move=TurnLeft;
00192                                         break;
00193                                 case TurnLeft:
00194                                         localPose.theta+=adelta;
00195                                         move=TurnRight;
00196                                         break;
00197                                 case TurnRight:
00198                                         localPose.theta-=adelta;
00199                                         move=Done;
00200                                         break;
00201                                 default:;
00202                         }
00203                         double localScore=score(map, localPose, readings);
00204                         if (localScore>currentScore){
00205                                 currentScore=localScore;
00206                                 bestLocalPose=localPose;
00207                         }
00208                         c_iterations++;
00209                 } while(move!=Done);
00210                 currentPose=bestLocalPose;
00211                 //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
00212                 //here we look for the best move;
00213         }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
00214         //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
00215         //cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
00216         pnew=currentPose;
00217         return bestScore;
00218 }
00219 
00220 struct ScoredMove{
00221         OrientedPoint pose;
00222         double score;
00223         double likelihood;
00224 };
00225 
00226 typedef std::list<ScoredMove> ScoredMoveList;
00227 
00228 double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
00229         ScoredMoveList moveList;
00230         double bestScore=-1;
00231         OrientedPoint currentPose=init;
00232         ScoredMove sm={currentPose,0,0};
00233         unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
00234         double currentScore=sm.score;
00235         moveList.push_back(sm);
00236         double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
00237         unsigned int refinement=0;
00238         enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
00239         do{
00240                 if (bestScore>=currentScore){
00241                         refinement++;
00242                         adelta*=.5;
00243                         ldelta*=.5;
00244                 }
00245                 bestScore=currentScore;
00246 //              cout <<"score="<< currentScore << " refinement=" << refinement;
00247 //              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
00248                 OrientedPoint bestLocalPose=currentPose;
00249                 OrientedPoint localPose=currentPose;
00250 
00251                 Move move=Front;
00252                 do {
00253                         localPose=currentPose;
00254                         switch(move){
00255                                 case Front:
00256                                         localPose.x+=ldelta;
00257                                         move=Back;
00258                                         break;
00259                                 case Back:
00260                                         localPose.x-=ldelta;
00261                                         move=Left;
00262                                         break;
00263                                 case Left:
00264                                         localPose.y-=ldelta;
00265                                         move=Right;
00266                                         break;
00267                                 case Right:
00268                                         localPose.y+=ldelta;
00269                                         move=TurnLeft;
00270                                         break;
00271                                 case TurnLeft:
00272                                         localPose.theta+=adelta;
00273                                         move=TurnRight;
00274                                         break;
00275                                 case TurnRight:
00276                                         localPose.theta-=adelta;
00277                                         move=Done;
00278                                         break;
00279                                 default:;
00280                         }
00281                         double localScore, localLikelihood;
00282                         //update the score
00283                         matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
00284                         if (localScore>currentScore){
00285                                 currentScore=localScore;
00286                                 bestLocalPose=localPose;
00287                         }
00288                         sm.score=localScore;
00289                         sm.likelihood=localLikelihood;
00290                         sm.pose=localPose;
00291                         moveList.push_back(sm);
00292                         //update the move list
00293                 } while(move!=Done);
00294                 currentPose=bestLocalPose;
00295                 //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
00296                 //here we look for the best move;
00297         }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
00298         //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
00299         
00300         //normalize the likelihood
00301         double lmin=1e9;
00302         double lmax=-1e9;
00303         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00304                 lmin=it->likelihood<lmin?it->likelihood:lmin;
00305                 lmax=it->likelihood>lmax?it->likelihood:lmax;
00306         }
00307         //cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
00308         for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
00309                 it->likelihood=exp(it->likelihood-lmax);
00310                 //cout << "l=" << it->likelihood << endl;
00311         }
00312         //compute the mean
00313         OrientedPoint mean(0,0,0);
00314         double lacc=0;
00315         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00316                 mean=mean+it->pose*it->likelihood;
00317                 lacc+=it->likelihood;
00318         }
00319         mean=mean*(1./lacc);
00320         //OrientedPoint delta=mean-currentPose;
00321         //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
00322         CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
00323         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00324                 OrientedPoint delta=it->pose-mean;
00325                 delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00326                 cov.xx+=delta.x*delta.x*it->likelihood;
00327                 cov.yy+=delta.y*delta.y*it->likelihood;
00328                 cov.tt+=delta.theta*delta.theta*it->likelihood;
00329                 cov.xy+=delta.x*delta.y*it->likelihood;
00330                 cov.xt+=delta.x*delta.theta*it->likelihood;
00331                 cov.yt+=delta.y*delta.theta*it->likelihood;
00332         }
00333         cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
00334         
00335         _mean=currentPose;
00336         _cov=cov;
00337         return bestScore;
00338 }
00339 
00340         void ScanMatcher::setLaserParameters
00341         (unsigned int beams, double* angles, const OrientedPoint& lpose){
00342         if (m_laserAngles)
00343                 delete [] m_laserAngles;
00344         m_laserPose=lpose;
00345         m_laserBeams=beams;
00346         m_laserAngles=new double[beams];
00347         memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);     
00348 }
00349         
00350 
00351 double ScanMatcher::likelihood
00352         (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00353         ScoredMoveList moveList;
00354         
00355         
00356         for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
00357         for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
00358         for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
00359                 
00360                 OrientedPoint rp=p;
00361                 rp.x+=xx;
00362                 rp.y+=yy;
00363                 rp.theta+=tt;
00364                 
00365                 ScoredMove sm;
00366                 sm.pose=rp;
00367                 
00368                 likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
00369                 moveList.push_back(sm);
00370         }
00371         
00372         //OrientedPoint delta=mean-currentPose;
00373         //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
00374         //normalize the likelihood
00375         double lmax=-1e9;
00376         double lcum=0;
00377         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00378                 lmax=it->likelihood>lmax?it->likelihood:lmax;
00379         }
00380         for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
00381                 //it->likelihood=exp(it->likelihood-lmax);
00382                 lcum+=exp(it->likelihood-lmax);
00383                 it->likelihood=exp(it->likelihood-lmax);
00384                 //cout << "l=" << it->likelihood << endl;
00385         }
00386         
00387         OrientedPoint mean(0,0,0);
00388         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00389                 mean=mean+it->pose*it->likelihood;
00390         }
00391         mean=mean*(1./lcum);
00392         
00393         CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
00394         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00395                 OrientedPoint delta=it->pose-mean;
00396                 delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00397                 cov.xx+=delta.x*delta.x*it->likelihood;
00398                 cov.yy+=delta.y*delta.y*it->likelihood;
00399                 cov.tt+=delta.theta*delta.theta*it->likelihood;
00400                 cov.xy+=delta.x*delta.y*it->likelihood;
00401                 cov.xt+=delta.x*delta.theta*it->likelihood;
00402                 cov.yt+=delta.y*delta.theta*it->likelihood;
00403         }
00404         cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
00405         
00406         _mean=mean;
00407         _cov=cov;
00408         _lmax=lmax;
00409         return log(lcum)+lmax;
00410 }
00411 
00412 void ScanMatcher::setMatchingParameters
00413         (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations,  double likelihoodSigma, unsigned int likelihoodSkip){      
00414         m_usableRange=urange;
00415         m_laserMaxRange=range;
00416         m_kernelSize=kernsize;
00417         m_optLinearDelta=lopt;
00418         m_optAngularDelta=aopt;
00419         m_optRecursiveIterations=iterations;
00420         m_gaussianSigma=sigma;
00421         m_likelihoodSigma=likelihoodSigma;
00422         m_likelihoodSkip=likelihoodSkip;
00423 }
00424 
00425 };
00426 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21