Main MRPT website > C++ reference for MRPT 1.5.9
CHMTSLAM_main.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 /** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
11  *
12  * The main entry points for a user are pushAction() and pushObservations(). Several parameters can be modified
13  * through m_options.
14  *
15  * The mathematical models of this approach have been reported in:
16  * - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J.,
17  * "Towards a Unified Bayesian Approach to Hybrid Metric-Topological SLAM",
18  * in IEEE Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
19  * - ...
20  *
21  * More information in the wiki page: http://www.mrpt.org/HMT-SLAM
22  *
23  */
24 
25 
26 #include "hmtslam-precomp.h" // Precomp header
27 
28 #include <mrpt/utils/CFileStream.h>
29 #include <mrpt/utils/CConfigFile.h>
31 #include <mrpt/system/filesystem.h>
34 
35 #include <mrpt/system/os.h>
36 
37 using namespace mrpt::slam;
38 using namespace mrpt::hmtslam;
39 using namespace mrpt::utils;
40 using namespace mrpt::synch;
41 using namespace mrpt::obs;
42 using namespace mrpt::maps;
43 using namespace mrpt::opengl;
44 using namespace std;
45 
46 
48 
49 
50 // Initialization of static members:
51 int64_t CHMTSLAM::m_nextAreaLabel = 0;
52 TPoseID CHMTSLAM::m_nextPoseID = 0;
53 THypothesisID CHMTSLAM::m_nextHypID = COMMON_TOPOLOG_HYP + 1;
54 
55 /*---------------------------------------------------------------
56  Constructor
57  ---------------------------------------------------------------*/
59  : m_inputQueue_cs("inputQueue_cs"),
60  m_map_cs("map_cs"),
61  m_LMHs_cs("LMHs_cs")
62 // m_semaphoreInputQueueHasData (0 /*Init state*/ ,1 /*Max*/ ),
63 // m_eventNewObservationInserted(0 /*Init state*/ ,10000 /*Max*/ )
64 {
65  // Initialize data structures:
66  // ----------------------------
67  m_terminateThreads = false;
71 
72  // Create threads:
73  // -----------------------
77 
78 
79  // Other variables:
80  m_LSLAM_method = NULL;
81 
82 
83  // Register default LC detectors:
84  // --------------------------------
87 
88  // Prepare an empty map:
90 }
91 
92 /*---------------------------------------------------------------
93  Destructor
94  ---------------------------------------------------------------*/
96 {
97  // Signal to threads that we are closing:
98  // -------------------------------------------
99  m_terminateThreads = true;
100 
101  // Wait for threads:
102  // ----------------------------------
103  MRPT_LOG_DEBUG("[CHMTSLAM::destructor] Waiting for threads end...\n");
104 
108 
109  MRPT_LOG_DEBUG("[CHMTSLAM::destructor] All threads finished.\n");
110 
111  // Save the resulting H.Map if logging
112  // --------------------------------------
113  if (!m_options.LOG_OUTPUT_DIR.empty())
114  {
115  try
116  {
117 /* // Update the last area(s) in the HMAP:
118  updateHierarchicalMapFromRBPF();
119 
120  // Save:
121  os::sprintf(auxFil,1000,"%s/hierarchicalMap.hmap",m_options.logOutputDirectory.c_str());
122 
123  CFileStream f(auxFil,fomWrite);
124  f << m_knownAreas;
125  */
126  }
127  catch(std::exception &e)
128  {
129  MRPT_LOG_WARN(mrpt::format("Ignoring exception at ~CHMTSLAM():\n%s",e.what()));
130  }
131  catch(...)
132  {
133  MRPT_LOG_WARN("Ignoring untyped exception at ~CHMTSLAM()");
134  }
135  }
136 
137  // Delete data structures:
138  // ----------------------------------
139  clearInputQueue();
140 
141  // Others:
142  if (m_LSLAM_method)
143  {
144  delete m_LSLAM_method;
145  m_LSLAM_method = NULL;
146  }
147 
148  // Delete TLC-detectors
149  {
151 
152  // Clear old list:
154  delete *it;
155  m_topLCdets.clear();
156  }
157 }
158 
159 
160 /*---------------------------------------------------------------
161  clearInputQueue
162  ---------------------------------------------------------------*/
164 {
165  // Wait for critical section
166  {
168 
169  while (!m_inputQueue.empty())
170  {
171  //delete m_inputQueue.front();
172  m_inputQueue.pop();
173  };
174  }
175 }
176 
177 
178 /*---------------------------------------------------------------
179  pushAction
180  ---------------------------------------------------------------*/
181 void CHMTSLAM::pushAction( const CActionCollectionPtr &acts )
182 {
183  if (m_terminateThreads)
184  {
185  // Discard it:
186  //delete acts;
187  return;
188  }
189 
190  { // Wait for critical section
192  m_inputQueue.push( acts );
193  }
194 }
195 
196 /*---------------------------------------------------------------
197  pushObservations
198  ---------------------------------------------------------------*/
199 void CHMTSLAM::pushObservations( const CSensoryFramePtr &sf )
200 {
201  if (m_terminateThreads)
202  {
203  // Discard it:
204  //delete sf;
205  return;
206  }
207 
208  { // Wait for critical section
210  m_inputQueue.push( sf );
211  }
212 }
213 
214 /*---------------------------------------------------------------
215  pushObservation
216  ---------------------------------------------------------------*/
217 void CHMTSLAM::pushObservation( const CObservationPtr &obs )
218 {
219  if (m_terminateThreads)
220  { // Discard it:
221  //delete obs;
222  return;
223  }
224 
225  // Add a CSensoryFrame with the obs:
226  CSensoryFramePtr sf = CSensoryFrame::Create();
227  sf->insert(obs); // memory will be freed when deleting the SF in other thread
228 
229  { // Wait for critical section
231  m_inputQueue.push( sf );
232  }
233 }
234 
235 /*---------------------------------------------------------------
236  loadOptions
237  ---------------------------------------------------------------*/
239 {
240  m_options.loadFromConfigFile(cfg,"HMT-SLAM");
241 
243 
244  m_options.pf_options.loadFromConfigFile(cfg,"PARTICLE_FILTER");
245 
247 
248  m_options.AA_options.loadFromConfigFile(cfg,"GRAPH_CUT");
249 
250  // Topological Loop Closure detector options:
251  m_options.TLC_grid_options.loadFromConfigFile(cfg,"TLC_GRIDMATCHING");
253 
255 }
256 
257 /*---------------------------------------------------------------
258  loadOptions
259  ---------------------------------------------------------------*/
260 void CHMTSLAM::loadOptions( const std::string &configFile )
261 {
262  ASSERT_( mrpt::system::fileExists(configFile) );
263  CConfigFile cfg(configFile);
264  loadOptions(cfg);
265 }
266 
267 /*---------------------------------------------------------------
268  TOptions
269  ---------------------------------------------------------------*/
271 {
272  LOG_OUTPUT_DIR = "";
273  LOG_FREQUENCY = 1;
274 
276 
279 
282 
285 
286  random_seed = 1234;
287 
288  TLC_detectors.clear();
289 
290  stds_Q_no_odo.resize(3);
291  stds_Q_no_odo[0] =
292  stds_Q_no_odo[1] = 0.10f;
293  stds_Q_no_odo[2] = DEG2RAD(4.0f);
294 }
295 
296 /*---------------------------------------------------------------
297  loadFromConfigFile
298  ---------------------------------------------------------------*/
301  const std::string &section)
302 {
303  MRPT_LOAD_CONFIG_VAR( LOG_OUTPUT_DIR, string, source, section);
304  MRPT_LOAD_CONFIG_VAR( LOG_FREQUENCY, int, source, section);
305 
306  MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(SLAM_METHOD, int, TLSlamMethod, source, section);
307 
308  MRPT_LOAD_CONFIG_VAR( SLAM_MIN_DIST_BETWEEN_OBS, float, source, section);
309  MRPT_LOAD_CONFIG_VAR_DEGREES( SLAM_MIN_HEADING_BETWEEN_OBS, source, section);
310 
311  MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY,float, source,section);
312  MRPT_LOAD_CONFIG_VAR_DEGREES( MIN_ODOMETRY_STD_PHI, source,section);
313 
314  MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_HEIGHT, float, source, section);
315  MRPT_LOAD_CONFIG_VAR( VIEW3D_AREA_SPHERES_RADIUS, float, source, section);
316 
317  MRPT_LOAD_CONFIG_VAR( random_seed, int, source,section);
318 
319  stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
320  source.read_vector(section,"stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo );
321  ASSERT_(stds_Q_no_odo.size()==3)
322 
323  stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);
324 
325  std::string sTLC_detectors = source.read_string(section,"TLC_detectors", "", true );
326 
327  mrpt::system::tokenize(sTLC_detectors,", ",TLC_detectors);
328 
329  std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;
330 
331  // load other sub-classes:
332  AA_options.loadFromConfigFile(source,section);
333 }
334 
335 /*---------------------------------------------------------------
336  dumpToTextStream
337  ---------------------------------------------------------------*/
339 {
340  out.printf("\n----------- [CHMTSLAM::TOptions] ------------ \n\n");
341 
342  LOADABLEOPTS_DUMP_VAR( LOG_OUTPUT_DIR, string );
343  LOADABLEOPTS_DUMP_VAR( LOG_FREQUENCY, int);
344 
345  LOADABLEOPTS_DUMP_VAR( SLAM_METHOD, int);
346 
347  LOADABLEOPTS_DUMP_VAR( SLAM_MIN_DIST_BETWEEN_OBS, float );
348  LOADABLEOPTS_DUMP_VAR_DEG( SLAM_MIN_HEADING_BETWEEN_OBS );
349 
350  LOADABLEOPTS_DUMP_VAR( MIN_ODOMETRY_STD_XY, float );
351  LOADABLEOPTS_DUMP_VAR_DEG( MIN_ODOMETRY_STD_PHI );
352 
353  LOADABLEOPTS_DUMP_VAR( random_seed, int );
354 
355  AA_options.dumpToTextStream(out);
356  pf_options.dumpToTextStream(out);
357  KLD_params.dumpToTextStream(out);
358  defaultMapsInitializers.dumpToTextStream(out);
359  TLC_grid_options.dumpToTextStream(out);
360  TLC_fabmap_options.dumpToTextStream(out);
361 }
362 
363 /*---------------------------------------------------------------
364  isInputQueueEmpty
365  ---------------------------------------------------------------*/
367 {
368  bool res;
369 
370  { // Wait for critical section
372  res = m_inputQueue.empty();
373  }
374  return res;
375 }
376 
377 /*---------------------------------------------------------------
378  inputQueueSize
379  ---------------------------------------------------------------*/
381 {
382  size_t res;
383  { // Wait for critical section
385  res = m_inputQueue.size();
386  }
387  return res;
388 }
389 
390 /*---------------------------------------------------------------
391  getNextObjectFromInputQueue
392  ---------------------------------------------------------------*/
394 {
395  CSerializablePtr obj;
396 
397  { // Wait for critical section
399  if (!m_inputQueue.empty())
400  {
401  obj = m_inputQueue.front();
402  m_inputQueue.pop();
403  }
404  }
405  return obj;
406 }
407 
408 /*---------------------------------------------------------------
409  initializeEmptyMap
410  ---------------------------------------------------------------*/
412 {
413  THypothesisIDSet LMH_hyps;
414  THypothesisID newHypothID = generateHypothesisID();
415 
416  LMH_hyps.insert( COMMON_TOPOLOG_HYP );
417  LMH_hyps.insert( newHypothID );
418 
419  // ------------------------------------
420  // CLEAR HIERARCHICAL MAP
421  // ------------------------------------
422  CHMHMapNode::TNodeID firstAreaID;
423  {
425 
426  // Initialize hierarchical structures:
427  // -----------------------------------------------------
428  m_map.clear();
429 
430  // Create a single node for the starting area:
431  CHMHMapNodePtr firstArea = CHMHMapNode::Create( &m_map );
432  firstAreaID = firstArea->getID();
433 
434  firstArea->m_hypotheses = LMH_hyps;
436 
437  firstArea->m_nodeType.setType( "Area" );
438  firstArea->m_label = generateUniqueAreaLabel();
439  firstArea->m_annotations.set( NODE_ANNOTATION_METRIC_MAPS, emptyMap, newHypothID );
440  firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, POSEID_INVALID , newHypothID );
441  } // end of lock m_map_cs
442 
443  // ------------------------------------
444  // CLEAR LIST OF HYPOTHESES
445  // ------------------------------------
446  {
448 
449  // Add to the list:
450  m_LMHs.clear();
451  CLocalMetricHypothesis &newLMH = m_LMHs[newHypothID];
452  newLMH.m_parent = this;
453 
454  newLMH.m_currentRobotPose = POSEID_INVALID ; // Special case: map is empty
455  newLMH.m_log_w = 0;
456  newLMH.m_ID = newHypothID;
457 
458  newLMH.m_neighbors.clear();
459  newLMH.m_neighbors.insert( firstAreaID );
460 
461  newLMH.clearRobotPoses();
462  } // end of cs
463 
464 
465  // ------------------------------------------
466  // Create the local SLAM algorithm object
467  // -----------------------------------------
468  switch( m_options.SLAM_METHOD )
469  {
470  case lsmRBPF_2DLASER:
471  {
472  if (m_LSLAM_method) delete m_LSLAM_method;
474  }
475  break;
476  default:
477  THROW_EXCEPTION_FMT("Invalid selection for LSLAM method: %i",(int)m_options.SLAM_METHOD );
478  };
479 
480  // ------------------------------------
481  // Topological LC detectors:
482  // ------------------------------------
483  {
485 
486  // Clear old list:
488  delete *it;
489  m_topLCdets.clear();
490 
491  // Create new list:
492  // 1: Occupancy Grid matching.
493  // 2: Cummins' image matching.
495  m_topLCdets.push_back( loopClosureDetector_factory(*d) );
496  }
497 
498 
499  // ------------------------------------
500  // Other variables:
501  // ------------------------------------
503 
504  // ------------------------------------
505  // Delete log files:
506  // ------------------------------------
507  if (!m_options.LOG_OUTPUT_DIR.empty())
508  {
516  }
517 
518 }
519 
520 /*---------------------------------------------------------------
521  generateUniqueAreaLabel
522  ---------------------------------------------------------------*/
524 {
525  return format( "%li", (long int)(m_nextAreaLabel++) );
526 }
527 
528 /*---------------------------------------------------------------
529  generatePoseID
530  ---------------------------------------------------------------*/
532 {
533  return m_nextPoseID++;
534 }
535 
536 /*---------------------------------------------------------------
537  generateHypothesisID
538  ---------------------------------------------------------------*/
540 {
541  return m_nextHypID++;
542 }
543 
544 
545 /*---------------------------------------------------------------
546  getAs3DScene
547  ---------------------------------------------------------------*/
549 {
550  MRPT_UNUSED_PARAM(scene3D);
551 }
552 
553 
554 /*---------------------------------------------------------------
555  abortedDueToErrors
556  ---------------------------------------------------------------*/
558 {
559  return m_terminationFlag_LSLAM ||
562 }
563 
564 /*---------------------------------------------------------------
565  registerLoopClosureDetector
566  ---------------------------------------------------------------*/
568  const std::string &name,
569  CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*)
570  )
571 {
572  m_registeredLCDetectors[name] = ptrCreateObject;
573 }
574 
575 /*---------------------------------------------------------------
576  loopClosureDetector_factory
577  ---------------------------------------------------------------*/
579 {
580  MRPT_START
582  if (it==m_registeredLCDetectors.end())
583  THROW_EXCEPTION_FMT("Invalid value for TLC_detectors: %s", name.c_str() );
584  return it->second(this);
585  MRPT_END
586 }
587 
588 
589 
590 /*---------------------------------------------------------------
591  saveState
592  ---------------------------------------------------------------*/
593 bool CHMTSLAM::saveState( CStream &out ) const
594 {
595  try
596  {
597  out << *this;
598  return true;
599  }
600  catch (...)
601  {
602  return false;
603  }
604 }
605 
606 /*---------------------------------------------------------------
607  loadState
608  ---------------------------------------------------------------*/
610 {
611  try
612  {
613  in >> *this;
614  return true;
615  }
616  catch (...)
617  {
618  return false;
619  }
620 }
621 
622 /*---------------------------------------------------------------
623  readFromStream
624  ---------------------------------------------------------------*/
626 {
627  switch(version)
628  {
629  case 0:
630  {
631  // Acquire all critical sections before!
632  // -------------------------------------------
633  //std::map< THypothesisID, CLocalMetricHypothesis >::const_iterator it;
634 
635  //CCriticalSectionLocker LMHs( & m_LMHs_cs );
636  //for (it=m_LMHs.begin();it!=m_LMHs.end();it++) it->second.m_lock.enter();
637 
638  CCriticalSectionLocker lock_map( &m_map_cs );
639 
640  // Data:
642  >> m_nextPoseID
643  >> m_nextHypID;
644 
645  // The HMT-MAP:
646  in >> m_map;
647 
648  // The LMHs:
649  in >> m_LMHs;
650 
651  // Save options??? Better allow changing them...
652 
653  // Release all critical sections:
654  //for (it=m_LMHs.begin();it!=m_LMHs.end();it++) it->second.m_lock.enter();
655 
656  } break;
657  default:
659  };
660 }
661 
662 /*---------------------------------------------------------------
663  writeToStream
664  Implements the writing to a CStream capability of
665  CSerializable objects
666  ---------------------------------------------------------------*/
668 {
669  if (version)
670  *version = 0;
671  else
672  {
673  // Acquire all critical sections before!
674  // -------------------------------------------
675  //std::map< THypothesisID, CLocalMetricHypothesis >::const_iterator it;
676 
677  //CCriticalSectionLocker LMHs( & m_LMHs_cs );
678  //for (it=m_LMHs.begin();it!=m_LMHs.end();it++) it->second.m_lock.enter();
679 
680  CCriticalSectionLocker lock_map( &m_map_cs );
681 
682  // Data:
683  out << m_nextAreaLabel
684  << m_nextPoseID
685  << m_nextHypID;
686 
687  // The HMT-MAP:
688  out << m_map;
689 
690  // The LMHs:
691  out << m_LMHs;
692 
693  // Save options??? Better allow changing them...
694 
695  // Release all critical sections:
696  //for (it=m_LMHs.begin();it!=m_LMHs.end();it++) it->second.m_lock.enter();
697 
698 
699  }
700 }
701 
702 
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
float MIN_ODOMETRY_STD_PHI
[LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0) ...
Definition: CHMTSLAM.h:420
int random_seed
0 means randomize, use any other value to have repetitive experiments.
Definition: CHMTSLAM.h:442
float MIN_ODOMETRY_STD_XY
[LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0) ...
Definition: CHMTSLAM.h:417
CLSLAMAlgorithmBase * m_LSLAM_method
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" o...
Definition: CHMTSLAM.h:275
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
synch::CCriticalSection m_map_cs
Critical section for accessing m_map.
Definition: CHMTSLAM.h:221
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
void thread_LSLAM()
The function for the "Local SLAM" thread.
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
int LOG_FREQUENCY
[LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
Definition: CHMTSLAM.h:406
#define COMMON_TOPOLOG_HYP
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:451
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
static TPoseID generatePoseID()
Generates a new and unique pose ID.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
mrpt::system::TThreadHandle m_hThread_TBI
Definition: CHMTSLAM.h:240
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Scalar * iterator
Definition: eigen_plugins.h:23
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
static TPoseID m_nextPoseID
Definition: CHMTSLAM.h:331
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
bool m_terminationFlag_3D_viewer
Definition: CHMTSLAM.h:317
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
vector_string TLC_detectors
A list of topological loop-closure detectors to use: can be one or more from this list: &#39;gridmaps&#39;: O...
Definition: CHMTSLAM.h:448
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
double m_log_w
Log-weight of this hypothesis.
STL namespace.
void pushObservations(const mrpt::obs::CSensoryFramePtr &sf)
Here the user can enter observations into the system (will go to the SLAM process).
const Scalar * const_iterator
Definition: eigen_plugins.h:24
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:450
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
aligned_containers< THypothesisID, CLocalMetricHypothesis >::map_t m_LMHs
The list of LMHs at each instant.
Definition: CHMTSLAM.h:380
static THypothesisID m_nextHypID
Definition: CHMTSLAM.h:332
bool m_terminateThreads
Termination flag for signaling all threads to terminate.
Definition: CHMTSLAM.h:313
static CTopLCDetectorBase * createNewInstance(CHMTSLAM *hmtslam)
A class factory, to be implemented in derived classes.
IMPLEMENTS_SERIALIZABLE(CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav) IMPLEMENTS_SERIALIZABLE(CHolonomicFullEval
utils::CMessageQueue m_LSLAM_queue
LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA.
Definition: CHMTSLAM.h:144
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
static int64_t m_nextAreaLabel
Definition: CHMTSLAM.h:330
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
This class allows loading and storing values and vectors of different types from a configuration text...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::queue< mrpt::utils::CSerializablePtr > m_inputQueue
The queue of pending actions/observations supplied by the user waiting for being processed.
Definition: CHMTSLAM.h:215
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::slam::TKLDParams KLD_params
Definition: CHMTSLAM.h:440
void clear()
Erase all the contents of map (It delete all nodes/arcs objects)
bool m_terminationFlag_LSLAM
Threads termination flags:
Definition: CHMTSLAM.h:317
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define MRPT_END
std::string LOG_OUTPUT_DIR
[LOGGING] If it is not an empty string (""), a directory with that name will be created and log files...
Definition: CHMTSLAM.h:405
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &sectionName) MRPT_OVERRIDE
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
#define POSEID_INVALID
#define MRPT_LOG_WARN(_STRING)
__int64 int64_t
Definition: rptypes.h:51
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers
The default set of maps to be created in each particle.
Definition: CHMTSLAM.h:438
mrpt::maps::CMultiMetricMapPtr CMultiMetricMapPtr
Backward compatible typedef.
mrpt::utils::CSerializablePtr getNextObjectFromInputQueue()
Used from the LSLAM thread to retrieve the next object from the queue.
std::deque< CTopLCDetectorBase * > m_topLCdets
The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".
Definition: CHMTSLAM.h:288
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
class HMTSLAM_IMPEXP CHMTSLAM
CHierarchicalMHMap m_map
The hiearchical, multi-hypothesis graph-based map.
Definition: CHMTSLAM.h:379
mrpt::hmtslam::CHMTSLAM::TOptions m_options
#define MRPT_LOG_DEBUG(_STRING)
int version
Definition: mrpt_jpeglib.h:898
This namespace provides multitask, synchronization utilities.
void pushAction(const mrpt::obs::CActionCollectionPtr &acts)
Here the user can enter an action into the system (will go to the SLAM process).
void thread_TBI()
The function for the "TBI" thread.
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:3919
void getAs3DScene(mrpt::opengl::COpenGLScene &outScene)
Gets a 3D representation of the current state of the whole mapping framework.
TLSlamMethod SLAM_METHOD
[LSLAM] The method to use for local SLAM
Definition: CHMTSLAM.h:410
virtual ~CHMTSLAM()
Destructor.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void BASE_IMPEXP joinThread(TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:74
bayes::CParticleFilter::TParticleFilterOptions pf_options
These params are used from every LMH object.
Definition: CHMTSLAM.h:439
size_t inputQueueSize()
Returns the number of objects waiting for processing in the input queue.
bool saveState(mrpt::utils::CStream &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:121
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
Definition: CHMTSLAM.h:432
#define MRPT_START
TOptions()
Initialization of default params.
void loadOptions(const std::string &configFile)
Loads the options from a config file.
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
#define RAD2DEG
mrpt::maps::CMultiMetricMap CMultiMetricMap
Backward compatible typedef.
void clear()
Clear the queue of messages, freeing memory as required.
mrpt::system::TThreadHandle m_hThread_LSLAM
Threads handles.
Definition: CHMTSLAM.h:240
static CTopLCDetectorBase * createNewInstance(CHMTSLAM *hmtslam)
A class factory, to be implemented in derived classes.
mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options
[AA] The options for the partitioning algorithm
Definition: CHMTSLAM.h:436
float VIEW3D_AREA_SPHERES_HEIGHT
[VIEW3D] The height of the areas&#39; spheres.
Definition: CHMTSLAM.h:424
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
std::map< std::string, TLopLCDetectorFactory > m_registeredLCDetectors
Definition: CHMTSLAM.h:285
void clearInputQueue()
Empty the input queue.
void registerLoopClosureDetector(const std::string &name, CTopLCDetectorBase *(*ptrCreateObject)(CHMTSLAM *))
Must be invoked before calling initializeEmptyMap, so LC objects can be created.
static CHMHMapNodePtr Create()
void pushObservation(const mrpt::obs::CObservationPtr &obs)
Here the user can enter an observation into the system (will go to the SLAM process).
float VIEW3D_AREA_SPHERES_RADIUS
[VIEW3D] The radius of the areas&#39; spheres.
Definition: CHMTSLAM.h:428
bool loadState(mrpt::utils::CStream &in)
Load the state of the whole HMT-SLAM framework from some binary stream (e.g.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
GLuint in
Definition: glext.h:6301
The namespace for 3D scene representation and rendering.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
GLuint const GLchar * name
Definition: glext.h:3891
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
#define NODE_ANNOTATION_REF_POSEID
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
static std::string generateUniqueAreaLabel()
Generates a new and unique area textual label (currently this generates "0","1",...)
bool BASE_IMPEXP deleteFilesInDirectory(const std::string &s, bool deleteDirectoryAsWell=false)
Delete all the files in a given directory (nothing done if directory does not exists, or path is a file).
Definition: filesystem.cpp:209
synch::CCriticalSection m_topLCdets_cs
The critical section for accessing m_topLCdets.
Definition: CHMTSLAM.h:291
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
void thread_3D_viewer()
The function for the "3D viewer" thread.
synch::CCriticalSection m_LMHs_cs
Critical section for accessing m_LMHs.
Definition: CHMTSLAM.h:223
GLuint res
Definition: glext.h:6298
float SLAM_MIN_DIST_BETWEEN_OBS
[LSLAM] Minimum distance (and heading) difference between observations inserted in the map...
Definition: CHMTSLAM.h:414
CTopLCDetectorBase * loopClosureDetector_factory(const std::string &name)
The class factory for topological loop closure detectors.
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
bool isInputQueueEmpty()
Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueu...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: TKLDParams.cpp:53
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
mrpt::system::TThreadHandle m_hThread_3D_viewer
Definition: CHMTSLAM.h:240
static THypothesisID generateHypothesisID()
Generates a new and unique hypothesis ID.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
synch::CCriticalSection m_inputQueue_cs
Critical section for accessing m_inputQueue.
Definition: CHMTSLAM.h:218
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
void initializeEmptyMap()
Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory...
bool abortedDueToErrors()
Return true if an exception has been caught in any thread leading to the end of the mapping applicati...
friend class CLSLAM_RBPF_2DLASER
Definition: CHMTSLAM.h:62



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020