scanmatcher.cpp
Go to the documentation of this file.
1 #include <cstring>
2 #include <limits>
3 #include <list>
4 #include <iostream>
5 
6 #include <gmapping/scanmatcher/scanmatcher.h>
7 #include "gridlinetraversal.h"
8 //#define GENERATE_MAPS
9 
10 namespace GMapping {
11 
12 using namespace std;
13 
14 const double ScanMatcher::nullLikelihood=-.5;
15 
16 ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){
17  //m_laserAngles=0;
18  m_laserBeams=0;
19  m_optRecursiveIterations=3;
20  m_activeAreaComputed=false;
21 
22  // This are the dafault settings for a grid map of 5 cm
23  m_llsamplerange=0.01;
24  m_llsamplestep=0.01;
25  m_lasamplerange=0.005;
26  m_lasamplestep=0.005;
27  m_enlargeStep=10.;
28  m_fullnessThreshold=0.1;
29  m_angularOdometryReliability=0.;
30  m_linearOdometryReliability=0.;
31  m_freeCellRatio=sqrt(2.);
32  m_initialBeamsSkip=0;
33 
34 /*
35  // This are the dafault settings for a grid map of 10 cm
36  m_llsamplerange=0.1;
37  m_llsamplestep=0.1;
38  m_lasamplerange=0.02;
39  m_lasamplestep=0.01;
40 */
41  // This are the dafault settings for a grid map of 20/25 cm
42 /*
43  m_llsamplerange=0.2;
44  m_llsamplestep=0.1;
45  m_lasamplerange=0.02;
46  m_lasamplestep=0.01;
47  m_generateMap=false;
48 */
49 
50  m_linePoints = new IntPoint[20000];
51 }
52 
53 ScanMatcher::~ScanMatcher(){
54  delete [] m_linePoints;
55 }
56 
57 void ScanMatcher::invalidateActiveArea(){
58  m_activeAreaComputed=false;
59 }
60 
61 /*
62 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
63  if (m_activeAreaComputed)
64  return;
65  HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
66  OrientedPoint lp=p;
67  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
68  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
69  lp.theta+=m_laserPose.theta;
70  IntPoint p0=map.world2map(lp);
71  const double * angle=m_laserAngles;
72  for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
73  if (m_generateMap){
74  double d=*r;
75  if (d>m_laserMaxRange)
76  continue;
77  if (d>m_usableRange)
78  d=m_usableRange;
79 
80  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
81  IntPoint p1=map.world2map(phit);
82 
83  d+=map.getDelta();
84  //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
85  //IntPoint p2=map.world2map(phit2);
86  IntPoint linePoints[20000] ;
87  GridLineTraversalLine line;
88  line.points=linePoints;
89  //GridLineTraversal::gridLine(p0, p2, &line);
90  GridLineTraversal::gridLine(p0, p1, &line);
91  for (int i=0; i<line.num_points-1; i++){
92  activeArea.insert(map.storage().patchIndexes(linePoints[i]));
93  }
94  if (d<=m_usableRange){
95  activeArea.insert(map.storage().patchIndexes(p1));
96  //activeArea.insert(map.storage().patchIndexes(p2));
97  }
98  } else {
99  if (*r>m_laserMaxRange||*r>m_usableRange) continue;
100  Point phit=lp;
101  phit.x+=*r*cos(lp.theta+*angle);
102  phit.y+=*r*sin(lp.theta+*angle);
103  IntPoint p1=map.world2map(phit);
104  assert(p1.x>=0 && p1.y>=0);
105  IntPoint cp=map.storage().patchIndexes(p1);
106  assert(cp.x>=0 && cp.y>=0);
107  activeArea.insert(cp);
108 
109  }
110  //this allocates the unallocated cells in the active area of the map
111  //cout << "activeArea::size() " << activeArea.size() << endl;
112  map.storage().setActiveArea(activeArea, true);
113  m_activeAreaComputed=true;
114 }
115 */
116 void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
117  if (m_activeAreaComputed)
118  return;
119  OrientedPoint lp=p;
120  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
121  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
122  lp.theta+=m_laserPose.theta;
123  IntPoint p0=map.world2map(lp);
124 
125  Point min(map.map2world(0,0));
126  Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
127 
128  if (lp.x<min.x) min.x=lp.x;
129  if (lp.y<min.y) min.y=lp.y;
130  if (lp.x>max.x) max.x=lp.x;
131  if (lp.y>max.y) max.y=lp.y;
132 
133  /*determine the size of the area*/
134  const double * angle=m_laserAngles+m_initialBeamsSkip;
135  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++){
136  if (*r>m_laserMaxRange||*r==0.0||isnan(*r)) continue;
137  double d=*r>m_usableRange?m_usableRange:*r;
138  Point phit=lp;
139  phit.x+=d*cos(lp.theta+*angle);
140  phit.y+=d*sin(lp.theta+*angle);
141  if (phit.x<min.x) min.x=phit.x;
142  if (phit.y<min.y) min.y=phit.y;
143  if (phit.x>max.x) max.x=phit.x;
144  if (phit.y>max.y) max.y=phit.y;
145  }
146  //min=min-Point(map.getDelta(),map.getDelta());
147  //max=max+Point(map.getDelta(),map.getDelta());
148 
149  if ( !map.isInside(min) || !map.isInside(max)){
150  Point lmin(map.map2world(0,0));
151  Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1));
152  //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl;
153  //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
154  min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep;
155  max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep;
156  min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep;
157  max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep;
158  map.resize(min.x, min.y, max.x, max.y);
159  //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl;
160  }
161 
162  HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
163  /*allocate the active area*/
164  angle=m_laserAngles+m_initialBeamsSkip;
165  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
166  if (m_generateMap){
167  double d=*r;
168  if (d>m_laserMaxRange||d==0.0||isnan(d))
169  continue;
170  if (d>m_usableRange)
171  d=m_usableRange;
172  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
173  IntPoint p0=map.world2map(lp);
174  IntPoint p1=map.world2map(phit);
175 
176  //IntPoint linePoints[20000] ;
177  GridLineTraversalLine line;
178  line.points=m_linePoints;
179  GridLineTraversal::gridLine(p0, p1, &line);
180  for (int i=0; i<line.num_points-1; i++){
181  assert(map.isInside(m_linePoints[i]));
182  activeArea.insert(map.storage().patchIndexes(m_linePoints[i]));
183  assert(m_linePoints[i].x>=0 && m_linePoints[i].y>=0);
184  }
185  if (d<m_usableRange){
186  IntPoint cp=map.storage().patchIndexes(p1);
187  assert(cp.x>=0 && cp.y>=0);
188  activeArea.insert(cp);
189  }
190  } else {
191  if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
192  Point phit=lp;
193  phit.x+=*r*cos(lp.theta+*angle);
194  phit.y+=*r*sin(lp.theta+*angle);
195  IntPoint p1=map.world2map(phit);
196  assert(p1.x>=0 && p1.y>=0);
197  IntPoint cp=map.storage().patchIndexes(p1);
198  assert(cp.x>=0 && cp.y>=0);
199  activeArea.insert(cp);
200  }
201 
202  //this allocates the unallocated cells in the active area of the map
203  //cout << "activeArea::size() " << activeArea.size() << endl;
204 /*
205  cerr << "ActiveArea=";
206  for (HierarchicalArray2D<PointAccumulator>::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){
207  cerr << "(" << it->x <<"," << it->y << ") ";
208  }
209  cerr << endl;
210 */
211  map.storage().setActiveArea(activeArea, true);
212  m_activeAreaComputed=true;
213 }
214 
215 double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
216  if (!m_activeAreaComputed)
217  computeActiveArea(map, p, readings);
218 
219  //this operation replicates the cells that will be changed in the registration operation
220  map.storage().allocActiveArea();
221 
222  OrientedPoint lp=p;
223  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
224  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
225  lp.theta+=m_laserPose.theta;
226  IntPoint p0=map.world2map(lp);
227 
228 
229  const double * angle=m_laserAngles+m_initialBeamsSkip;
230  double esum=0;
231  for (const double* r=readings+m_initialBeamsSkip; r<readings+m_laserBeams; r++, angle++)
232  if (m_generateMap){
233  double d=*r;
234  if (d>m_laserMaxRange||d==0.0||isnan(d))
235  continue;
236  if (d>m_usableRange)
237  d=m_usableRange;
238  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
239  IntPoint p1=map.world2map(phit);
240  //IntPoint linePoints[20000] ;
241  GridLineTraversalLine line;
242  line.points=m_linePoints;
243  GridLineTraversal::gridLine(p0, p1, &line);
244  for (int i=0; i<line.num_points-1; i++){
245  PointAccumulator& cell=map.cell(line.points[i]);
246  double e=-cell.entropy();
247  cell.update(false, Point(0,0));
248  e+=cell.entropy();
249  esum+=e;
250  }
251  if (d<m_usableRange){
252  double e=-map.cell(p1).entropy();
253  map.cell(p1).update(true, phit);
254  e+=map.cell(p1).entropy();
255  esum+=e;
256  }
257  } else {
258  if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue;
259  Point phit=lp;
260  phit.x+=*r*cos(lp.theta+*angle);
261  phit.y+=*r*sin(lp.theta+*angle);
262  IntPoint p1=map.world2map(phit);
263  assert(p1.x>=0 && p1.y>=0);
264  map.cell(p1).update(true,phit);
265  }
266  //cout << "informationGain=" << -esum << endl;
267  return esum;
268 }
269 
270 /*
271 void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
272  if (!m_activeAreaComputed)
273  computeActiveArea(map, p, readings);
274 
275  //this operation replicates the cells that will be changed in the registration operation
276  map.storage().allocActiveArea();
277 
278  OrientedPoint lp=p;
279  lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y;
280  lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y;
281  lp.theta+=m_laserPose.theta;
282  IntPoint p0=map.world2map(lp);
283  const double * angle=m_laserAngles;
284  for (const double* r=readings; r<readings+m_laserBeams; r++, angle++)
285  if (m_generateMap){
286  double d=*r;
287  if (d>m_laserMaxRange)
288  continue;
289  if (d>m_usableRange)
290  d=m_usableRange;
291  Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle));
292  IntPoint p1=map.world2map(phit);
293 
294  IntPoint linePoints[20000] ;
295  GridLineTraversalLine line;
296  line.points=linePoints;
297  GridLineTraversal::gridLine(p0, p1, &line);
298  for (int i=0; i<line.num_points-1; i++){
299  IntPoint ci=map.storage().patchIndexes(line.points[i]);
300  if (map.storage().getActiveArea().find(ci)==map.storage().getActiveArea().end())
301  cerr << "BIG ERROR" <<endl;
302  map.cell(line.points[i]).update(false, Point(0,0));
303  }
304  if (d<=m_usableRange){
305 
306  map.cell(p1).update(true,phit);
307  }
308  } else {
309  if (*r>m_laserMaxRange||*r>m_usableRange) continue;
310  Point phit=lp;
311  phit.x+=*r*cos(lp.theta+*angle);
312  phit.y+=*r*sin(lp.theta+*angle);
313  map.cell(phit).update(true,phit);
314  }
315 }
316 
317 */
318 
319 double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
320  double currentScore;
321  double sc=score(map, init, readings);;
322  OrientedPoint start=init;
323  pnew=init;
324  int iterations=0;
325  do{
326  currentScore=sc;
327  sc=icpStep(pnew, map, start, readings);
328  //cerr << "pstart=" << start.x << " " <<start.y << " " << start.theta << endl;
329  //cerr << "pret=" << pnew.x << " " <<pnew.y << " " << pnew.theta << endl;
330  start=pnew;
331  iterations++;
332  } while (sc>currentScore);
333  cerr << "i="<< iterations << endl;
334  return currentScore;
335 }
336 
337 double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
338  double bestScore=-1;
339  OrientedPoint currentPose=init;
340  double currentScore=score(map, currentPose, readings);
341  double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
342  unsigned int refinement=0;
343  enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
344 /* cout << __PRETTY_FUNCTION__<< " readings: ";
345  for (int i=0; i<m_laserBeams; i++){
346  cout << readings[i] << " ";
347  }
348  cout << endl;
349 */ int c_iterations=0;
350  do{
351  if (bestScore>=currentScore){
352  refinement++;
353  adelta*=.5;
354  ldelta*=.5;
355  }
356  bestScore=currentScore;
357 // cout <<"score="<< currentScore << " refinement=" << refinement;
358 // cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
359  OrientedPoint bestLocalPose=currentPose;
360  OrientedPoint localPose=currentPose;
361 
362  Move move=Front;
363  do {
364  localPose=currentPose;
365  switch(move){
366  case Front:
367  localPose.x+=ldelta;
368  move=Back;
369  break;
370  case Back:
371  localPose.x-=ldelta;
372  move=Left;
373  break;
374  case Left:
375  localPose.y-=ldelta;
376  move=Right;
377  break;
378  case Right:
379  localPose.y+=ldelta;
380  move=TurnLeft;
381  break;
382  case TurnLeft:
383  localPose.theta+=adelta;
384  move=TurnRight;
385  break;
386  case TurnRight:
387  localPose.theta-=adelta;
388  move=Done;
389  break;
390  default:;
391  }
392 
393  double odo_gain=1;
394  if (m_angularOdometryReliability>0.){
395  double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
396  odo_gain*=exp(-m_angularOdometryReliability*dth);
397  }
398  if (m_linearOdometryReliability>0.){
399  double dx=init.x-localPose.x;
400  double dy=init.y-localPose.y;
401  double drho=dx*dx+dy*dy;
402  odo_gain*=exp(-m_linearOdometryReliability*drho);
403  }
404  double localScore=odo_gain*score(map, localPose, readings);
405 
406  if (localScore>currentScore){
407  currentScore=localScore;
408  bestLocalPose=localPose;
409  }
410  c_iterations++;
411  } while(move!=Done);
412  currentPose=bestLocalPose;
413 // cout << "currentScore=" << currentScore<< endl;
414  //here we look for the best move;
415  }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
416  //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
417  //cout << __PRETTY_FUNCTION__ << "iterations=" << c_iterations<< endl;
418  pnew=currentPose;
419  return bestScore;
420 }
421 
422 struct ScoredMove{
423  OrientedPoint pose;
424  double score;
425  double likelihood;
426 };
427 
428 typedef std::list<ScoredMove> ScoredMoveList;
429 
430 double ScanMatcher::optimize(OrientedPoint& _mean, ScanMatcher::CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{
431  ScoredMoveList moveList;
432  double bestScore=-1;
433  OrientedPoint currentPose=init;
434  ScoredMove sm={currentPose,0,0};
435  unsigned int matched=likelihoodAndScore(sm.score, sm.likelihood, map, currentPose, readings);
436  double currentScore=sm.score;
437  moveList.push_back(sm);
438  double adelta=m_optAngularDelta, ldelta=m_optLinearDelta;
439  unsigned int refinement=0;
440  int count=0;
441  enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done};
442  do{
443  if (bestScore>=currentScore){
444  refinement++;
445  adelta*=.5;
446  ldelta*=.5;
447  }
448  bestScore=currentScore;
449 // cout <<"score="<< currentScore << " refinement=" << refinement;
450 // cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl;
451  OrientedPoint bestLocalPose=currentPose;
452  OrientedPoint localPose=currentPose;
453 
454  Move move=Front;
455  do {
456  localPose=currentPose;
457  switch(move){
458  case Front:
459  localPose.x+=ldelta;
460  move=Back;
461  break;
462  case Back:
463  localPose.x-=ldelta;
464  move=Left;
465  break;
466  case Left:
467  localPose.y-=ldelta;
468  move=Right;
469  break;
470  case Right:
471  localPose.y+=ldelta;
472  move=TurnLeft;
473  break;
474  case TurnLeft:
475  localPose.theta+=adelta;
476  move=TurnRight;
477  break;
478  case TurnRight:
479  localPose.theta-=adelta;
480  move=Done;
481  break;
482  default:;
483  }
484  double localScore, localLikelihood;
485 
486  double odo_gain=1;
487  if (m_angularOdometryReliability>0.){
488  double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth;
489  odo_gain*=exp(-m_angularOdometryReliability*dth);
490  }
491  if (m_linearOdometryReliability>0.){
492  double dx=init.x-localPose.x;
493  double dy=init.y-localPose.y;
494  double drho=dx*dx+dy*dy;
495  odo_gain*=exp(-m_linearOdometryReliability*drho);
496  }
497  localScore=odo_gain*score(map, localPose, readings);
498  //update the score
499  count++;
500  matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings);
501  if (localScore>currentScore){
502  currentScore=localScore;
503  bestLocalPose=localPose;
504  }
505  sm.score=localScore;
506  sm.likelihood=localLikelihood;//+log(odo_gain);
507  sm.pose=localPose;
508  moveList.push_back(sm);
509  //update the move list
510  } while(move!=Done);
511  currentPose=bestLocalPose;
512  //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl;
513  //here we look for the best move;
514  }while (currentScore>bestScore || refinement<m_optRecursiveIterations);
515  //cout << __PRETTY_FUNCTION__ << "bestScore=" << bestScore<< endl;
516  //cout << __PRETTY_FUNCTION__ << "iterations=" << count<< endl;
517 
518  //normalize the likelihood
519  double lmin=1e9;
520  double lmax=-1e9;
521  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
522  lmin=it->likelihood<lmin?it->likelihood:lmin;
523  lmax=it->likelihood>lmax?it->likelihood:lmax;
524  }
525  //cout << "lmin=" << lmin << " lmax=" << lmax<< endl;
526  for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
527  it->likelihood=exp(it->likelihood-lmax);
528  //cout << "l=" << it->likelihood << endl;
529  }
530  //compute the mean
531  OrientedPoint mean(0,0,0);
532  double lacc=0;
533  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
534  mean=mean+it->pose*it->likelihood;
535  lacc+=it->likelihood;
536  }
537  mean=mean*(1./lacc);
538  //OrientedPoint delta=mean-currentPose;
539  //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
540  CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
541  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
542  OrientedPoint delta=it->pose-mean;
543  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
544  cov.xx+=delta.x*delta.x*it->likelihood;
545  cov.yy+=delta.y*delta.y*it->likelihood;
546  cov.tt+=delta.theta*delta.theta*it->likelihood;
547  cov.xy+=delta.x*delta.y*it->likelihood;
548  cov.xt+=delta.x*delta.theta*it->likelihood;
549  cov.yt+=delta.y*delta.theta*it->likelihood;
550  }
551  cov.xx/=lacc, cov.xy/=lacc, cov.xt/=lacc, cov.yy/=lacc, cov.yt/=lacc, cov.tt/=lacc;
552 
553  _mean=currentPose;
554  _cov=cov;
555  return bestScore;
556 }
557 
558 void ScanMatcher::setLaserParameters
559  (unsigned int beams, double* angles, const OrientedPoint& lpose){
560  /*if (m_laserAngles)
561  delete [] m_laserAngles;
562  */
563  assert(beams<LASER_MAXBEAMS);
564  m_laserPose=lpose;
565  m_laserBeams=beams;
566  //m_laserAngles=new double[beams];
567  memcpy(m_laserAngles, angles, sizeof(double)*m_laserBeams);
568 }
569 
570 
571 double ScanMatcher::likelihood
572  (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings){
573  ScoredMoveList moveList;
574 
575  for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
576  for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
577  for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
578 
579  OrientedPoint rp=p;
580  rp.x+=xx;
581  rp.y+=yy;
582  rp.theta+=tt;
583 
584  ScoredMove sm;
585  sm.pose=rp;
586 
587  likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
588  moveList.push_back(sm);
589  }
590 
591  //OrientedPoint delta=mean-currentPose;
592  //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
593  //normalize the likelihood
594  double lmax=-1e9;
595  double lcum=0;
596  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
597  lmax=it->likelihood>lmax?it->likelihood:lmax;
598  }
599  for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
600  //it->likelihood=exp(it->likelihood-lmax);
601  lcum+=exp(it->likelihood-lmax);
602  it->likelihood=exp(it->likelihood-lmax);
603  //cout << "l=" << it->likelihood << endl;
604  }
605 
606  OrientedPoint mean(0,0,0);
607  double s=0,c=0;
608  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
609  mean=mean+it->pose*it->likelihood;
610  s+=it->likelihood*sin(it->pose.theta);
611  c+=it->likelihood*cos(it->pose.theta);
612  }
613  mean=mean*(1./lcum);
614  s/=lcum;
615  c/=lcum;
616  mean.theta=atan2(s,c);
617 
618 
619  CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
620  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
621  OrientedPoint delta=it->pose-mean;
622  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
623  cov.xx+=delta.x*delta.x*it->likelihood;
624  cov.yy+=delta.y*delta.y*it->likelihood;
625  cov.tt+=delta.theta*delta.theta*it->likelihood;
626  cov.xy+=delta.x*delta.y*it->likelihood;
627  cov.xt+=delta.x*delta.theta*it->likelihood;
628  cov.yt+=delta.y*delta.theta*it->likelihood;
629  }
630  cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
631 
632  _mean=mean;
633  _cov=cov;
634  _lmax=lmax;
635  return log(lcum)+lmax;
636 }
637 
638 double ScanMatcher::likelihood
639  (double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p,
640  Gaussian3& odometry, const double* readings, double gain){
641  ScoredMoveList moveList;
642 
643 
644  for (double xx=-m_llsamplerange; xx<=m_llsamplerange; xx+=m_llsamplestep)
645  for (double yy=-m_llsamplerange; yy<=m_llsamplerange; yy+=m_llsamplestep)
646  for (double tt=-m_lasamplerange; tt<=m_lasamplerange; tt+=m_lasamplestep){
647 
648  OrientedPoint rp=p;
649  rp.x+=xx;
650  rp.y+=yy;
651  rp.theta+=tt;
652 
653  ScoredMove sm;
654  sm.pose=rp;
655 
656  likelihoodAndScore(sm.score, sm.likelihood, map, rp, readings);
657  sm.likelihood+=odometry.eval(rp)/gain;
658  assert(!isnan(sm.likelihood));
659  moveList.push_back(sm);
660  }
661 
662  //OrientedPoint delta=mean-currentPose;
663  //cout << "delta.x=" << delta.x << " delta.y=" << delta.y << " delta.theta=" << delta.theta << endl;
664  //normalize the likelihood
665  double lmax=-std::numeric_limits<double>::max();
666  double lcum=0;
667  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
668  lmax=it->likelihood>lmax?it->likelihood:lmax;
669  }
670  for (ScoredMoveList::iterator it=moveList.begin(); it!=moveList.end(); it++){
671  //it->likelihood=exp(it->likelihood-lmax);
672  lcum+=exp(it->likelihood-lmax);
673  it->likelihood=exp(it->likelihood-lmax);
674  //cout << "l=" << it->likelihood << endl;
675  }
676 
677  OrientedPoint mean(0,0,0);
678  double s=0,c=0;
679  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
680  mean=mean+it->pose*it->likelihood;
681  s+=it->likelihood*sin(it->pose.theta);
682  c+=it->likelihood*cos(it->pose.theta);
683  }
684  mean=mean*(1./lcum);
685  s/=lcum;
686  c/=lcum;
687  mean.theta=atan2(s,c);
688 
689 
690  CovarianceMatrix cov={0.,0.,0.,0.,0.,0.};
691  for (ScoredMoveList::const_iterator it=moveList.begin(); it!=moveList.end(); it++){
692  OrientedPoint delta=it->pose-mean;
693  delta.theta=atan2(sin(delta.theta), cos(delta.theta));
694  cov.xx+=delta.x*delta.x*it->likelihood;
695  cov.yy+=delta.y*delta.y*it->likelihood;
696  cov.tt+=delta.theta*delta.theta*it->likelihood;
697  cov.xy+=delta.x*delta.y*it->likelihood;
698  cov.xt+=delta.x*delta.theta*it->likelihood;
699  cov.yt+=delta.y*delta.theta*it->likelihood;
700  }
701  cov.xx/=lcum, cov.xy/=lcum, cov.xt/=lcum, cov.yy/=lcum, cov.yt/=lcum, cov.tt/=lcum;
702 
703  _mean=mean;
704  _cov=cov;
705  _lmax=lmax;
706  double v=log(lcum)+lmax;
707  assert(!isnan(v));
708  return v;
709 }
710 
711 void ScanMatcher::setMatchingParameters
712  (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma, unsigned int likelihoodSkip){
713  m_usableRange=urange;
714  m_laserMaxRange=range;
715  m_kernelSize=kernsize;
716  m_optLinearDelta=lopt;
717  m_optAngularDelta=aopt;
718  m_optRecursiveIterations=iterations;
719  m_gaussianSigma=sigma;
720  m_likelihoodSigma=likelihoodSigma;
721  m_likelihoodSkip=likelihoodSkip;
722 }
723 
724 };
725 
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GMapping::PointAccumulator::update
void update(bool value, const Point &p=Point(0, 0))
Definition: smmap.h:30
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::min
point< T > min(const point< T > &p1, const point< T > &p2)
Definition: point.h:154
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
GMapping::Point
point< double > Point
Definition: point.h:202
GMapping::ScanMatcher::likelihoodAndScore
unsigned int likelihoodAndScore(double &s, double &l, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
Definition: scanmatcher.h:191
GMapping::ScoredMove::pose
OrientedPoint pose
Definition: scanmatcher.cpp:423
LASER_MAXBEAMS
#define LASER_MAXBEAMS
Definition: scanmatcher.h:10
GMapping::Map::isInside
bool isInside(int x, int y) const
Definition: map.h:65
GMapping::max
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
GMapping::ScanMatcher::setLaserParameters
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
Definition: scanmatcher.cpp:559
GMapping::HierarchicalArray2D::setActiveArea
void setActiveArea(const PointSet &, bool patchCoords=false)
Definition: harray2d.h:119
GMapping::Map::world2map
IntPoint world2map(const Point &p) const
Definition: map.h:179
GMapping::Map::getMapSizeX
int getMapSizeX() const
Definition: map.h:36
GMapping::HierarchicalArray2D::patchIndexes
IntPoint patchIndexes(int x, int y) const
Definition: harray2d.h:170
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::Gaussian3::eval
double eval(const OrientedPoint &p) const
Definition: stat.cpp:182
GMapping::PointAccumulator::entropy
double entropy() const
Definition: smmap.h:40
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::Map::getMapSizeY
int getMapSizeY() const
Definition: map.h:37
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::nullLikelihood
static const double nullLikelihood
Definition: scanmatcher.h:40
GMapping::Map::resize
void resize(double xmin, double ymin, double xmax, double ymax)
Definition: map.h:138
GMapping::ScanMatcher::likelihood
double likelihood(double &lmax, OrientedPoint &mean, CovarianceMatrix &cov, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:572
max
double max(double a, double b)
Definition: gfs2img.cpp:22
GMapping::ScanMatcher::computeActiveArea
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
Definition: scanmatcher.cpp:116
GMapping::Map::map2world
Point map2world(const IntPoint &p) const
Definition: map.h:184
GMapping::Map::storage
Storage & storage()
Definition: map.h:81

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