scanmatcher.new.cpp
Go to the documentation of this file.
1 #include <list>
2 #include <iostream>
3 
4 #include "gmapping/scanmatcher/scanmatcher.h"
5 #include "gmapping/scanmatcher/gridlinetraversal.h"
6 //#define GENERATE_MAPS
7 
8 using namespace std;
9 namespace GMapping {
10 
11 const double ScanMatcher::nullLikelihood=-1.;
12 
13 ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
14  m_laserAngles=0;
15  m_laserBeams=0;
16  m_optRecursiveIterations=3;
17  m_activeAreaComputed=false;
18 
19  // This are the dafault settings for a grid map of 5 cm
20  m_llsamplerange=0.01;
21  m_llsamplestep=0.01;
22  m_lasamplerange=0.005;
23  m_lasamplestep=0.005;
24 /*
25  // This are the dafault settings for a grid map of 10 cm
26  m_llsamplerange=0.1;
27  m_llsamplestep=0.1;
28  m_lasamplerange=0.02;
29  m_lasamplestep=0.01;
30 */
31  // This are the dafault settings for a grid map of 20/25 cm
32 /*
33  m_llsamplerange=0.2;
34  m_llsamplestep=0.1;
35  m_lasamplerange=0.02;
36  m_lasamplestep=0.01;
37  m_generateMap=false;
38 */
39 }
40 
41 ScanMatcher::~ScanMatcher(){
42  if (m_laserAngles)
43  delete [] m_laserAngles;
44 }
45 
46 void ScanMatcher::invalidateActiveArea(){
47  m_activeAreaComputed=false;
48 }
49 
50 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
51  if (m_activeAreaComputed)
52  return;
53  HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
54  OrientedPoint lp=p;
55  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
56  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
57  lp.theta+=m_laserPose.theta;
58  IntPoint p0=map.world2map(lp);
59  const double * angle=m_laserAngles;
60  for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
61  if (m_generateMap){
62  double d=*r;
63  if (d>m_laserMaxRange)
64  continue;
65  if (d>m_usableRange)
66  d=m_usableRange;
67 
68  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
69  IntPoint p1=map.world2map(phit);
70 
71  d+=map.getDelta();
72  //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
73  //IntPoint p2=map.world2map(phit2);
74  IntPoint linePoints[20000] ;
75  GridLineTraversalLine line;
76  line.points=linePoints;
77  //GridLineTraversal::gridLine(p0, p2, &line);
78  GridLineTraversal::gridLine(p0, p1, &line);
79  for (int i=0; i<line.num_points-1; i++){
80  activeArea.insert(map.storage().patchIndexes(linePoints[i]));
81  }
82  if (d<=m_usableRange){
83  activeArea.insert(map.storage().patchIndexes(p1));
84  //activeArea.insert(map.storage().patchIndexes(p2));
85  }
86  } else {
87  if (*r>m_laserMaxRange||*r>m_usableRange) continue;
88  Point phit=lp;
89  phit.x+=*r*cos(lp.theta+*angle);
90  phit.y+=*r*sin(lp.theta+*angle);
91  IntPoint p1=map.world2map(phit);
92  assert(p1.x>=0 && p1.y>=0);
93  IntPoint cp=map.storage().patchIndexes(p1);
94  assert(cp.x>=0 && cp.y>=0);
95  activeArea.insert(cp);
96 
97  }
98  //this allocates the unallocated cells in the active area of the map
99  //cout << "activeArea::size() " << activeArea.size() << endl;
100  map.storage().setActiveArea(activeArea, true);
101  m_activeAreaComputed=true;
102 }
103 
104 void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
105  if (!m_activeAreaComputed)
106  computeActiveArea(map, p, readings);
107 
108  //this operation replicates the cells that will be changed in the registration operation
109  map.storage().allocActiveArea();
110 
111  OrientedPoint lp=p;
112  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
113  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
114  lp.theta+=m_laserPose.theta;
115  IntPoint p0=map.world2map(lp);
116  const double * angle=m_laserAngles;
117  for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
118  if (m_generateMap){
119  double d=*r;
120  if (d>m_laserMaxRange)
121  continue;
122  if (d>m_usableRange)
123  d=m_usableRange;
124  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
125  IntPoint p1=map.world2map(phit);
126 
127  d+=map.getDelta();
128  //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
129  //IntPoint p2=map.world2map(phit2);
130  IntPoint linePoints[20000] ;
131  GridLineTraversalLine line;
132  line.points=linePoints;
133  //GridLineTraversal::gridLine(p0, p2, &line);
134  GridLineTraversal::gridLine(p0, p1, &line);
135  for (int i=0; i<line.num_points-1; i++){
136  map.cell(line.points[i]).update(false, Point(0,0));
137  }
138  if (d<=m_usableRange){
139  map.cell(p1).update(true,phit);
140  // map.cell(p2).update(true,phit);
141  }
142  } else {
143  if (*r>m_laserMaxRange||*r>m_usableRange) continue;
144  Point phit=lp;
145  phit.x+=*r*cos(lp.theta+*angle);
146  phit.y+=*r*sin(lp.theta+*angle);
147  map.cell(phit).update(true,phit);
148  }
149 }
150 
151 
152 
153 double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
154  double bestScore=-1;
155  OrientedPoint currentPose=init;
156  double currentScore=score(map, currentPose, readings);
157  double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
158  unsigned int refinement=0;
159  enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
160  int c_iterations=0;
161  do{
162  if (bestScore>=currentScore){
163  refinement++;
164  adelta*=.5;
165  ldelta*=.5;
166  }
167  bestScore=currentScore;
168 // cout <<"score="<< currentScore << " refinement=" << refinement;
169 // cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
170  OrientedPoint bestLocalPose=currentPose;
171  OrientedPoint localPose=currentPose;
172 
173  Move move=Front;
174  do {
175  localPose=currentPose;
176  switch(move){
177  case Front:
178  localPose.x+=ldelta;
179  move=Back;
180  break;
181  case Back:
182  localPose.x-=ldelta;
183  move=Left;
184  break;
185  case Left:
186  localPose.y-=ldelta;
187  move=Right;
188  break;
189  case Right:
190  localPose.y+=ldelta;
191  move=TurnLeft;
192  break;
193  case TurnLeft:
194  localPose.theta+=adelta;
195  move=TurnRight;
196  break;
197  case TurnRight:
198  localPose.theta-=adelta;
199  move=Done;
200  break;
201  default:;
202  }
203  double localScore=score(map, localPose, readings);
204  if (localScore>currentScore){
205  currentScore=localScore;
206  bestLocalPose=localPose;
207  }
208  c_iterations++;
209  } while(move!=Done);
210  currentPose=bestLocalPose;
211  //cout << __func__ << "currentScore=" << currentScore<< endl;
212  //here we look for the best move;
213  }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
214  //cout << __func__ << "bestScore=" << bestScore<< endl;
215  //cout << __func__ << "iterations=" << c_iterations<< endl;
216  pnew=currentPose;
217  return bestScore;
218 }
219 
220 struct ScoredMove{
221  OrientedPoint pose;
222  double score;
223  double likelihood;
224 };
225 
226 typedef std::list<ScoredMove> ScoredMoveList;
227 
228 double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
229  ScoredMoveList moveList;
230  double bestScore=-1;
231  OrientedPoint currentPose=init;
232  ScoredMove sm={currentPose,0,0};
233  unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
234  double currentScore=sm.score;
235  moveList.push_back(sm);
236  double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
237  unsigned int refinement=0;
238  enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
239  do{
240  if (bestScore>=currentScore){
241  refinement++;
242  adelta*=.5;
243  ldelta*=.5;
244  }
245  bestScore=currentScore;
246 // cout <<"score="<< currentScore << " refinement=" << refinement;
247 // cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
248  OrientedPoint bestLocalPose=currentPose;
249  OrientedPoint localPose=currentPose;
250 
251  Move move=Front;
252  do {
253  localPose=currentPose;
254  switch(move){
255  case Front:
256  localPose.x+=ldelta;
257  move=Back;
258  break;
259  case Back:
260  localPose.x-=ldelta;
261  move=Left;
262  break;
263  case Left:
264  localPose.y-=ldelta;
265  move=Right;
266  break;
267  case Right:
268  localPose.y+=ldelta;
269  move=TurnLeft;
270  break;
271  case TurnLeft:
272  localPose.theta+=adelta;
273  move=TurnRight;
274  break;
275  case TurnRight:
276  localPose.theta-=adelta;
277  move=Done;
278  break;
279  default:;
280  }
281  double localScore, localLikelihood;
282  //update the score
283  matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
284  if (localScore>currentScore){
285  currentScore=localScore;
286  bestLocalPose=localPose;
287  }
288  sm.score=localScore;
289  sm.likelihood=localLikelihood;
290  sm.pose=localPose;
291  moveList.push_back(sm);
292  //update the move list
293  } while(move!=Done);
294  currentPose=bestLocalPose;
295  //cout << __func__ << "currentScore=" << currentScore<< endl;
296  //here we look for the best move;
297  }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
298  //cout << __func__ << "bestScore=" << bestScore<< endl;
299 
300  //normalize the likelihood
301  double lmin=1e9;
302  double lmax=-1e9;
303  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
304  lmin=it->likelihood<lmin?it->likelihood:lmin;
305  lmax=it->likelihood>lmax?it->likelihood:lmax;
306  }
307  //cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
308  for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
309  it->likelihood=exp(it->likelihood-lmax);
310  //cout << "l=" << it->likelihood << endl;
311  }
312  //compute the mean
313  OrientedPoint mean(0,0,0);
314  double lacc=0;
315  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
316  mean=mean+it->pose*it->likelihood;
317  lacc+=it->likelihood;
318  }
319  mean=mean*(1./lacc);
320  //OrientedPoint delta=mean-currentPose;
321  //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
322  CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
323  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
324  OrientedPoint delta=it->pose-mean;
325  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
326  cov.xx+=delta.x*delta.x*it->likelihood;
327  cov.yy+=delta.y*delta.y*it->likelihood;
328  cov.tt+=delta.theta*delta.theta*it->likelihood;
329  cov.xy+=delta.x*delta.y*it->likelihood;
330  cov.xt+=delta.x*delta.theta*it->likelihood;
331  cov.yt+=delta.y*delta.theta*it->likelihood;
332  }
333  cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
334 
335  _mean=currentPose;
336  _cov=cov;
337  return bestScore;
338 }
339 
340  void ScanMatcher::setLaserParameters
341  (unsigned int beams, double* angles, const OrientedPoint& lpose){
342  if (m_laserAngles)
343  delete [] m_laserAngles;
344  m_laserPose=lpose;
345  m_laserBeams=beams;
346  m_laserAngles=new double[beams];
347  memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
348 }
349 
350 
351 double ScanMatcher::likelihood
352  (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
353  ScoredMoveList moveList;
354 
355 
356  for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
357  for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
358  for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
359 
360  OrientedPoint rp=p;
361  rp.x+=xx;
362  rp.y+=yy;
363  rp.theta+=tt;
364 
365  ScoredMove sm;
366  sm.pose=rp;
367 
368  likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
369  moveList.push_back(sm);
370  }
371 
372  //OrientedPoint delta=mean-currentPose;
373  //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
374  //normalize the likelihood
375  double lmax=-1e9;
376  double lcum=0;
377  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
378  lmax=it->likelihood>lmax?it->likelihood:lmax;
379  }
380  for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
381  //it->likelihood=exp(it->likelihood-lmax);
382  lcum+=exp(it->likelihood-lmax);
383  it->likelihood=exp(it->likelihood-lmax);
384  //cout << "l=" << it->likelihood << endl;
385  }
386 
387  OrientedPoint mean(0,0,0);
388  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
389  mean=mean+it->pose*it->likelihood;
390  }
391  mean=mean*(1./lcum);
392 
393  CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
394  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
395  OrientedPoint delta=it->pose-mean;
396  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
397  cov.xx+=delta.x*delta.x*it->likelihood;
398  cov.yy+=delta.y*delta.y*it->likelihood;
399  cov.tt+=delta.theta*delta.theta*it->likelihood;
400  cov.xy+=delta.x*delta.y*it->likelihood;
401  cov.xt+=delta.x*delta.theta*it->likelihood;
402  cov.yt+=delta.y*delta.theta*it->likelihood;
403  }
404  cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
405 
406  _mean=mean;
407  _cov=cov;
408  _lmax=lmax;
409  return log(lcum)+lmax;
410 }
411 
412 void ScanMatcher::setMatchingParameters
413  (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma, unsigned int likelihoodSkip){
414  m_usableRange=urange;
415  m_laserMaxRange=range;
416  m_kernelSize=kernsize;
417  m_optLinearDelta=lopt;
418  m_optAngularDelta=aopt;
419  m_optRecursiveIterations=iterations;
420  m_gaussianSigma=sigma;
421  m_likelihoodSigma=likelihoodSigma;
422  m_likelihoodSkip=likelihoodSkip;
423 }
424 
425 };
426 
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GMapping::HierarchicalArray2D< PointAccumulator >::PointSet
std::set< point< int >, pointcomparator< int > > PointSet
Definition: harray2d.h:13
GMapping::ScanMatcher::registerScan
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:215
GMapping::Point
point< double > Point
Definition: point.h:202
GMapping::IntPoint
point< int > IntPoint
Definition: point.h:201
GMapping::ScanMatcher::setLaserParameters
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
Definition: scanmatcher.cpp:559
GMapping::ScanMatcher::optimize
double optimize(OrientedPoint &pnew, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.cpp:337
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::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
GMapping::ScanMatcherMap
Map< PointAccumulator, HierarchicalArray2D< PointAccumulator > > ScanMatcherMap
Definition: smmap.h:50
GMapping::ScanMatcher::m_laserAngles
double m_laserAngles[LASER_MAXBEAMS]
Definition: scanmatcher.h:48
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::GridLineTraversal::gridLine
static void gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)
Definition: gridlinetraversal.h:110
GMapping::ScoredMoveList
std::list< ScoredMove > ScoredMoveList
Definition: scanmatcher.cpp:428
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:17
GMapping::ScanMatcher::computeActiveArea
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:116
GMapping::OrientedPoint
orientedpoint< double, double > OrientedPoint
Definition: point.h:203

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