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

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20