scanmatcher.h
Go to the documentation of this file.
1 #ifndef SCANMATCHER_H
2 #define SCANMATCHER_H
3 
4 #include "icp.h"
5 #include "smmap.h"
6 #include <gmapping/utils/macro_params.h>
7 #include <gmapping/utils/stat.h>
8 #include <iostream>
9 #include <gmapping/utils/gvalues.h>
10 #define LASER_MAXBEAMS 2048
11 
12 namespace GMapping {
13 
14 class ScanMatcher{
15  public:
16  typedef Covariance3 CovarianceMatrix;
17 
18  ScanMatcher();
19  ~ScanMatcher();
20  double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
21  double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
22  double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
23 
24  double registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
25  void setLaserParameters
26  (unsigned int beams, double* angles, const OrientedPoint& lpose);
27  void setMatchingParameters
28  (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );
29  void invalidateActiveArea();
30  void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
31 
32  inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
33  inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
34  inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const;
35  double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings);
36  double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.);
37  inline const double* laserAngles() const { return m_laserAngles; }
38  inline unsigned int laserBeams() const { return m_laserBeams; }
39 
40  static const double nullLikelihood;
41  protected:
42  //state of the matcher
43  bool m_activeAreaComputed;
44 
46  unsigned int m_laserBeams;
47  double m_laserAngles[LASER_MAXBEAMS];
48  //OrientedPoint m_laserPose;
49  PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public)
50  PARAM_SET_GET(double, laserMaxRange, protected, public, public)
52  PARAM_SET_GET(double, usableRange, protected, public, public)
53  PARAM_SET_GET(double, gaussianSigma, protected, public, public)
54  PARAM_SET_GET(double, likelihoodSigma, protected, public, public)
55  PARAM_SET_GET(int, kernelSize, protected, public, public)
56  PARAM_SET_GET(double, optAngularDelta, protected, public, public)
57  PARAM_SET_GET(double, optLinearDelta, protected, public, public)
58  PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public)
59  PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public)
60  PARAM_SET_GET(double, llsamplerange, protected, public, public)
61  PARAM_SET_GET(double, llsamplestep, protected, public, public)
62  PARAM_SET_GET(double, lasamplerange, protected, public, public)
63  PARAM_SET_GET(double, lasamplestep, protected, public, public)
64  PARAM_SET_GET(bool, generateMap, protected, public, public)
65  PARAM_SET_GET(double, enlargeStep, protected, public, public)
66  PARAM_SET_GET(double, fullnessThreshold, protected, public, public)
67  PARAM_SET_GET(double, angularOdometryReliability, protected, public, public)
68  PARAM_SET_GET(double, linearOdometryReliability, protected, public, public)
69  PARAM_SET_GET(double, freeCellRatio, protected, public, public)
70  PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public)
71 
72  // allocate this large array only once
73  IntPoint* m_linePoints;
74 };
75 
76 inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
77  const double * angle=m_laserAngles+m_initialBeamsSkip;
78  OrientedPoint lp=p;
79  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
80  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
81  lp.theta+=m_laserPose.theta;
82  unsigned int skip=0;
83  double freeDelta=map.getDelta()*m_freeCellRatio;
84  std::list<PointPair> pairs;
85 
86  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
87  skip++;
88  skip=skip>m_likelihoodSkip?0:skip;
89  if (*r>m_usableRange||*r==0.0) continue;
90  if (skip) continue;
91  Point phit=lp;
92  phit.x+=*r*cos(lp.theta+*angle);
93  phit.y+=*r*sin(lp.theta+*angle);
94  IntPoint iphit=map.world2map(phit);
95  Point pfree=lp;
96  pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
97  pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
98  pfree=pfree-phit;
99  IntPoint ipfree=map.world2map(pfree);
100  bool found=false;
101  Point bestMu(0.,0.);
102  Point bestCell(0.,0.);
103  for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
104  for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
105  IntPoint pr=iphit+IntPoint(xx,yy);
106  IntPoint pf=pr+ipfree;
107  //AccessibilityState s=map.storage().cellState(pr);
108  //if (s&Inside && s&Allocated){
109  const PointAccumulator& cell=map.cell(pr);
110  const PointAccumulator& fcell=map.cell(pf);
111  if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
112  Point mu=phit-cell.mean();
113  if (!found){
114  bestMu=mu;
115  bestCell=cell.mean();
116  found=true;
117  }else
118  if((mu*mu)<(bestMu*bestMu)){
119  bestMu=mu;
120  bestCell=cell.mean();
121  }
122 
123  }
124  //}
125  }
126  if (found){
127  pairs.push_back(std::make_pair(phit, bestCell));
128  //std::cerr << "(" << phit.x-bestCell.x << "," << phit.y-bestCell.y << ") ";
129  }
130  //std::cerr << std::endl;
131  }
132 
133  OrientedPoint result(0,0,0);
134  //double icpError=icpNonlinearStep(result,pairs);
135  std::cerr << "result(" << pairs.size() << ")=" << result.x << " " << result.y << " " << result.theta << std::endl;
136  pret.x=p.x+result.x;
137  pret.y=p.y+result.y;
138  pret.theta=p.theta+result.theta;
139  pret.theta=atan2(sin(pret.theta), cos(pret.theta));
140  return score(map, p, readings);
141 }
142 
143 inline double ScanMatcher::score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
144  double s=0;
145  const double * angle=m_laserAngles+m_initialBeamsSkip;
146  OrientedPoint lp=p;
147  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
148  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
149  lp.theta+=m_laserPose.theta;
150  unsigned int skip=0;
151  double freeDelta=map.getDelta()*m_freeCellRatio;
152  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
153  skip++;
154  skip=skip>m_likelihoodSkip?0:skip;
155  if (skip||*r>m_usableRange||*r==0.0) continue;
156  Point phit=lp;
157  phit.x+=*r*cos(lp.theta+*angle);
158  phit.y+=*r*sin(lp.theta+*angle);
159  IntPoint iphit=map.world2map(phit);
160  Point pfree=lp;
161  pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle);
162  pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle);
163  pfree=pfree-phit;
164  IntPoint ipfree=map.world2map(pfree);
165  bool found=false;
166  Point bestMu(0.,0.);
167  for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
168  for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
169  IntPoint pr=iphit+IntPoint(xx,yy);
170  IntPoint pf=pr+ipfree;
171  //AccessibilityState s=map.storage().cellState(pr);
172  //if (s&Inside && s&Allocated){
173  const PointAccumulator& cell=map.cell(pr);
174  const PointAccumulator& fcell=map.cell(pf);
175  if (((double)cell )> m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
176  Point mu=phit-cell.mean();
177  if (!found){
178  bestMu=mu;
179  found=true;
180  }else
181  bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
182  }
183  //}
184  }
185  if (found)
186  s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
187  }
188  return s;
189 }
190 
191 inline unsigned int ScanMatcher::likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{
192  using namespace std;
193  l=0;
194  s=0;
195  const double * angle=m_laserAngles+m_initialBeamsSkip;
196  OrientedPoint lp=p;
197  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
198  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
199  lp.theta+=m_laserPose.theta;
200  double noHit=nullLikelihood/(m_likelihoodSigma);
201  unsigned int skip=0;
202  unsigned int c=0;
203  double freeDelta=map.getDelta()*m_freeCellRatio;
204  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
205  skip++;
206  skip=skip>m_likelihoodSkip?0:skip;
207  if (*r>m_usableRange) continue;
208  if (skip) continue;
209  Point phit=lp;
210  phit.x+=*r*cos(lp.theta+*angle);
211  phit.y+=*r*sin(lp.theta+*angle);
212  IntPoint iphit=map.world2map(phit);
213  Point pfree=lp;
214  pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle);
215  pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle);
216  pfree=pfree-phit;
217  IntPoint ipfree=map.world2map(pfree);
218  bool found=false;
219  Point bestMu(0.,0.);
220  for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++)
221  for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){
222  IntPoint pr=iphit+IntPoint(xx,yy);
223  IntPoint pf=pr+ipfree;
224  //AccessibilityState s=map.storage().cellState(pr);
225  //if (s&Inside && s&Allocated){
226  const PointAccumulator& cell=map.cell(pr);
227  const PointAccumulator& fcell=map.cell(pf);
228  if (((double)cell )>m_fullnessThreshold && ((double)fcell )<m_fullnessThreshold){
229  Point mu=phit-cell.mean();
230  if (!found){
231  bestMu=mu;
232  found=true;
233  }else
234  bestMu=(mu*mu)<(bestMu*bestMu)?mu:bestMu;
235  }
236  //}
237  }
238  if (found){
239  s+=exp(-1./m_gaussianSigma*bestMu*bestMu);
240  c++;
241  }
242  if (!skip){
243  double f=(-1./m_likelihoodSigma)*(bestMu*bestMu);
244  l+=(found)?f:noHit;
245  }
246  }
247  return c;
248 }
249 
250 };
251 
252 #endif
GMapping::ScanMatcher::optimize
double optimize(OrientedPoint &pnew, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.cpp:337
GMapping::ScanMatcher::registerScan
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:215
GMapping::Map::cell
Cell & cell(int x, int y)
Definition: map.h:46
GMapping::ScanMatcher::icpStep
double icpStep(OrientedPoint &pret, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.h:76
PARAM_SET_GET
#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)
Definition: macro_params.h:4
GMapping::ScanMatcher::likelihoodAndScore
unsigned int likelihoodAndScore(double &s, double &l, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.h:191
LASER_MAXBEAMS
#define LASER_MAXBEAMS
Definition: scanmatcher.h:10
GMapping::IntPoint
point< int > IntPoint
Definition: point.h:201
GMapping::ScanMatcher::laserAngles
const double * laserAngles() const
Definition: scanmatcher.h:37
macro_params.h
GMapping::ScanMatcher::setLaserParameters
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
Definition: scanmatcher.cpp:559
GMapping::Map::world2map
IntPoint world2map(const Point &p) const
Definition: map.h:179
GMapping::ScanMatcher::setMatchingParameters
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0)
Definition: scanmatcher.cpp:712
c
unsigned int c
Definition: gfs2stream.cpp:41
GMapping::ScanMatcher::m_laserAngles
double m_laserAngles[LASER_MAXBEAMS]
Definition: scanmatcher.h:47
GMapping::ScanMatcher::m_laserBeams
unsigned int m_laserBeams
Definition: scanmatcher.h:46
GMapping::ScanMatcher::icpOptimize
double icpOptimize(OrientedPoint &pnew, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.cpp:319
GMapping::ScanMatcher::m_linePoints
IntPoint * m_linePoints
Definition: scanmatcher.h:73
GMapping::ScanMatcher::score
double score(const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.h:143
GMapping::ScanMatcher::laserBeams
unsigned int laserBeams() const
Definition: scanmatcher.h:38
GMapping::ScanMatcher::nullLikelihood
static const double nullLikelihood
Definition: scanmatcher.h:40
GMapping::Map::getDelta
double getDelta() const
Definition: map.h:38
GMapping::ScanMatcher::likelihood
double likelihood(double &lmax, OrientedPoint &mean, CovarianceMatrix &cov, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:572
GMapping::ScanMatcher::CovarianceMatrix
Covariance3 CovarianceMatrix
Definition: scanmatcher.h:16
GMapping::ScanMatcher::computeActiveArea
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:116
skip
double skip
Definition: gfs2stream.cpp:20

openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22