scanmatcher.cpp
Go to the documentation of this file.
00001 #include <cstring>
00002 #include <limits>
00003 #include <list>
00004 #include <iostream>
00005 
00006 #include <gmapping/scanmatcher/scanmatcher.h>
00007 #include "gridlinetraversal.h"
00008 //#define GENERATE_MAPS
00009 
00010 namespace GMapping {
00011 
00012 using namespace std;
00013 
00014 const double ScanMatcher::nullLikelihood=-.5;
00015 
00016 ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
00017         //m_laserAngles=0;
00018         m_laserBeams=0;
00019         m_optRecursiveIterations=3;
00020         m_activeAreaComputed=false;
00021 
00022         // This  are the dafault settings for a grid map of 5 cm
00023         m_llsamplerange=0.01;
00024         m_llsamplestep=0.01;
00025         m_lasamplerange=0.005;
00026         m_lasamplestep=0.005;
00027         m_enlargeStep=10.;
00028         m_fullnessThreshold=0.1;
00029         m_angularOdometryReliability=0.;
00030         m_linearOdometryReliability=0.;
00031         m_freeCellRatio=sqrt(2.);
00032         m_initialBeamsSkip=0;
00033         
00034 /*      
00035         // This  are the dafault settings for a grid map of 10 cm
00036         m_llsamplerange=0.1;
00037         m_llsamplestep=0.1;
00038         m_lasamplerange=0.02;
00039         m_lasamplestep=0.01;
00040 */      
00041         // This  are the dafault settings for a grid map of 20/25 cm
00042 /*
00043         m_llsamplerange=0.2;
00044         m_llsamplestep=0.1;
00045         m_lasamplerange=0.02;
00046         m_lasamplestep=0.01;
00047         m_generateMap=false;
00048 */
00049 
00050    m_linePoints = new IntPoint[20000];
00051 }
00052 
00053 ScanMatcher::~ScanMatcher(){
00054         delete [] m_linePoints;
00055 }
00056 
00057 void ScanMatcher::invalidateActiveArea(){
00058         m_activeAreaComputed=false;
00059 }
00060 
00061 /*
00062 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00063         if (m_activeAreaComputed)
00064                 return;
00065         HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
00066         OrientedPoint lp=p;
00067         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00068         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00069         lp.theta+=m_laserPose.theta;
00070         IntPoint p0=map.world2map(lp);
00071         const double * angle=m_laserAngles;
00072         for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
00073                 if (m_generateMap){
00074                         double d=*r;
00075                         if (d>m_laserMaxRange)
00076                                 continue;
00077                         if (d>m_usableRange)
00078                                 d=m_usableRange;
00079                         
00080                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00081                         IntPoint p1=map.world2map(phit);
00082                         
00083                         d+=map.getDelta();
00084                         //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00085                         //IntPoint p2=map.world2map(phit2);
00086                         IntPoint linePoints[20000] ;
00087                         GridLineTraversalLine line;
00088                         line.points=linePoints;
00089                         //GridLineTraversal::gridLine(p0, p2, &line);
00090                         GridLineTraversal::gridLine(p0, p1, &line);
00091                         for (int i=0; i<line.num_points-1; i++){
00092                                 activeArea.insert(map.storage().patchIndexes(linePoints[i]));
00093                         }
00094                         if (d<=m_usableRange){
00095                                 activeArea.insert(map.storage().patchIndexes(p1));
00096                                 //activeArea.insert(map.storage().patchIndexes(p2));
00097                         }
00098                 } else {
00099                         if (*r>m_laserMaxRange||*r>m_usableRange) continue;
00100                         Point phit=lp;
00101                         phit.x+=*r*cos(lp.theta+*angle);
00102                         phit.y+=*r*sin(lp.theta+*angle);
00103                         IntPoint p1=map.world2map(phit);
00104                         assert(p1.x>=0 && p1.y>=0);
00105                         IntPoint cp=map.storage().patchIndexes(p1);
00106                         assert(cp.x>=0 && cp.y>=0);
00107                         activeArea.insert(cp);
00108                         
00109                 }
00110         //this allocates the unallocated cells in the active area of the map
00111         //cout << "activeArea::size() " << activeArea.size() << endl;
00112         map.storage().setActiveArea(activeArea, true);
00113         m_activeAreaComputed=true;
00114 }
00115 */
00116 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00117         if (m_activeAreaComputed)
00118                 return;
00119         OrientedPoint lp=p;
00120         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00121         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00122         lp.theta+=m_laserPose.theta;
00123         IntPoint p0=map.world2map(lp);
00124         
00125         Point min(map.map2world(0,0));
00126         Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
00127                
00128         if (lp.x<min.x) min.x=lp.x;
00129         if (lp.y<min.y) min.y=lp.y;
00130         if (lp.x>max.x) max.x=lp.x;
00131         if (lp.y>max.y) max.y=lp.y;
00132         
00133         /*determine the size of the area*/
00134         const double * angle=m_laserAngles+m_initialBeamsSkip;
00135         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
00136                 if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
00137                 double d=*r>m_usableRange?m_usableRange:*r;
00138                 Point phit=lp;
00139                 phit.x+=d*cos(lp.theta+*angle);
00140                 phit.y+=d*sin(lp.theta+*angle);
00141                 if (phit.x<min.x) min.x=phit.x;
00142                 if (phit.y<min.y) min.y=phit.y;
00143                 if (phit.x>max.x) max.x=phit.x;
00144                 if (phit.y>max.y) max.y=phit.y;
00145         }
00146         //min=min-Point(map.getDelta(),map.getDelta());
00147         //max=max+Point(map.getDelta(),map.getDelta());
00148         
00149         if ( !map.isInside(min) || !map.isInside(max)){
00150                 Point lmin(map.map2world(0,0));
00151                 Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
00152                 //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
00153                 //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
00154                 min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
00155                 max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
00156                 min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
00157                 max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
00158                 map.resize(min.x, min.y, max.x, max.y);
00159                 //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
00160         }
00161         
00162         HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
00163         /*allocate the active area*/
00164         angle=m_laserAngles+m_initialBeamsSkip;
00165         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
00166                 if (m_generateMap){
00167                         double d=*r;
00168                         if (d>m_laserMaxRange||d==0.0||isnan(d))
00169                                 continue;
00170                         if (d>m_usableRange)
00171                                 d=m_usableRange;
00172                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00173                         IntPoint p0=map.world2map(lp);
00174                         IntPoint p1=map.world2map(phit);
00175                         
00176                         //IntPoint linePoints[20000] ;
00177                         GridLineTraversalLine line;
00178                         line.points=m_linePoints;
00179                         GridLineTraversal::gridLine(p0, p1, &line);
00180                         for (int i=0; i<line.num_points-1; i++){
00181                                 assert(map.isInside(m_linePoints[i]));
00182                                 activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
00183                                 assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
00184                         }
00185                         if (d<m_usableRange){
00186                                 IntPoint cp=map.storage().patchIndexes(p1);
00187                                 assert(cp.x>=0 && cp.y>=0);
00188                                 activeArea.insert(cp);
00189                         }
00190                 } else {
00191                         if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
00192                         Point phit=lp;
00193                         phit.x+=*r*cos(lp.theta+*angle);
00194                         phit.y+=*r*sin(lp.theta+*angle);
00195                         IntPoint p1=map.world2map(phit);
00196                         assert(p1.x>=0 && p1.y>=0);
00197                         IntPoint cp=map.storage().patchIndexes(p1);
00198                         assert(cp.x>=0 && cp.y>=0);
00199                         activeArea.insert(cp);
00200                 }
00201         
00202         //this allocates the unallocated cells in the active area of the map
00203         //cout << "activeArea::size() " << activeArea.size() << endl;
00204 /*      
00205         cerr << "ActiveArea=";
00206         for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
00207                 cerr << "(" << it->x <<"," << it->y << ") ";
00208         }
00209         cerr << endl;
00210 */              
00211         map.storage().setActiveArea(activeArea, true);
00212         m_activeAreaComputed=true;
00213 }
00214 
00215 double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00216         if (!m_activeAreaComputed)
00217                 computeActiveArea(map, p, readings);
00218                 
00219         //this operation replicates the cells that will be changed in the registration operation
00220         map.storage().allocActiveArea();
00221         
00222         OrientedPoint lp=p;
00223         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00224         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00225         lp.theta+=m_laserPose.theta;
00226         IntPoint p0=map.world2map(lp);
00227         
00228         
00229         const double * angle=m_laserAngles+m_initialBeamsSkip;
00230         double esum=0;
00231         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
00232                 if (m_generateMap){
00233                         double d=*r;
00234                         if (d>m_laserMaxRange||d==0.0||isnan(d))
00235                                 continue;
00236                         if (d>m_usableRange)
00237                                 d=m_usableRange;
00238                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00239                         IntPoint p1=map.world2map(phit);
00240                         //IntPoint linePoints[20000] ;
00241                         GridLineTraversalLine line;
00242                         line.points=m_linePoints;
00243                         GridLineTraversal::gridLine(p0, p1, &line);
00244                         for (int i=0; i<line.num_points-1; i++){
00245                                 PointAccumulator& cell=map.cell(line.points[i]);
00246                                 double e=-cell.entropy();
00247                                 cell.update(false, Point(0,0));
00248                                 e+=cell.entropy();
00249                                 esum+=e;
00250                         }
00251                         if (d<m_usableRange){
00252                                 double e=-map.cell(p1).entropy();
00253                                 map.cell(p1).update(true, phit);
00254                                 e+=map.cell(p1).entropy();
00255                                 esum+=e;
00256                         }
00257                 } else {
00258                         if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
00259                         Point phit=lp;
00260                         phit.x+=*r*cos(lp.theta+*angle);
00261                         phit.y+=*r*sin(lp.theta+*angle);
00262                         IntPoint p1=map.world2map(phit);
00263                         assert(p1.x>=0 && p1.y>=0);
00264                         map.cell(p1).update(true,phit);
00265                 }
00266         //cout  << "informationGain=" << -esum << endl;
00267         return esum;
00268 }
00269 
00270 /*
00271 void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00272         if (!m_activeAreaComputed)
00273                 computeActiveArea(map, p, readings);
00274                 
00275         //this operation replicates the cells that will be changed in the registration operation
00276         map.storage().allocActiveArea();
00277         
00278         OrientedPoint lp=p;
00279         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00280         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00281         lp.theta+=m_laserPose.theta;
00282         IntPoint p0=map.world2map(lp);
00283         const double * angle=m_laserAngles;
00284         for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
00285                 if (m_generateMap){     
00286                         double d=*r;
00287                         if (d>m_laserMaxRange)
00288                                 continue;
00289                         if (d>m_usableRange)
00290                                 d=m_usableRange;
00291                         Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
00292                         IntPoint p1=map.world2map(phit);
00293                         
00294                         IntPoint linePoints[20000] ;
00295                         GridLineTraversalLine line;
00296                         line.points=linePoints;
00297                         GridLineTraversal::gridLine(p0, p1, &line);
00298                         for (int i=0; i<line.num_points-1; i++){
00299                                 IntPoint ci=map.storage().patchIndexes(line.points[i]);
00300                                 if (map.storage().getActiveArea().find(ci)==map.storage().getActiveArea().end())
00301                                         cerr << "BIG ERROR" <<endl;
00302                                 map.cell(line.points[i]).update(false, Point(0,0));
00303                         }
00304                         if (d<=m_usableRange){
00305                                 
00306                                 map.cell(p1).update(true,phit);
00307                         }
00308                 } else {
00309                         if (*r>m_laserMaxRange||*r>m_usableRange) continue;
00310                         Point phit=lp;
00311                         phit.x+=*r*cos(lp.theta+*angle);
00312                         phit.y+=*r*sin(lp.theta+*angle);
00313                         map.cell(phit).update(true,phit);
00314                 }
00315 }
00316 
00317 */
00318 
00319 double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
00320         double currentScore;
00321         double sc=score(map, init, readings);;
00322         OrientedPoint start=init;
00323         pnew=init;
00324         int iterations=0;
00325         do{
00326                 currentScore=sc;
00327                 sc=icpStep(pnew, map, start, readings);
00328                 //cerr << "pstart=" << start.x << " " <<start.y << " " << start.theta << endl;
00329                 //cerr << "pret=" << pnew.x << " " <<pnew.y << " " << pnew.theta << endl;
00330                 start=pnew;
00331                 iterations++;
00332         } while (sc>currentScore);
00333         cerr << "i="<< iterations << endl;
00334         return currentScore;
00335 }
00336 
00337 double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
00338         double bestScore=-1;
00339         OrientedPoint currentPose=init;
00340         double currentScore=score(map, currentPose, readings);
00341         double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
00342         unsigned int refinement=0;
00343         enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
00344 /*      cout << __PRETTY_FUNCTION__<<  " readings: ";
00345         for (int i=0; i<m_laserBeams; i++){
00346                 cout << readings[i] << " ";
00347         }
00348         cout << endl;
00349 */      int c_iterations=0;
00350         do{
00351                 if (bestScore>=currentScore){
00352                         refinement++;
00353                         adelta*=.5;
00354                         ldelta*=.5;
00355                 }
00356                 bestScore=currentScore;
00357 //              cout <<"score="<< currentScore << " refinement=" << refinement;
00358 //              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
00359                 OrientedPoint bestLocalPose=currentPose;
00360                 OrientedPoint localPose=currentPose;
00361 
00362                 Move move=Front;
00363                 do {
00364                         localPose=currentPose;
00365                         switch(move){
00366                                 case Front:
00367                                         localPose.x+=ldelta;
00368                                         move=Back;
00369                                         break;
00370                                 case Back:
00371                                         localPose.x-=ldelta;
00372                                         move=Left;
00373                                         break;
00374                                 case Left:
00375                                         localPose.y-=ldelta;
00376                                         move=Right;
00377                                         break;
00378                                 case Right:
00379                                         localPose.y+=ldelta;
00380                                         move=TurnLeft;
00381                                         break;
00382                                 case TurnLeft:
00383                                         localPose.theta+=adelta;
00384                                         move=TurnRight;
00385                                         break;
00386                                 case TurnRight:
00387                                         localPose.theta-=adelta;
00388                                         move=Done;
00389                                         break;
00390                                 default:;
00391                         }
00392                         
00393                         double odo_gain=1;
00394                         if (m_angularOdometryReliability>0.){
00395                                 double dth=init.theta-localPose.theta;  dth=atan2(sin(dth), cos(dth));  dth*=dth;
00396                                 odo_gain*=exp(-m_angularOdometryReliability*dth);
00397                         }
00398                         if (m_linearOdometryReliability>0.){
00399                                 double dx=init.x-localPose.x;
00400                                 double dy=init.y-localPose.y;
00401                                 double drho=dx*dx+dy*dy;
00402                                 odo_gain*=exp(-m_linearOdometryReliability*drho);
00403                         }
00404                         double localScore=odo_gain*score(map, localPose, readings);
00405                         
00406                         if (localScore>currentScore){
00407                                 currentScore=localScore;
00408                                 bestLocalPose=localPose;
00409                         }
00410                         c_iterations++;
00411                 } while(move!=Done);
00412                 currentPose=bestLocalPose;
00413 //              cout << "currentScore=" << currentScore<< endl;
00414                 //here we look for the best move;
00415         }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
00416         //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
00417         //cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
00418         pnew=currentPose;
00419         return bestScore;
00420 }
00421 
00422 struct ScoredMove{
00423         OrientedPoint pose;
00424         double score;
00425         double likelihood;
00426 };
00427 
00428 typedef std::list<ScoredMove> ScoredMoveList;
00429 
00430 double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
00431         ScoredMoveList moveList;
00432         double bestScore=-1;
00433         OrientedPoint currentPose=init;
00434         ScoredMove sm={currentPose,0,0};
00435         unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
00436         double currentScore=sm.score;
00437         moveList.push_back(sm);
00438         double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
00439         unsigned int refinement=0;
00440         int count=0;
00441         enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
00442         do{
00443                 if (bestScore>=currentScore){
00444                         refinement++;
00445                         adelta*=.5;
00446                         ldelta*=.5;
00447                 }
00448                 bestScore=currentScore;
00449 //              cout <<"score="<< currentScore << " refinement=" << refinement;
00450 //              cout <<  "pose=" << currentPose.x  << " " << currentPose.y << " " << currentPose.theta << endl;
00451                 OrientedPoint bestLocalPose=currentPose;
00452                 OrientedPoint localPose=currentPose;
00453 
00454                 Move move=Front;
00455                 do {
00456                         localPose=currentPose;
00457                         switch(move){
00458                                 case Front:
00459                                         localPose.x+=ldelta;
00460                                         move=Back;
00461                                         break;
00462                                 case Back:
00463                                         localPose.x-=ldelta;
00464                                         move=Left;
00465                                         break;
00466                                 case Left:
00467                                         localPose.y-=ldelta;
00468                                         move=Right;
00469                                         break;
00470                                 case Right:
00471                                         localPose.y+=ldelta;
00472                                         move=TurnLeft;
00473                                         break;
00474                                 case TurnLeft:
00475                                         localPose.theta+=adelta;
00476                                         move=TurnRight;
00477                                         break;
00478                                 case TurnRight:
00479                                         localPose.theta-=adelta;
00480                                         move=Done;
00481                                         break;
00482                                 default:;
00483                         }
00484                         double localScore, localLikelihood;
00485                         
00486                         double odo_gain=1;
00487                         if (m_angularOdometryReliability>0.){
00488                                 double dth=init.theta-localPose.theta;  dth=atan2(sin(dth), cos(dth));  dth*=dth;
00489                                 odo_gain*=exp(-m_angularOdometryReliability*dth);
00490                         }
00491                         if (m_linearOdometryReliability>0.){
00492                                 double dx=init.x-localPose.x;
00493                                 double dy=init.y-localPose.y;
00494                                 double drho=dx*dx+dy*dy;
00495                                 odo_gain*=exp(-m_linearOdometryReliability*drho);
00496                         }
00497                         localScore=odo_gain*score(map, localPose, readings);
00498                         //update the score
00499                         count++;
00500                         matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
00501                         if (localScore>currentScore){
00502                                 currentScore=localScore;
00503                                 bestLocalPose=localPose;
00504                         }
00505                         sm.score=localScore;
00506                         sm.likelihood=localLikelihood;//+log(odo_gain);
00507                         sm.pose=localPose;
00508                         moveList.push_back(sm);
00509                         //update the move list
00510                 } while(move!=Done);
00511                 currentPose=bestLocalPose;
00512                 //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
00513                 //here we look for the best move;
00514         }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
00515         //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
00516         //cout << __PRETTY_FUNCTION__ << "iterations=" << count<< endl;
00517         
00518         //normalize the likelihood
00519         double lmin=1e9;
00520         double lmax=-1e9;
00521         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00522                 lmin=it->likelihood<lmin?it->likelihood:lmin;
00523                 lmax=it->likelihood>lmax?it->likelihood:lmax;
00524         }
00525         //cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
00526         for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
00527                 it->likelihood=exp(it->likelihood-lmax);
00528                 //cout << "l=" << it->likelihood << endl;
00529         }
00530         //compute the mean
00531         OrientedPoint mean(0,0,0);
00532         double lacc=0;
00533         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00534                 mean=mean+it->pose*it->likelihood;
00535                 lacc+=it->likelihood;
00536         }
00537         mean=mean*(1./lacc);
00538         //OrientedPoint delta=mean-currentPose;
00539         //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
00540         CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
00541         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00542                 OrientedPoint delta=it->pose-mean;
00543                 delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00544                 cov.xx+=delta.x*delta.x*it->likelihood;
00545                 cov.yy+=delta.y*delta.y*it->likelihood;
00546                 cov.tt+=delta.theta*delta.theta*it->likelihood;
00547                 cov.xy+=delta.x*delta.y*it->likelihood;
00548                 cov.xt+=delta.x*delta.theta*it->likelihood;
00549                 cov.yt+=delta.y*delta.theta*it->likelihood;
00550         }
00551         cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
00552         
00553         _mean=currentPose;
00554         _cov=cov;
00555         return bestScore;
00556 }
00557 
00558 void ScanMatcher::setLaserParameters
00559         (unsigned int beams, double* angles, const OrientedPoint& lpose){
00560         /*if (m_laserAngles)
00561                 delete [] m_laserAngles;
00562         */
00563         assert(beams<LASER_MAXBEAMS);
00564         m_laserPose=lpose;
00565         m_laserBeams=beams;
00566         //m_laserAngles=new double[beams];
00567         memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);     
00568 }
00569         
00570 
00571 double ScanMatcher::likelihood
00572         (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
00573         ScoredMoveList moveList;
00574         
00575         for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
00576         for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
00577         for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
00578                 
00579                 OrientedPoint rp=p;
00580                 rp.x+=xx;
00581                 rp.y+=yy;
00582                 rp.theta+=tt;
00583                 
00584                 ScoredMove sm;
00585                 sm.pose=rp;
00586                 
00587                 likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
00588                 moveList.push_back(sm);
00589         }
00590         
00591         //OrientedPoint delta=mean-currentPose;
00592         //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
00593         //normalize the likelihood
00594         double lmax=-1e9;
00595         double lcum=0;
00596         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00597                 lmax=it->likelihood>lmax?it->likelihood:lmax;
00598         }
00599         for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
00600                 //it->likelihood=exp(it->likelihood-lmax);
00601                 lcum+=exp(it->likelihood-lmax);
00602                 it->likelihood=exp(it->likelihood-lmax);
00603                 //cout << "l=" << it->likelihood << endl;
00604         }
00605         
00606         OrientedPoint mean(0,0,0);
00607         double s=0,c=0;
00608         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00609                 mean=mean+it->pose*it->likelihood;
00610                 s+=it->likelihood*sin(it->pose.theta);
00611                 c+=it->likelihood*cos(it->pose.theta);
00612         }
00613         mean=mean*(1./lcum);
00614         s/=lcum;
00615         c/=lcum;
00616         mean.theta=atan2(s,c);
00617         
00618         
00619         CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
00620         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00621                 OrientedPoint delta=it->pose-mean;
00622                 delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00623                 cov.xx+=delta.x*delta.x*it->likelihood;
00624                 cov.yy+=delta.y*delta.y*it->likelihood;
00625                 cov.tt+=delta.theta*delta.theta*it->likelihood;
00626                 cov.xy+=delta.x*delta.y*it->likelihood;
00627                 cov.xt+=delta.x*delta.theta*it->likelihood;
00628                 cov.yt+=delta.y*delta.theta*it->likelihood;
00629         }
00630         cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
00631         
00632         _mean=mean;
00633         _cov=cov;
00634         _lmax=lmax;
00635         return log(lcum)+lmax;
00636 }
00637 
00638 double ScanMatcher::likelihood
00639         (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p,
00640         Gaussian3& odometry, const double* readings, double gain){
00641         ScoredMoveList moveList;
00642         
00643         
00644         for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
00645         for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
00646         for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
00647                 
00648                 OrientedPoint rp=p;
00649                 rp.x+=xx;
00650                 rp.y+=yy;
00651                 rp.theta+=tt;
00652                 
00653                 ScoredMove sm;
00654                 sm.pose=rp;
00655                 
00656                 likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
00657                 sm.likelihood+=odometry.eval(rp)/gain;
00658                 assert(!isnan(sm.likelihood));
00659                 moveList.push_back(sm);
00660         }
00661         
00662         //OrientedPoint delta=mean-currentPose;
00663         //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
00664         //normalize the likelihood
00665   double lmax=-std::numeric_limits<double>::max();
00666         double lcum=0;
00667         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00668                 lmax=it->likelihood>lmax?it->likelihood:lmax;
00669         }
00670         for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
00671                 //it->likelihood=exp(it->likelihood-lmax);
00672                 lcum+=exp(it->likelihood-lmax);
00673                 it->likelihood=exp(it->likelihood-lmax);
00674                 //cout << "l=" << it->likelihood << endl;
00675         }
00676         
00677         OrientedPoint mean(0,0,0);
00678         double s=0,c=0;
00679         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00680                 mean=mean+it->pose*it->likelihood;
00681                 s+=it->likelihood*sin(it->pose.theta);
00682                 c+=it->likelihood*cos(it->pose.theta);
00683         }
00684         mean=mean*(1./lcum);
00685         s/=lcum;
00686         c/=lcum;
00687         mean.theta=atan2(s,c);
00688         
00689         
00690         CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
00691         for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
00692                 OrientedPoint delta=it->pose-mean;
00693                 delta.theta=atan2(sin(delta.theta), cos(delta.theta));
00694                 cov.xx+=delta.x*delta.x*it->likelihood;
00695                 cov.yy+=delta.y*delta.y*it->likelihood;
00696                 cov.tt+=delta.theta*delta.theta*it->likelihood;
00697                 cov.xy+=delta.x*delta.y*it->likelihood;
00698                 cov.xt+=delta.x*delta.theta*it->likelihood;
00699                 cov.yt+=delta.y*delta.theta*it->likelihood;
00700         }
00701         cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
00702         
00703         _mean=mean;
00704         _cov=cov;
00705         _lmax=lmax;
00706         double v=log(lcum)+lmax;
00707         assert(!isnan(v));
00708         return v;
00709 }
00710 
00711 void ScanMatcher::setMatchingParameters
00712         (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations,  double likelihoodSigma, unsigned int likelihoodSkip){      
00713         m_usableRange=urange;
00714         m_laserMaxRange=range;
00715         m_kernelSize=kernsize;
00716         m_optLinearDelta=lopt;
00717         m_optAngularDelta=aopt;
00718         m_optRecursiveIterations=iterations;
00719         m_gaussianSigma=sigma;
00720         m_likelihoodSigma=likelihoodSigma;
00721         m_likelihoodSkip=likelihoodSkip;
00722 }
00723 
00724 };
00725 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13