gsp_thread.cpp
Go to the documentation of this file.
1 /*****************************************************************
2  *
3  * This file is part of the GMAPPING project
4  *
5  * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
6  * Cyrill Stachniss, and Wolfram Burgard
7  *
8  * This software is licensed under the 3-Clause BSD License
9  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
10  * and Wolfram Burgard.
11  *
12  * Further information on this license can be found at:
13  * https://opensource.org/licenses/BSD-3-Clause
14  *
15  * GMAPPING is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied
17  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
18  * PURPOSE.
19  *
20  *****************************************************************/
21 
22 
25 #include <gmapping/utils/stat.h>
27 
28 #ifdef CARMEN_SUPPORT
30 #endif
31 
32 #define DEBUG cout << __func__
33 
34 using namespace std;
35 
36 int GridSlamProcessorThread::init(int argc, const char * const * argv){
37  m_argc=argc;
38  m_argv=argv;
39  std::string configfilename;
40  std::string ebuf="not_set";
41 
42  CMD_PARSE_BEGIN_SILENT(1,argc);
43  parseStringSilent("-cfg",configfilename);
45 
46  if (configfilename.length()>0){
47  ConfigFile cfg(configfilename);
48 
49  filename = (std::string) cfg.value("gfs","filename",filename);
50  outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
51  xmin = cfg.value("gfs","xmin", xmin);
52  xmax = cfg.value("gfs","xmax",xmax);
53  ymin = cfg.value("gfs","ymin",ymin);
54  ymax = cfg.value("gfs","ymax",ymax);
55  delta = cfg.value("gfs","delta",delta);
56  maxrange = cfg.value("gfs","maxrange",maxrange);
57  maxUrange = cfg.value("gfs","maxUrange",maxUrange);
58  regscore = cfg.value("gfs","regscore",regscore);
59  critscore = cfg.value("gfs","critscore",critscore);
60  kernelSize = cfg.value("gfs","kernelSize",kernelSize);
61  sigma = cfg.value("gfs","sigma",sigma);
62  iterations = cfg.value("gfs","iterations",iterations);
63  lstep = cfg.value("gfs","lstep",lstep);
64  astep = cfg.value("gfs","astep",astep);
65  maxMove = cfg.value("gfs","maxMove",maxMove);
66  srr = cfg.value("gfs","srr", srr);
67  srt = cfg.value("gfs","srt", srt);
68  str = cfg.value("gfs","str", str);
69  stt = cfg.value("gfs","stt", stt);
70  particles = cfg.value("gfs","particles",particles);
71  angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate);
72  linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate);
73  lsigma = cfg.value("gfs","lsigma", lsigma);
74  ogain = cfg.value("gfs","lobsGain", ogain);
75  lskip = (int)cfg.value("gfs","lskip", lskip);
76  mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime);
77  randseed = cfg.value("gfs","randseed", randseed);
78  autosize = cfg.value("gfs","autosize", autosize);
79  readFromStdin = cfg.value("gfs","stdin", readFromStdin);
80  resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold);
81  skipMatching = cfg.value("gfs","skipMatching", skipMatching);
82  onLine = cfg.value("gfs","onLine", onLine);
83  generateMap = cfg.value("gfs","generateMap", generateMap);
84  m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore);
85  llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange);
86  lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange );
87  llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep);
88  lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep);
89  linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability);
90  angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability);
91  ebuf = (std::string) cfg.value("gfs","estrategy", ebuf);
92  considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance);
93 
94  }
95 
96 
97  CMD_PARSE_BEGIN(1,argc);
98  parseString("-cfg",configfilename); /* to avoid the warning*/
99  parseString("-filename",filename);
100  parseString("-outfilename",outfilename);
101  parseDouble("-xmin",xmin);
102  parseDouble("-xmax",xmax);
103  parseDouble("-ymin",ymin);
104  parseDouble("-ymax",ymax);
105  parseDouble("-delta",delta);
106  parseDouble("-maxrange",maxrange);
107  parseDouble("-maxUrange",maxUrange);
108  parseDouble("-regscore",regscore);
109  parseDouble("-critscore",critscore);
110  parseInt("-kernelSize",kernelSize);
111  parseDouble("-sigma",sigma);
112  parseInt("-iterations",iterations);
113  parseDouble("-lstep",lstep);
114  parseDouble("-astep",astep);
115  parseDouble("-maxMove",maxMove);
116  parseDouble("-srr", srr);
117  parseDouble("-srt", srt);
118  parseDouble("-str", str);
119  parseDouble("-stt", stt);
120  parseInt("-particles",particles);
121  parseDouble("-angularUpdate", angularUpdate);
122  parseDouble("-linearUpdate", linearUpdate);
123  parseDouble("-lsigma", lsigma);
124  parseDouble("-lobsGain", ogain);
125  parseInt("-lskip", lskip);
126  parseInt("-mapUpdate", mapUpdateTime);
127  parseInt("-randseed", randseed);
128  parseFlag("-autosize", autosize);
129  parseFlag("-stdin", readFromStdin);
130  parseDouble("-resampleThreshold", resampleThreshold);
131  parseFlag("-skipMatching", skipMatching);
132  parseFlag("-onLine", onLine);
133  parseFlag("-generateMap", generateMap);
134  parseDouble("-minimumScore", m_minimumScore);
135  parseDouble("-llsamplerange", llsamplerange);
136  parseDouble("-lasamplerange", lasamplerange);
137  parseDouble("-llsamplestep", llsamplestep);
138  parseDouble("-lasamplestep", lasamplestep);
139  parseDouble("-linearOdometryReliability",linearOdometryReliability);
140  parseDouble("-angularOdometryReliability",angularOdometryReliability);
141  parseString("-estrategy", ebuf);
142 
143  parseFlag("-considerOdometryCovariance",considerOdometryCovariance);
145 
146  if (filename.length() <=0){
147  cout << "no filename specified" << endl;
148  return -1;
149  }
150  return 0;
151 }
152 
154  if (onLine){
155  cout << " onLineProcessing" << endl;
156  return 0;
157  }
158  ifstream is;
159  if (fn)
160  is.open(fn);
161  else
162  is.open(filename.c_str());
163  if (! is){
164  cout << "no file found" << endl;
165  return -1;
166  }
167 
168  CarmenConfiguration conf;
169  conf.load(is);
170  is.close();
171 
172  sensorMap=conf.computeSensorMap();
173 
174  if (input)
175  delete input;
176 
177  if (! readFromStdin){
178  plainStream.open(filename.c_str());
179  input=new InputSensorStream(sensorMap, plainStream);
180  cout << "Plain Stream opened="<< (bool) plainStream << endl;
181  } else {
182  input=new InputSensorStream(sensorMap, cin);
183  cout << "Plain Stream opened on stdin" << endl;
184  }
185  return 0;
186 }
187 
189  //This are the processor parameters
190  filename="";
191  outfilename="";
192  xmin=-100.;
193  ymin=-100.;
194  xmax=100.;
195  ymax=100.;
196  delta=0.05;
197 
198  //scan matching parameters
199  sigma=0.05;
200  maxrange=80.;
201  maxUrange=80.;
202  regscore=1e4;
203  lstep=.05;
204  astep=.05;
205  kernelSize=1;
206  iterations=5;
207  critscore=0.;
208  maxMove=1.;
209  lsigma=.075;
210  ogain=3;
211  lskip=0;
212  autosize=false;
213  skipMatching=false;
214 
215  //motion model parameters
216  srr=0.1, srt=0.1, str=0.1, stt=0.1;
217  //particle parameters
218  particles=30;
219  randseed=0;
220 
221  //gfs parameters
222  angularUpdate=0.5;
223  linearUpdate=1;
224  resampleThreshold=0.5;
225 
226  input=0;
227 
228  pthread_mutex_init(&hp_mutex,0);
229  pthread_mutex_init(&ind_mutex,0);
230  pthread_mutex_init(&hist_mutex,0);
231  running=false;
233  mapUpdateTime=5;
234  mapTimer=0;
235  readFromStdin=false;
236  onLine=false;
237  generateMap=false;
238 
239  // This are the dafault settings for a grid map of 5 cm
240  llsamplerange=0.01;
241  llsamplestep=0.01;
242  lasamplerange=0.005;
243  lasamplestep=0.005;
246 
248 /*
249  // This are the dafault settings for a grid map of 10 cm
250  m_llsamplerange=0.1;
251  m_llsamplestep=0.1;
252  m_lasamplerange=0.02;
253  m_lasamplestep=0.01;
254 */
255  // This are the dafault settings for a grid map of 20/25 cm
256 /*
257  m_llsamplerange=0.2;
258  m_llsamplestep=0.1;
259  m_lasamplerange=0.02;
260  m_lasamplestep=0.01;
261  m_generateMap=false;
262 */
263 
264 
265 }
266 
268  pthread_mutex_destroy(&hp_mutex);
269  pthread_mutex_destroy(&ind_mutex);
270  pthread_mutex_destroy(&hist_mutex);
271 
272  for (deque<Event*>::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++)
273  delete *it;
274 }
275 
276 
278  if (! gpt->input && ! gpt->onLine)
279  return 0;
280 
281 
282  //if started online retrieve the settings from the connection
283 #ifdef CARMEN_SUPPORT
284  if (gpt->onLine){
285  cout << "starting the process:" << endl;
286  CarmenWrapper::initializeIPC(gpt->m_argv[0]);
287  CarmenWrapper::start(gpt->m_argv[0]);
288  cout << "Waiting for retrieving the sensor map:" << endl;
289  while (! CarmenWrapper::sensorMapComputed()){
290  usleep(500000);
291  cout << "." << flush;
292  }
293  gpt->sensorMap=CarmenWrapper::sensorMap();
294  cout << "Connected " << endl;
295  }
296 #else
297  if (gpt->onLine){
298  cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
299  DoneEvent *done=new DoneEvent;
300  gpt->addEvent(done);
301  return 0;
302  }
303 #endif
304 
305  gpt->setSensorMap(gpt->sensorMap);
306  gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip);
307 
308  double xmin=gpt->xmin,
309  ymin=gpt->ymin,
310  xmax=gpt->xmax,
311  ymax=gpt->ymax;
312 
313  OrientedPoint initialPose(0,0,0);
314 
315  if (gpt->autosize){
316  if (gpt->readFromStdin || gpt->onLine)
317  cout << "Error, cant autosize form stdin" << endl;
318  SensorLog * log=new SensorLog(gpt->sensorMap);
319  ifstream is(gpt->filename.c_str());
320  log->load(is);
321  is.close();
322  initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax);
323  delete log;
324  }
325 
326  if( gpt->infoStream()){
327  gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta
328  << cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl;
329  }
330  gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt);
332  gpt->setgenerateMap(gpt->generateMap);
333  gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose);
334  gpt->setllsamplerange(gpt->llsamplerange);
335  gpt->setllsamplestep(gpt->llsamplestep);
336  gpt->setlasamplerange(gpt->llsamplerange);
337  gpt->setlasamplestep(gpt->llsamplestep);
338 
339 #define printParam(n)\
340  gpt->outputStream() \
341  << "PARAM " << \
342  #n \
343  << " " << gpt->n << endl
344 
345  if (gpt->outfilename.length()>0 ){
346  gpt->outputStream().open(gpt->outfilename.c_str());
349  printParam(xmin);
350  printParam(ymin);
351  printParam(xmax);
352  printParam(ymax);
353  printParam(delta);
354 
355  //scan matching parameters
356  printParam(sigma);
360  printParam(lstep);
361  printParam(astep);
367  printParam(ogain);
368  printParam(lskip);
371 
372  //motion model parameters
373  printParam(srr);
374  printParam(srt);
375  printParam(str);
376  printParam(stt);
377  //particle parameters
380 
381  //gfs parameters
385 
390  }
391  #undef printParam
392 
393  if (gpt->randseed!=0)
394  sampleGaussian(1,gpt->randseed);
395  if (!gpt->infoStream()){
396  cerr << "cant open info stream for writing by unuseful debug messages" << endl;
397  } else {
398  gpt->infoStream() << "setting randseed" << gpt->randseed<< endl;
399  }
400 
401 
402 #ifdef CARMEN_SUPPORT
403  list<RangeReading*> rrlist;
404  if (gpt->onLine){
405  RangeReading rr(0,0);
406  while (1){
407  while (CarmenWrapper::getReading(rr)){
408  RangeReading* nr=new RangeReading(rr);
409  rrlist.push_back(nr);
410  gpt->processScan(*nr);
411  }
412  }
413  }
414 #endif
415  ofstream rawpath("rawpath.dat");
416  if (!gpt->onLine){
417  while(*(gpt->input) && gpt->running){
418  const SensorReading* r;
419  (*(gpt->input)) >> r;
420  if (! r)
421  continue;
422  const RangeReading* rr=dynamic_cast<const RangeReading*>(r);
423  if (rr && gpt->running){
424  const RangeSensor* rs=dynamic_cast<const RangeSensor*>(rr->getSensor());
425  assert (rs && rs->beams().size()==rr->size());
426 
427  bool processed=gpt->processScan(*rr);
428  rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl;
429  if (0 && processed){
430  cerr << "Retrieving state .. ";
431  TNodeVector trajetories=gpt->getTrajectories();
432  cerr << "Done" << endl;
433  cerr << "Deleting Tree state .. ";
434  for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++)
435  delete *it;
436  cerr << "Done" << endl;
437  }
438 // if (0 && processed){
439 // cerr << "generating copy" << endl;;
440 // GridSlamProcessor* m_gsp=gpt->clone();
441 // Map<double, DoubleArray2D, false>* pmap=m_gsp->getParticles()[0].map.toDoubleMap() ;
442 // cerr << "deleting" << endl;
443 // delete m_gsp;
444 // delete pmap;
445 // }
446  }
447  const OdometryReading* o=dynamic_cast<const OdometryReading*>(r);
448  if (o && gpt->running){
449  gpt->processTruePos(*o);
450  TruePosEvent* truepos=new TruePosEvent;
451  truepos->pose=o->getPose();
452  }
453  }
454  }
455  rawpath.close();
456 
457  TNodeVector trajetories=gpt->getTrajectories();
458  cerr << "WRITING WEIGHTS" << endl;
459  int pnumber=0;
460  for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){
461  char buf[10];
462  sprintf(buf, "w-%03d.dat",pnumber);
463  ofstream weightsStream(buf);
465  double oldWeight=0, oldgWeight=0;
466  while (n!=0){
467  double w=n->weight-oldWeight;
468  double gw=n->gweight-oldgWeight;
469  oldWeight=n->weight;
470  oldgWeight=n->gweight;
471  weightsStream << w << " " << gw << endl;
472  n=n->parent;
473  }
474  weightsStream.close();
475  pnumber++;
476  cerr << buf << endl;
477  }
478 
479  DoneEvent *done=new DoneEvent;
480  gpt->addEvent(done);
481  gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl;
482  return 0;
483 }
484 
485 std::vector<OrientedPoint> GridSlamProcessorThread::getHypotheses(){
486  pthread_mutex_lock(&hp_mutex);
487  std::vector<OrientedPoint> retval(hypotheses);
488  pthread_mutex_unlock(&hp_mutex);
489  return retval;
490 }
491 
492 std::vector<unsigned int> GridSlamProcessorThread::getIndexes(){
493  pthread_mutex_lock(&ind_mutex);
494  std::vector<unsigned int> retval(indexes);
495  pthread_mutex_unlock(&ind_mutex);
496  return retval;
497 }
498 
500  if (running)
501  return;
502  running=true;
503  pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
504 }
505 
507  if (! running){
508  cout << "PORCO CAZZO" << endl;
509  return;
510  }
511  running=false;
512  void * retval;
513  pthread_join(gfs_thread, &retval);
514 }
515 
517  pthread_mutex_lock(&hp_mutex);
518  hypotheses.clear();
519  weightSums.clear();
520  for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
521  hypotheses.push_back(part->pose);
522  weightSums.push_back(part->weightSum);
523  }
524 
526  event->scanmatched=false;
527  event->hypotheses=hypotheses;
528  event->weightSums=weightSums;
529  event->neff=m_neff;
530  pthread_mutex_unlock(&hp_mutex);
531 
532  addEvent(event);
533 
535 }
536 
538  pthread_mutex_lock(&ind_mutex);
539  pthread_mutex_lock(&hp_mutex);
540 
541  indexes=GridSlamProcessor::getIndexes();
542 
543  assert (indexes.size()==getParticles().size());
544  ResampleEvent* event=new ResampleEvent;
545  event->indexes=indexes;
546 
547  pthread_mutex_unlock(&hp_mutex);
548  pthread_mutex_unlock(&ind_mutex);
549 
550  addEvent(event);
551 
553 }
554 
556  pthread_mutex_lock(&hp_mutex);
557  hypotheses.clear();
558  weightSums.clear();
559  unsigned int bestIdx=0;
560  double bestWeight=-1e1000;
561  unsigned int idx=0;
562  for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){
563  hypotheses.push_back(part->pose);
564  weightSums.push_back(part->weightSum);
565  if(part->weightSum>bestWeight){
566  bestIdx=idx;
567  bestWeight=part->weightSum;
568  }
569  idx++;
570  }
571 
573  event->scanmatched=true;
574  event->hypotheses=hypotheses;
575  event->weightSums=weightSums;
576  event->neff=m_neff;
577  addEvent(event);
578 
579  if (! mapTimer){
580  MapEvent* event=new MapEvent;
581  event->index=bestIdx;
582  event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map);
583  event->pose=getParticles()[bestIdx].pose;
584  addEvent(event);
585  }
586 
587  mapTimer++;
589 
590  pthread_mutex_unlock(&hp_mutex);
591 
593 }
594 
596 }
597 
599 }
600 
602 }
603 
605  pthread_mutex_lock(&hist_mutex);
606  while (eventBuffer.size()>eventBufferLength){
607  Event* event=eventBuffer.front();
608  delete event;
609  eventBuffer.pop_front();
610  }
611  eventBuffer.push_back(e);
612  pthread_mutex_unlock(&hist_mutex);
613 }
614 
616  pthread_mutex_lock(&hist_mutex);
617  EventDeque copy(eventBuffer);
618  eventBuffer.clear();
619  pthread_mutex_unlock(&hist_mutex);
620  return copy;
621 }
622 
624 
626  if (pmap)
627  delete pmap;
628 }
629 
631  eventBufferLength=length;
632 }
633 
634 OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{
635  OrientedPoint initialPose(0,0,0);
636  initialPose=log->boundingBox(xmin, ymin, xmax, ymax);
637  xmin-=3*maxrange;
638  ymin-=3*maxrange;
639  xmax+=3*maxrange;
640  ymax+=3*maxrange;
641  return initialPose;
642 }
GridSlamProcessorThread::astep
double astep
Definition: gsp_thread.h:136
GMapping::ConfigFile
Definition: configfile.h:85
GMapping::GridSlamProcessor::infoStream
std::ostream & infoStream()
Definition: gridslamprocessor.cpp:493
GridSlamProcessorThread::angularOdometryReliability
double angularOdometryReliability
Definition: gsp_thread.h:149
GridSlamProcessorThread::addEvent
void addEvent(Event *)
Definition: gsp_thread.cpp:604
CMD_PARSE_BEGIN_SILENT
#define CMD_PARSE_BEGIN_SILENT(i, count)
Definition: commandline.h:72
parseString
#define parseString(name, value)
Definition: commandline.h:34
GridSlamProcessorThread::srr
double srr
Definition: gsp_thread.h:153
GridSlamProcessorThread::sigma
double sigma
Definition: gsp_thread.h:131
GridSlamProcessorThread::eventBuffer
EventDeque eventBuffer
Definition: gsp_thread.h:104
GridSlamProcessorThread::gfs_thread
pthread_t gfs_thread
Definition: gsp_thread.h:115
GridSlamProcessorThread::hypotheses
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:111
GMapping::GridSlamProcessor::TNodeVector
std::vector< GridSlamProcessor::TNode * > TNodeVector
Definition: gridslamprocessor.h:85
GridSlamProcessorThread::critscore
double critscore
Definition: gsp_thread.h:139
GridSlamProcessorThread::~GridSlamProcessorThread
~GridSlamProcessorThread()
Definition: gsp_thread.cpp:267
GridSlamProcessorThread::llsamplestep
double llsamplestep
Definition: gsp_thread.h:147
GMapping::GridSlamProcessor::setMatchingParameters
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)
Definition: gridslamprocessor.cpp:195
GridSlamProcessorThread::maxMove
double maxMove
Definition: gsp_thread.h:140
GridSlamProcessorThread::boundingBox
OrientedPoint boundingBox(SensorLog *log, double &xmin, double &ymin, double &xmax, double &ymax) const
Definition: gsp_thread.cpp:634
GMapping::InputSensorStream
Definition: sensorstream.h:24
GridSlamProcessorThread::ymax
double ymax
Definition: gsp_thread.h:125
is
ifstream is(argv[c])
GridSlamProcessorThread::TruePosEvent::pose
OrientedPoint pose
Definition: gsp_thread.h:57
GridSlamProcessorThread::onScanmatchUpdate
virtual void onScanmatchUpdate()
Definition: gsp_thread.cpp:555
GridSlamProcessorThread::mapTimer
unsigned int mapTimer
Definition: gsp_thread.h:108
GridSlamProcessorThread::init
int init(int argc, const char *const *argv)
Definition: gsp_thread.cpp:36
GridSlamProcessorThread::kernelSize
int kernelSize
Definition: gsp_thread.h:137
GridSlamProcessorThread
Definition: gsp_thread.h:44
parseInt
#define parseInt(name, value)
Definition: commandline.h:51
GMapping::CarmenConfiguration::computeSensorMap
virtual SensorMap computeSensorMap() const
Definition: carmenconfiguration.cpp:97
GridSlamProcessorThread::str
double str
Definition: gsp_thread.h:153
GridSlamProcessorThread::TruePosEvent
Definition: gsp_thread.h:56
GridSlamProcessorThread::MapEvent::~MapEvent
virtual ~MapEvent()
Definition: gsp_thread.cpp:625
n
#define n
Definition: eig3.cpp:11
GMapping::GridSlamProcessor::outputStream
std::ofstream & outputStream()
Definition: gridslamprocessor.cpp:489
GridSlamProcessorThread::stop
void stop()
Definition: gsp_thread.cpp:506
GridSlamProcessorThread::autosize
bool autosize
Definition: gsp_thread.h:126
GridSlamProcessorThread::srt
double srt
Definition: gsp_thread.h:153
GridSlamProcessorThread::readFromStdin
bool readFromStdin
Definition: gsp_thread.h:167
GridSlamProcessorThread::m_argv
const char *const * m_argv
Definition: gsp_thread.h:174
GridSlamProcessorThread::syncOdometryUpdate
virtual void syncOdometryUpdate()
Definition: gsp_thread.cpp:595
GMapping::OdometryReading::getPose
const OrientedPoint & getPose() const
Definition: odometryreading.h:15
GridSlamProcessorThread::GridSlamProcessorThread
GridSlamProcessorThread()
Definition: gsp_thread.cpp:188
GridSlamProcessorThread::DoneEvent
Definition: gsp_thread.h:71
GridSlamProcessorThread::stt
double stt
Definition: gsp_thread.h:153
GMapping::sampleGaussian
double UTILS_EXPORT sampleGaussian(double sigma, unsigned long int S=0)
Definition: stat.cpp:48
GMapping::RangeSensor::beams
const std::vector< Beam > & beams() const
Definition: rangesensor.h:24
GridSlamProcessorThread::onLine
bool onLine
Definition: gsp_thread.h:168
GMapping::GridSlamProcessor::processScan
bool processScan(const RangeReading &reading, int adaptParticles=0)
Definition: gridslamprocessor.cpp:316
parseDouble
#define parseDouble(name, value)
Definition: commandline.h:43
GridSlamProcessorThread::xmax
double xmax
Definition: gsp_thread.h:124
GridSlamProcessorThread::ResampleEvent::indexes
std::vector< unsigned int > indexes
Definition: gsp_thread.h:61
GridSlamProcessorThread::particles
int particles
Definition: gsp_thread.h:155
GridSlamProcessorThread::eventBufferLength
unsigned int eventBufferLength
Definition: gsp_thread.h:106
GridSlamProcessorThread::loadFiles
int loadFiles(const char *fn=0)
Definition: gsp_thread.cpp:153
GridSlamProcessorThread::resampleThreshold
double resampleThreshold
Definition: gsp_thread.h:128
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GridSlamProcessorThread::maxUrange
double maxUrange
Definition: gsp_thread.h:133
CMD_PARSE_END_SILENT
#define CMD_PARSE_END_SILENT
Definition: commandline.h:78
commandline.h
GridSlamProcessorThread::considerOdometryCovariance
bool considerOdometryCovariance
Definition: gsp_thread.h:170
GridSlamProcessorThread::setEventBufferSize
void setEventBufferSize(unsigned int length)
Definition: gsp_thread.cpp:630
GMapping::SensorLog::boundingBox
OrientedPoint boundingBox(double &xmin, double &ymin, double &xmax, double &ymax) const
Definition: sensorlog.cpp:121
GridSlamProcessorThread::ParticleMoveEvent::scanmatched
bool scanmatched
Definition: gsp_thread.h:50
GMapping::CarmenConfiguration
Definition: carmenconfiguration.h:14
CMD_PARSE_BEGIN
CMD_PARSE_BEGIN(1, argc-2)
GMapping::RangeSensor
Definition: rangesensor.h:11
GridSlamProcessorThread::generateMap
bool generateMap
Definition: gsp_thread.h:169
GMapping::OdometryReading
Definition: odometryreading.h:12
GMapping::GridSlamProcessor::processTruePos
void processTruePos(const OdometryReading &odometry)
Definition: gridslamprocessor.cpp:306
GridSlamProcessorThread::sensorMap
SensorMap sensorMap
Definition: gsp_thread.h:163
GridSlamProcessorThread::llsamplerange
double llsamplerange
Definition: gsp_thread.h:146
GMapping::GridSlamProcessor
Definition: gridslamprocessor.h:35
GridSlamProcessorThread::lstep
double lstep
Definition: gsp_thread.h:135
GridSlamProcessorThread::lskip
unsigned int lskip
Definition: gsp_thread.h:141
GridSlamProcessorThread::syncResampleUpdate
virtual void syncResampleUpdate()
Definition: gsp_thread.cpp:598
GMapping::point::y
T y
Definition: point.h:16
GMapping::GridSlamProcessor::setUpdateDistances
void setUpdateDistances(double linear, double angular, double resampleThreshold)
Definition: gridslamprocessor.cpp:224
GMapping::orientedpoint::theta
A theta
Definition: point.h:60
GMapping::GridSlamProcessor::TNode
Definition: gridslamprocessor.h:43
GridSlamProcessorThread::xmin
double xmin
Definition: gsp_thread.h:122
GridSlamProcessorThread::MapEvent::index
unsigned int index
Definition: gsp_thread.h:66
GMapping::SensorLog::load
std::istream & load(std::istream &is)
Definition: sensorlog.cpp:23
parseFlag
#define parseFlag(name, value)
Definition: commandline.h:27
GridSlamProcessorThread::ResampleEvent
Definition: gsp_thread.h:60
GridSlamProcessorThread::getHypotheses
std::vector< OrientedPoint > getHypotheses()
Definition: gsp_thread.cpp:485
GridSlamProcessorThread::EventDeque
deque< Event * > EventDeque
Definition: gsp_thread.h:74
GMapping::GridSlamProcessor::getParticles
const ParticleVector & getParticles() const
Definition: gridslamprocessor.h:173
GridSlamProcessorThread::randseed
unsigned int randseed
Definition: gsp_thread.h:171
GridSlamProcessorThread::running
bool running
Definition: gsp_thread.h:116
GMapping::SensorReading
Definition: sensoreading.h:9
GMapping::SensorReading::getSensor
const Sensor * getSensor() const
Definition: sensoreading.h:13
GMapping::ConfigFile::value
const AutoVal & value(const std::string &section, const std::string &entry) const
Definition: configfile.cpp:227
GridSlamProcessorThread::syncScanmatchUpdate
virtual void syncScanmatchUpdate()
Definition: gsp_thread.cpp:601
GridSlamProcessorThread::ParticleMoveEvent
Definition: gsp_thread.h:49
GridSlamProcessorThread::fastslamthread
static void * fastslamthread(GridSlamProcessorThread *gpt)
Definition: gsp_thread.cpp:277
GridSlamProcessorThread::delta
double delta
Definition: gsp_thread.h:127
GMapping::GridSlamProcessor::setSensorMap
void setSensorMap(const SensorMap &smap)
Definition: gridslamprocessor.cpp:242
GridSlamProcessorThread::onResampleUpdate
virtual void onResampleUpdate()
Definition: gsp_thread.cpp:537
GMapping::RangeReading
Definition: rangereading.h:17
GridSlamProcessorThread::iterations
int iterations
Definition: gsp_thread.h:138
GMapping::ScanMatcherMap
Map< PointAccumulator, HierarchicalArray2D< PointAccumulator > > ScanMatcherMap
Definition: smmap.h:50
GridSlamProcessorThread::filename
std::string filename
Definition: gsp_thread.h:119
stat.h
GridSlamProcessorThread::start
void start()
Definition: gsp_thread.cpp:499
GridSlamProcessorThread::hist_mutex
pthread_mutex_t hist_mutex
Definition: gsp_thread.h:114
GridSlamProcessorThread::linearOdometryReliability
double linearOdometryReliability
Definition: gsp_thread.h:148
GMapping::RangeReading::getPose
const OrientedPoint & getPose() const
Definition: rangereading.h:22
GMapping::GridSlamProcessor::setMotionModelParameters
void setMotionModelParameters(double srr, double srt, double str, double stt)
Definition: gridslamprocessor.cpp:212
GridSlamProcessorThread::outfilename
std::string outfilename
Definition: gsp_thread.h:120
GridSlamProcessorThread::input
InputSensorStream * input
Definition: gsp_thread.h:165
GridSlamProcessorThread::ind_mutex
pthread_mutex_t ind_mutex
Definition: gsp_thread.h:114
gsp_thread.h
GridSlamProcessorThread::lasamplestep
double lasamplestep
Definition: gsp_thread.h:147
GridSlamProcessorThread::ogain
double ogain
Definition: gsp_thread.h:145
GridSlamProcessorThread::regscore
double regscore
Definition: gsp_thread.h:134
CMD_PARSE_END
CMD_PARSE_END
Definition: gfs2stream.cpp:32
GridSlamProcessorThread::skipMatching
bool skipMatching
Definition: gsp_thread.h:156
GridSlamProcessorThread::Event::~Event
virtual ~Event()
Definition: gsp_thread.cpp:623
GridSlamProcessorThread::lsigma
double lsigma
Definition: gsp_thread.h:144
parseStringSilent
#define parseStringSilent(name, value)
Definition: commandline.h:90
GMapping::CarmenConfiguration::load
virtual std::istream & load(std::istream &is)
Definition: carmenconfiguration.cpp:17
GridSlamProcessorThread::onOdometryUpdate
virtual void onOdometryUpdate()
Definition: gsp_thread.cpp:516
GridSlamProcessorThread::MapEvent
Definition: gsp_thread.h:64
GMapping::orientedpoint< double, double >
GridSlamProcessorThread::getEvents
EventDeque getEvents()
Definition: gsp_thread.cpp:615
GridSlamProcessorThread::mapUpdateTime
unsigned int mapUpdateTime
Definition: gsp_thread.h:107
GridSlamProcessorThread::angularUpdate
double angularUpdate
Definition: gsp_thread.h:159
maxrange
double maxrange
Definition: gfs2stream.cpp:22
GridSlamProcessorThread::getIndexes
std::vector< unsigned int > getIndexes()
Definition: gsp_thread.cpp:492
GMapping::SensorLog
Definition: sensorlog.h:16
GridSlamProcessorThread::indexes
std::vector< unsigned int > indexes
Definition: gsp_thread.h:112
GridSlamProcessorThread::linearUpdate
double linearUpdate
Definition: gsp_thread.h:160
configfile.h
GridSlamProcessorThread::lasamplerange
double lasamplerange
Definition: gsp_thread.h:146
carmenwrapper.h
GridSlamProcessorThread::maxrange
double maxrange
Definition: gsp_thread.h:132
GridSlamProcessorThread::Event
Definition: gsp_thread.h:45
GMapping::GridSlamProcessor::getTrajectories
TNodeVector getTrajectories() const
Definition: gridslamprocessor_tree.cpp:40
printParam
#define printParam(n)
part
bool part
Definition: gfs2stream.cpp:40
GridSlamProcessorThread::weightSums
std::vector< double > weightSums
Definition: gsp_thread.h:113
GMapping::point::x
T x
Definition: point.h:16
GridSlamProcessorThread::hp_mutex
pthread_mutex_t hp_mutex
Definition: gsp_thread.h:114
GridSlamProcessorThread::ymin
double ymin
Definition: gsp_thread.h:123


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51