00001 #ifndef SCANMATCHER_H
00002 #define SCANMATCHER_H
00003
00004 #include "icp.h"
00005 #include "smmap.h"
00006 #include <utils/macro_params.h>
00007 #include <utils/stat.h>
00008 #include <iostream>
00009 #include <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
00043 bool m_activeAreaComputed;
00044
00046 unsigned int m_laserBeams;
00047 double m_laserAngles[LASER_MAXBEAMS];
00048
00049
00050 PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
00051 PARAM_SET_GET(double, laserMaxRange, protected, public, public)
00053 PARAM_SET_GET(double, usableRange, protected, public, public)
00054 PARAM_SET_GET(double, gaussianSigma, protected, public, public)
00055 PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
00056 PARAM_SET_GET(int, kernelSize, protected, public, public)
00057 PARAM_SET_GET(double, optAngularDelta, protected, public, public)
00058 PARAM_SET_GET(double, optLinearDelta, protected, public, public)
00059 PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)
00060 PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
00061 PARAM_SET_GET(double, llsamplerange, protected, public, public)
00062 PARAM_SET_GET(double, llsamplestep, protected, public, public)
00063 PARAM_SET_GET(double, lasamplerange, protected, public, public)
00064 PARAM_SET_GET(double, lasamplestep, protected, public, public)
00065 PARAM_SET_GET(bool, generateMap, protected, public, public)
00066 PARAM_SET_GET(double, enlargeStep, protected, public, public)
00067 PARAM_SET_GET(double, fullnessThreshold, protected, public, public)
00068 PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)
00069 PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)
00070 PARAM_SET_GET(double, freeCellRatio, protected, public, public)
00071 PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)
00072
00073
00074 IntPoint* m_linePoints;
00075 };
00076
00077 inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
00078 const double * angle=m_laserAngles+m_initialBeamsSkip;
00079 OrientedPoint lp=p;
00080 lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
00081 lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
00082 lp.theta+=m_laserPose.theta;
00083 unsigned int skip=0;
00084 double freeDelta=map.getDelta()*m_freeCellRatio;
00085 std::list<PointPair> pairs;
00086
00087 for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
00088 skip++;
00089 skip=skip>m_likelihoodSkip?0:skip;
00090 if (skip||*r>m_usableRange||*r==0.0) 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
00108
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
00129 }
00130
00131 }
00132
00133 OrientedPoint result(0,0,0);
00134
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
00172
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
00225
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
