scanmatcher.h
Go to the documentation of this file.
00001 #ifndef SCANMATCHER_H
00002 #define SCANMATCHER_H
00003 
00004 #include "icp.h"
00005 #include "smmap.h"
00006 #include <gmapping/utils/macro_params.h>
00007 #include <gmapping/utils/stat.h>
00008 #include <iostream>
00009 #include <gmapping/utils/gvalues.h>
00010 #define LASER_MAXBEAMS 2048
00011 
00012 namespace GMapping {
00013 
00014 class ScanMatcher{
00015         public:
00016                 typedef Covariance3 CovarianceMatrix;
00017                 
00018                 ScanMatcher();
00019                 ~ScanMatcher();
00020                 double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00021                 double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00022                 double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00023                 
00024                 double   registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
00025                 void setLaserParameters
00026                         (unsigned int beams, double* angles, const OrientedPoint& lpose);
00027                 void setMatchingParameters
00028                         (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );
00029                 void invalidateActiveArea();
00030                 void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
00031 
00032                 inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00033                 inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00034                 inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
00035                 double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
00036                 double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
00037                 inline const double* laserAngles() const { return m_laserAngles; }
00038                 inline unsigned int laserBeams() const { return m_laserBeams; }
00039                 
00040                 static const double nullLikelihood;
00041         protected:
00042                 //state of the matcher
00043                 bool m_activeAreaComputed;
00044                 
00046                 unsigned int m_laserBeams;
00047                 double       m_laserAngles[LASER_MAXBEAMS];
00048                 //OrientedPoint m_laserPose;
00049                 PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
00050                 PARAM_SET_GET(double, laserMaxRange, protected, public, public)
00052                 PARAM_SET_GET(double, usableRange, protected, public, public)
00053                 PARAM_SET_GET(double, gaussianSigma, protected, public, public)
00054                 PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
00055                 PARAM_SET_GET(int,    kernelSize, protected, public, public)
00056                 PARAM_SET_GET(double, optAngularDelta, protected, public, public)
00057                 PARAM_SET_GET(double, optLinearDelta, protected, public, public)
00058                 PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)
00059                 PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
00060                 PARAM_SET_GET(double, llsamplerange, protected, public, public)
00061                 PARAM_SET_GET(double, llsamplestep, protected, public, public)
00062                 PARAM_SET_GET(double, lasamplerange, protected, public, public)
00063                 PARAM_SET_GET(double, lasamplestep, protected, public, public)
00064                 PARAM_SET_GET(bool, generateMap, protected, public, public)
00065                 PARAM_SET_GET(double, enlargeStep, protected, public, public)
00066                 PARAM_SET_GET(double, fullnessThreshold, protected, public, public)
00067                 PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)
00068                 PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)
00069                 PARAM_SET_GET(double, freeCellRatio, protected, public, public)
00070                 PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)
00071 
00072                 // allocate this large array only once
00073                 IntPoint* m_linePoints;
00074 };
00075 
00076 inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
00077         const double * angle=m_laserAngles+m_initialBeamsSkip;
00078         OrientedPoint lp=p;
00079         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00080         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00081         lp.theta+=m_laserPose.theta;
00082         unsigned int skip=0;
00083         double freeDelta=map.getDelta()*m_freeCellRatio;
00084         std::list<PointPair> pairs;
00085         
00086         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
00087                 skip++;
00088                 skip=skip>m_likelihoodSkip?0:skip;
00089                 if (*r>m_usableRange||*r==0.0) continue;
00090                 if (skip) continue;
00091                 Point phit=lp;
00092                 phit.x+=*r*cos(lp.theta+*angle);
00093                 phit.y+=*r*sin(lp.theta+*angle);
00094                 IntPoint iphit=map.world2map(phit);
00095                 Point pfree=lp;
00096                 pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
00097                 pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
00098                 pfree=pfree-phit;
00099                 IntPoint ipfree=map.world2map(pfree);
00100                 bool found=false;
00101                 Point bestMu(0.,0.);
00102                 Point bestCell(0.,0.);
00103                 for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
00104                 for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
00105                         IntPoint pr=iphit+IntPoint(xx,yy);
00106                         IntPoint pf=pr+ipfree;
00107                         //AccessibilityState s=map.storage().cellState(pr);
00108                         //if (s&Inside && s&Allocated){
00109                                 const PointAccumulator& cell=map.cell(pr);
00110                                 const PointAccumulator& fcell=map.cell(pf);
00111                                 if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
00112                                         Point mu=phit-cell.mean();
00113                                         if (!found){
00114                                                 bestMu=mu;
00115                                                 bestCell=cell.mean();
00116                                                 found=true;
00117                                         }else
00118                                                 if((mu*mu)<(bestMu*bestMu)){
00119                                                         bestMu=mu;
00120                                                         bestCell=cell.mean();
00121                                                 } 
00122                                                 
00123                                 }
00124                         //}
00125                 }
00126                 if (found){
00127                         pairs.push_back(std::make_pair(phit, bestCell));
00128                         //std::cerr << "(" << phit.x-bestCell.x << "," << phit.y-bestCell.y << ") ";
00129                 }
00130                 //std::cerr << std::endl;
00131         }
00132         
00133         OrientedPoint result(0,0,0);
00134         //double icpError=icpNonlinearStep(result,pairs);
00135         std::cerr << "result(" << pairs.size() << ")=" << result.x << " " << result.y << " " << result.theta << std::endl;
00136         pret.x=p.x+result.x;
00137         pret.y=p.y+result.y;
00138         pret.theta=p.theta+result.theta;
00139         pret.theta=atan2(sin(pret.theta), cos(pret.theta));
00140         return score(map, p, readings);
00141 }
00142 
00143 inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
00144         double s=0;
00145         const double * angle=m_laserAngles+m_initialBeamsSkip;
00146         OrientedPoint lp=p;
00147         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00148         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00149         lp.theta+=m_laserPose.theta;
00150         unsigned int skip=0;
00151         double freeDelta=map.getDelta()*m_freeCellRatio;
00152         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
00153                 skip++;
00154                 skip=skip>m_likelihoodSkip?0:skip;
00155                 if (skip||*r>m_usableRange||*r==0.0) continue;
00156                 Point phit=lp;
00157                 phit.x+=*r*cos(lp.theta+*angle);
00158                 phit.y+=*r*sin(lp.theta+*angle);
00159                 IntPoint iphit=map.world2map(phit);
00160                 Point pfree=lp;
00161                 pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
00162                 pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
00163                 pfree=pfree-phit;
00164                 IntPoint ipfree=map.world2map(pfree);
00165                 bool found=false;
00166                 Point bestMu(0.,0.);
00167                 for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
00168                 for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
00169                         IntPoint pr=iphit+IntPoint(xx,yy);
00170                         IntPoint pf=pr+ipfree;
00171                         //AccessibilityState s=map.storage().cellState(pr);
00172                         //if (s&Inside && s&Allocated){
00173                                 const PointAccumulator& cell=map.cell(pr);
00174                                 const PointAccumulator& fcell=map.cell(pf);
00175                                 if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
00176                                         Point mu=phit-cell.mean();
00177                                         if (!found){
00178                                                 bestMu=mu;
00179                                                 found=true;
00180                                         }else
00181                                                 bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
00182                                 }
00183                         //}
00184                 }
00185                 if (found)
00186                         s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
00187         }
00188         return s;
00189 }
00190 
00191 inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
00192         using namespace std;
00193         l=0;
00194         s=0;
00195         const double * angle=m_laserAngles+m_initialBeamsSkip;
00196         OrientedPoint lp=p;
00197         lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00198         lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00199         lp.theta+=m_laserPose.theta;
00200         double noHit=nullLikelihood/(m_likelihoodSigma);
00201         unsigned int skip=0;
00202         unsigned int c=0;
00203         double freeDelta=map.getDelta()*m_freeCellRatio;
00204         for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
00205                 skip++;
00206                 skip=skip>m_likelihoodSkip?0:skip;
00207                 if (*r>m_usableRange) continue;
00208                 if (skip) continue;
00209                 Point phit=lp;
00210                 phit.x+=*r*cos(lp.theta+*angle);
00211                 phit.y+=*r*sin(lp.theta+*angle);
00212                 IntPoint iphit=map.world2map(phit);
00213                 Point pfree=lp;
00214                 pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
00215                 pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
00216                 pfree=pfree-phit;
00217                 IntPoint ipfree=map.world2map(pfree);
00218                 bool found=false;
00219                 Point bestMu(0.,0.);
00220                 for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
00221                 for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
00222                         IntPoint pr=iphit+IntPoint(xx,yy);
00223                         IntPoint pf=pr+ipfree;
00224                         //AccessibilityState s=map.storage().cellState(pr);
00225                         //if (s&Inside && s&Allocated){
00226                                 const PointAccumulator& cell=map.cell(pr);
00227                                 const PointAccumulator& fcell=map.cell(pf);
00228                                 if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
00229                                         Point mu=phit-cell.mean();
00230                                         if (!found){
00231                                                 bestMu=mu;
00232                                                 found=true;
00233                                         }else
00234                                                 bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
00235                                 }
00236                         //}     
00237                 }
00238                 if (found){
00239                         s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
00240                         c++;
00241                 }
00242                 if (!skip){
00243                         double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
00244                         l+=(found)?f:noHit;
00245                 }
00246         }
00247         return c;
00248 }
00249 
00250 };
00251 
00252 #endif


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