gridslamprocessor.cpp
Go to the documentation of this file.
1 #include <string>
2 #include <deque>
3 #include <list>
4 #include <map>
5 #include <set>
6 #include <fstream>
7 #include <iomanip>
8 #include <gmapping/utils/stat.h>
10 
11 //#define MAP_CONSISTENCY_CHECK
12 //#define GENERATE_TRAJECTORIES
13 
14 namespace GMapping {
15 
16 const double m_distanceThresholdCheck = 20;
17 
18 using namespace std;
19 
20  GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){
21 
22  period_ = 5.0;
23  m_obsSigmaGain=1;
24  m_resampleThreshold=0.5;
25  m_minimumScore=0.;
26  }
27 
30 
31  period_ = 5.0;
32 
33  m_obsSigmaGain=gsp.m_obsSigmaGain;
34  m_resampleThreshold=gsp.m_resampleThreshold;
35  m_minimumScore=gsp.m_minimumScore;
36 
37  m_beams=gsp.m_beams;
38  m_indexes=gsp.m_indexes;
40  m_resampleThreshold=gsp.m_resampleThreshold;
41  m_matcher=gsp.m_matcher;
42 
43  m_count=gsp.m_count;
46  m_pose=gsp.m_pose;
47  m_odoPose=gsp.m_odoPose;
50  m_neff=gsp.m_neff;
51 
52  cerr << "FILTER COPY CONSTRUCTOR" << endl;
53  cerr << "m_odoPose=" << m_odoPose.x << " " <<m_odoPose.y << " " << m_odoPose.theta << endl;
54  cerr << "m_lastPartPose=" << m_lastPartPose.x << " " <<m_lastPartPose.y << " " << m_lastPartPose.theta << endl;
55  cerr << "m_linearDistance=" << m_linearDistance << endl;
56  cerr << "m_angularDistance=" << m_linearDistance << endl;
57 
58 
59  m_xmin=gsp.m_xmin;
60  m_ymin=gsp.m_ymin;
61  m_xmax=gsp.m_xmax;
62  m_ymax=gsp.m_ymax;
63  m_delta=gsp.m_delta;
64 
65  m_regScore=gsp.m_regScore;
66  m_critScore=gsp.m_critScore;
67  m_maxMove=gsp.m_maxMove;
68 
69  m_linearThresholdDistance=gsp.m_linearThresholdDistance;
70  m_angularThresholdDistance=gsp.m_angularThresholdDistance;
71  m_obsSigmaGain=gsp.m_obsSigmaGain;
72 
73 #ifdef MAP_CONSISTENCY_CHECK
74  cerr << __func__ << ": trajectories copy.... ";
75 #endif
77  for (unsigned int i=0; i<v.size(); i++){
78  m_particles[i].node=v[i];
79  }
80 #ifdef MAP_CONSISTENCY_CHECK
81  cerr << "end" << endl;
82 #endif
83 
84 
85  cerr << "Tree: normalizing, resetting and propagating weights within copy construction/cloneing ..." ;
86  updateTreeWeights(false);
87  cerr << ".done!" <<endl;
88  }
89 
90  GridSlamProcessor::GridSlamProcessor(std::ostream& infoS): m_infoStream(infoS){
91  period_ = 5.0;
92  m_obsSigmaGain=1;
93  m_resampleThreshold=0.5;
94  m_minimumScore=0.;
95 
96  }
97 
99 # ifdef MAP_CONSISTENCY_CHECK
100  cerr << __func__ << ": performing preclone_fit_test" << endl;
101  typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
102  PointerMap pmap;
103  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
104  const ScanMatcherMap& m1(it->map);
106  for (int x=0; x<h1.getXSize(); x++){
107  for (int y=0; y<h1.getYSize(); y++){
108  const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
109  if (a1.m_reference){
110  PointerMap::iterator f=pmap.find(a1.m_reference);
111  if (f==pmap.end())
112  pmap.insert(make_pair(a1.m_reference, 1));
113  else
114  f->second++;
115  }
116  }
117  }
118  }
119  cerr << __func__ << ": Number of allocated chunks" << pmap.size() << endl;
120  for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
121  assert(it->first->shares==(unsigned int)it->second);
122 
123  cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl;
124 # endif
125  GridSlamProcessor* cloned=new GridSlamProcessor(*this);
126 
127 # ifdef MAP_CONSISTENCY_CHECK
128  cerr << __func__ << ": trajectories end" << endl;
129  cerr << __func__ << ": performing afterclone_fit_test" << endl;
130  ParticleVector::const_iterator jt=cloned->m_particles.begin();
131  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
132  const ScanMatcherMap& m1(it->map);
133  const ScanMatcherMap& m2(jt->map);
136  jt++;
137  for (int x=0; x<h1.getXSize(); x++){
138  for (int y=0; y<h1.getYSize(); y++){
139  const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
140  const autoptr< Array2D<PointAccumulator> >& a2(h2.m_cells[x][y]);
141  assert(a1.m_reference==a2.m_reference);
142  assert((!a1.m_reference) || !(a1.m_reference->shares%2));
143  }
144  }
145  }
146  cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl;
147 # endif
148  return cloned;
149 }
150 
152  cerr << __func__ << ": Start" << endl;
153  cerr << __func__ << ": Deleting tree" << endl;
154  for (std::vector<Particle>::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
155 #ifdef TREE_CONSISTENCY_CHECK
156  TNode* node=it->node;
157  while(node)
158  node=node->parent;
159  cerr << "@" << endl;
160 #endif
161  if (it->node)
162  delete it->node;
163  //cout << "l=" << it->weight<< endl;
164  }
165 
166 # ifdef MAP_CONSISTENCY_CHECK
167  cerr << __func__ << ": performing predestruction_fit_test" << endl;
168  typedef std::map<autoptr< Array2D<PointAccumulator> >::reference* const, int> PointerMap;
169  PointerMap pmap;
170  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
171  const ScanMatcherMap& m1(it->map);
173  for (int x=0; x<h1.getXSize(); x++){
174  for (int y=0; y<h1.getYSize(); y++){
175  const autoptr< Array2D<PointAccumulator> >& a1(h1.m_cells[x][y]);
176  if (a1.m_reference){
177  PointerMap::iterator f=pmap.find(a1.m_reference);
178  if (f==pmap.end())
179  pmap.insert(make_pair(a1.m_reference, 1));
180  else
181  f->second++;
182  }
183  }
184  }
185  }
186  cerr << __func__ << ": Number of allocated chunks" << pmap.size() << endl;
187  for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++)
188  assert(it->first->shares>=(unsigned int)it->second);
189  cerr << __func__ << ": SUCCESS, the error is somewhere else" << endl;
190 # endif
191  }
192 
193 
194 
195  void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt,
196  int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){
197  m_obsSigmaGain=likelihoodGain;
198  m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip);
199  if (m_infoStream)
200  m_infoStream << " -maxUrange "<< urange
201  << " -maxUrange "<< range
202  << " -sigma "<< sigma
203  << " -kernelSize "<< kernsize
204  << " -lstep " << lopt
205  << " -lobsGain " << m_obsSigmaGain
206  << " -astep " << aopt << endl;
207 
208 
209  }
210 
212 (double srr, double srt, double str, double stt){
213  m_motionModel.srr=srr;
214  m_motionModel.srt=srt;
215  m_motionModel.str=str;
216  m_motionModel.stt=stt;
217 
218  if (m_infoStream)
219  m_infoStream << " -srr "<< srr << " -srt "<< srt
220  << " -str "<< str << " -stt "<< stt << endl;
221 
222 }
223 
224  void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){
225  m_linearThresholdDistance=linear;
226  m_angularThresholdDistance=angular;
227  m_resampleThreshold=resampleThreshold;
228  if (m_infoStream)
229  m_infoStream << " -linearUpdate " << linear
230  << " -angularUpdate "<< angular
231  << " -resampleThreshold " << m_resampleThreshold << endl;
232  }
233 
234  //HERE STARTS THE BEEF
235 
237  map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){
238  node=0;
239  }
240 
241 
243 
244  /*
245  Construct the angle table for the sensor
246 
247  FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot
248  */
249 
250  SensorMap::const_iterator laser_it=smap.find(std::string("FLASER"));
251  if (laser_it==smap.end()){
252  cerr << "Attempting to load the new carmen log format" << endl;
253  laser_it=smap.find(std::string("ROBOTLASER1"));
254  assert(laser_it!=smap.end());
255  }
256  const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
257  assert(rangeSensor && rangeSensor->beams().size());
258 
259  m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
260  double* angles=new double[rangeSensor->beams().size()];
261  for (unsigned int i=0; i<m_beams; i++){
262  angles[i]=rangeSensor->beams()[i].pose.theta;
263  }
264  m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
265  delete [] angles;
266  }
267 
268  void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){
269  m_xmin=xmin;
270  m_ymin=ymin;
271  m_xmax=xmax;
272  m_ymax=ymax;
273  m_delta=delta;
274  if (m_infoStream)
275  m_infoStream
276  << " -xmin "<< m_xmin
277  << " -xmax "<< m_xmax
278  << " -ymin "<< m_ymin
279  << " -ymax "<< m_ymax
280  << " -delta "<< m_delta
281  << " -particles "<< size << endl;
282 
283 
284  m_particles.clear();
285  TNode* node=new TNode(initialPose, 0, 0, 0);
286  ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta);
287  for (unsigned int i=0; i<size; i++){
288  m_particles.push_back(Particle(lmap));
289  m_particles.back().pose=initialPose;
290  m_particles.back().previousPose=initialPose;
291  m_particles.back().setWeight(0);
292  m_particles.back().previousIndex=0;
293 
294  // this is not needed
295  // m_particles.back().node=new TNode(initialPose, 0, node, 0);
296 
297  // we use the root directly
298  m_particles.back().node= node;
299  }
300  m_neff=(double)size;
301  m_count=0;
302  m_readingCount=0;
304  }
305 
307  const OdometrySensor* os=dynamic_cast<const OdometrySensor*>(o.getSensor());
308  if (os && os->isIdeal() && m_outputStream){
309  m_outputStream << setiosflags(ios::fixed) << setprecision(3);
310  m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ;
311  m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl;
312  }
313  }
314 
315 
316  bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){
317 
319  OrientedPoint relPose=reading.getPose();
320  if (!m_count){
321  m_lastPartPose=m_odoPose=relPose;
322  }
323 
324  //write the state of the reading and update all the particles using the motion model
325  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
326  OrientedPoint& pose(it->pose);
327  pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose);
328  }
329 
330  // update the output file
331  if (m_outputStream.is_open()){
332  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
333  m_outputStream << "ODOM ";
334  m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " ";
335  m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " ";
336  m_outputStream << reading.getTime();
337  m_outputStream << endl;
338  }
339  if (m_outputStream.is_open()){
340  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
341  m_outputStream << "ODO_UPDATE "<< m_particles.size() << " ";
342  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
343  OrientedPoint& pose(it->pose);
344  m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
345  m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
346  }
347  m_outputStream << reading.getTime();
348  m_outputStream << endl;
349  }
350 
351  //invoke the callback
353 
354 
355  // accumulate the robot translation and rotation
356  OrientedPoint move=relPose-m_odoPose;
357  move.theta=atan2(sin(move.theta), cos(move.theta));
358  m_linearDistance+=sqrt(move*move);
359  m_angularDistance+=fabs(move.theta);
360 
361  // if the robot jumps throw a warning
362  if (m_linearDistance>m_distanceThresholdCheck){
363  cerr << "***********************************************************************" << endl;
364  cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl;
365  cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl;
366  cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y
367  << " " <<m_odoPose.theta << endl;
368  cerr << "New Odometry Pose (reported from observation)= " << relPose.x << " " << relPose.y
369  << " " <<relPose.theta << endl;
370  cerr << "***********************************************************************" << endl;
371  cerr << "** The Odometry has a big jump here. This is probably a bug in the **" << endl;
372  cerr << "** odometry/laser input. We continue now, but the result is probably **" << endl;
373  cerr << "** crap or can lead to a core dump since the map doesn't fit.... C&G **" << endl;
374  cerr << "***********************************************************************" << endl;
375  }
376 
377  m_odoPose=relPose;
378 
379  bool processed=false;
380 
381  // process a scan only if the robot has traveled a given distance or a certain amount of time has elapsed
382  if (! m_count
383  || m_linearDistance>=m_linearThresholdDistance
384  || m_angularDistance>=m_angularThresholdDistance
385  || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){
386  last_update_time_ = reading.getTime();
387 
388  if (m_outputStream.is_open()){
389  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
390  m_outputStream << "FRAME " << m_readingCount;
392  m_outputStream << " " << m_angularDistance << endl;
393  }
394 
395  if (m_infoStream)
396  m_infoStream << "update frame " << m_readingCount << endl
397  << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl;
398 
399 
400  cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y
401  << " " << reading.getPose().theta << endl;
402 
403 
404  //this is for converting the reading in a scan-matcher feedable form
405  assert(reading.size()==m_beams);
406  double * plainReading = new double[m_beams];
407  for(unsigned int i=0; i<m_beams; i++){
408  plainReading[i]=reading[i];
409  }
410  m_infoStream << "m_count " << m_count << endl;
411 
412  RangeReading* reading_copy =
413  new RangeReading(reading.size(),
414  &(reading[0]),
415  static_cast<const RangeSensor*>(reading.getSensor()),
416  reading.getTime());
417 
418  if (m_count>0){
419  scanMatch(plainReading);
420  if (m_outputStream.is_open()){
421  m_outputStream << "LASER_READING "<< reading.size() << " ";
422  m_outputStream << setiosflags(ios::fixed) << setprecision(2);
423  for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){
424  m_outputStream << *b << " ";
425  }
426  OrientedPoint p=reading.getPose();
427  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
428  m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl;
429  m_outputStream << "SM_UPDATE "<< m_particles.size() << " ";
430  for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){
431  const OrientedPoint& pose=it->pose;
432  m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " ";
433  m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " ";
434  }
435  m_outputStream << endl;
436  }
438 
439  updateTreeWeights(false);
440 
441  if (m_infoStream){
442  m_infoStream << "neff= " << m_neff << endl;
443  }
444  if (m_outputStream.is_open()){
445  m_outputStream << setiosflags(ios::fixed) << setprecision(6);
446  m_outputStream << "NEFF " << m_neff << endl;
447  }
448  resample(plainReading, adaptParticles, reading_copy);
449 
450  } else {
451  m_infoStream << "Registering First Scan"<< endl;
452  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
454  m_matcher.computeActiveArea(it->map, it->pose, plainReading);
455  m_matcher.registerScan(it->map, it->pose, plainReading);
456 
457  // cyr: not needed anymore, particles refer to the root in the beginning!
458  TNode* node=new TNode(it->pose, 0., it->node, 0);
459  //node->reading=0;
460  node->reading = reading_copy;
461  it->node=node;
462 
463  }
464  }
465  // cerr << "Tree: normalizing, resetting and propagating weights at the end..." ;
466  updateTreeWeights(false);
467  // cerr << ".done!" <<endl;
468 
469  delete [] plainReading;
470  m_lastPartPose=m_odoPose; //update the past pose for the next iteration
473  m_count++;
474  processed=true;
475 
476  //keep ready for the next step
477  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
478  it->previousPose=it->pose;
479  }
480 
481  }
482  if (m_outputStream.is_open())
483  m_outputStream << flush;
484  m_readingCount++;
485  return processed;
486  }
487 
488 
490  return m_outputStream;
491  }
492 
494  return m_infoStream;
495  }
496 
497 
499  unsigned int bi=0;
501  for (unsigned int i=0; i<m_particles.size(); i++)
502  if (bw<m_particles[i].weightSum){
503  bw=m_particles[i].weightSum;
504  bi=i;
505  }
506  return (int) bi;
507  }
508 
512 
513 
514 };// end namespace
515 
516 
517 
518 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
point< double > Point
Definition: point.h:202
std::vector< GridSlamProcessor::TNode * > TNodeVector
std::vector< unsigned int > m_indexes
OrientedPoint drawFromMotion(const OrientedPoint &p, double linearMove, double angularMove) const
Definition: motionmodel.cpp:13
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)
const Sensor * getSensor() const
Definition: sensoreading.h:13
double getTime() const
Definition: sensoreading.h:12
bool processScan(const RangeReading &reading, int adaptParticles=0)
bool resample(const double *plainReading, int adaptParticles, const RangeReading *rr=0)
reference * m_reference
Definition: autoptr.h:24
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
void setUpdateDistances(double linear, double angular, double resampleThreshold)
void setSensorMap(const SensorMap &smap)
void processTruePos(const OdometryReading &odometry)
void scanMatch(const double *plainReading)
GridSlamProcessor * clone() const
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0)
const OrientedPoint & getPose() const
void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose=OrientedPoint(0, 0, 0))
double max(double a, double b)
Definition: gfs2img.cpp:22
void setMotionModelParameters(double srr, double srt, double str, double stt)
void computeActiveArea(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
const double m_distanceThresholdCheck
void updateTreeWeights(bool weightsAlreadyNormalized=false)
const OrientedPoint & getPose() const
Definition: rangereading.h:22
Storage & storage()
Definition: map.h:81


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