Main MRPT website > C++ reference for MRPT 1.5.6
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;
68  m_terminationFlag_LSLAM =
69  m_terminationFlag_TBI =
70  m_terminationFlag_3D_viewer = false;
71 
72  // Create threads:
73  // -----------------------
74  m_hThread_LSLAM = mrpt::system::createThreadFromObjectMethod( this, &CHMTSLAM::thread_LSLAM );
75  m_hThread_TBI = mrpt::system::createThreadFromObjectMethod( this, &CHMTSLAM::thread_TBI );
76  m_hThread_3D_viewer = mrpt::system::createThreadFromObjectMethod( this, &CHMTSLAM::thread_3D_viewer );
77 
78 
79  // Other variables:
80  m_LSLAM_method = NULL;
81 
82 
83  // Register default LC detectors:
84  // --------------------------------
85  registerLoopClosureDetector("gridmaps", & CTopLCDetector_GridMatching::createNewInstance );
86  registerLoopClosureDetector("fabmap", & CTopLCDetector_FabMap::createNewInstance );
87 
88  // Prepare an empty map:
89  initializeEmptyMap();
90 }
91 
92 /*---------------------------------------------------------------
93  Destructor
94  ---------------------------------------------------------------*/
95 CHMTSLAM::~CHMTSLAM()
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 
105  mrpt::system::joinThread( m_hThread_3D_viewer );
106  mrpt::system::joinThread( m_hThread_LSLAM );
107  mrpt::system::joinThread( m_hThread_TBI );
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  {
150  synch::CCriticalSectionLocker lock( &m_topLCdets_cs );
151 
152  // Clear old list:
153  for (std::deque<CTopLCDetectorBase*>::iterator it=m_topLCdets.begin();it!=m_topLCdets.end();++it)
154  delete *it;
155  m_topLCdets.clear();
156  }
157 }
158 
159 
160 /*---------------------------------------------------------------
161  clearInputQueue
162  ---------------------------------------------------------------*/
163 void CHMTSLAM::clearInputQueue()
164 {
165  // Wait for critical section
166  {
167  CCriticalSectionLocker locker( &m_inputQueue_cs);
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
191  CCriticalSectionLocker locker( &m_inputQueue_cs);
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
209  CCriticalSectionLocker locker( &m_inputQueue_cs);
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
230  CCriticalSectionLocker locker( &m_inputQueue_cs);
231  m_inputQueue.push( sf );
232  }
233 }
234 
235 /*---------------------------------------------------------------
236  loadOptions
237  ---------------------------------------------------------------*/
238 void CHMTSLAM::loadOptions( const mrpt::utils::CConfigFileBase &cfg )
239 {
240  m_options.loadFromConfigFile(cfg,"HMT-SLAM");
241 
242  m_options.defaultMapsInitializers.loadFromConfigFile(cfg,"MetricMaps");
243 
244  m_options.pf_options.loadFromConfigFile(cfg,"PARTICLE_FILTER");
245 
246  m_options.KLD_params.loadFromConfigFile(cfg,"KLD");
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");
252  m_options.TLC_fabmap_options.loadFromConfigFile(cfg,"TLC_FABMAP");
253 
254  m_options.dumpToConsole();
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  ---------------------------------------------------------------*/
270 CHMTSLAM::TOptions::TOptions()
271 {
272  LOG_OUTPUT_DIR = "";
273  LOG_FREQUENCY = 1;
274 
275  SLAM_METHOD = lsmRBPF_2DLASER;
276 
277  SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
278  SLAM_MIN_HEADING_BETWEEN_OBS = DEG2RAD(25.0f);
279 
280  MIN_ODOMETRY_STD_XY = 0;
281  MIN_ODOMETRY_STD_PHI = 0;
282 
283  VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
284  VIEW3D_AREA_SPHERES_RADIUS = 1.0f;
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  ---------------------------------------------------------------*/
299 void CHMTSLAM::TOptions::loadFromConfigFile(
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  ---------------------------------------------------------------*/
338 void CHMTSLAM::TOptions::dumpToTextStream(mrpt::utils::CStream &out) const
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  ---------------------------------------------------------------*/
366 bool CHMTSLAM::isInputQueueEmpty()
367 {
368  bool res;
369 
370  { // Wait for critical section
371  CCriticalSectionLocker locker( &m_inputQueue_cs);
372  res = m_inputQueue.empty();
373  }
374  return res;
375 }
376 
377 /*---------------------------------------------------------------
378  inputQueueSize
379  ---------------------------------------------------------------*/
380 size_t CHMTSLAM::inputQueueSize()
381 {
382  size_t res;
383  { // Wait for critical section
384  CCriticalSectionLocker locker( &m_inputQueue_cs);
385  res = m_inputQueue.size();
386  }
387  return res;
388 }
389 
390 /*---------------------------------------------------------------
391  getNextObjectFromInputQueue
392  ---------------------------------------------------------------*/
393 CSerializablePtr CHMTSLAM::getNextObjectFromInputQueue()
394 {
395  CSerializablePtr obj;
396 
397  { // Wait for critical section
398  CCriticalSectionLocker locker( &m_inputQueue_cs);
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  ---------------------------------------------------------------*/
411 void CHMTSLAM::initializeEmptyMap()
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  {
424  synch::CCriticalSectionLocker locker( &m_map_cs );
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;
435  CMultiMetricMapPtr emptyMap = CMultiMetricMapPtr(new CMultiMetricMap(&m_options.defaultMapsInitializers) );
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  {
447  synch::CCriticalSectionLocker lock( &m_LMHs_cs );
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;
473  m_LSLAM_method = new CLSLAM_RBPF_2DLASER(this);
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  {
484  synch::CCriticalSectionLocker lock( &m_topLCdets_cs );
485 
486  // Clear old list:
487  for (std::deque<CTopLCDetectorBase*>::iterator it=m_topLCdets.begin();it!=m_topLCdets.end();++it)
488  delete *it;
489  m_topLCdets.clear();
490 
491  // Create new list:
492  // 1: Occupancy Grid matching.
493  // 2: Cummins' image matching.
494  for (vector_string::const_iterator d=m_options.TLC_detectors.begin();d!=m_options.TLC_detectors.end();++d)
495  m_topLCdets.push_back( loopClosureDetector_factory(*d) );
496  }
497 
498 
499  // ------------------------------------
500  // Other variables:
501  // ------------------------------------
502  m_LSLAM_queue.clear();
503 
504  // ------------------------------------
505  // Delete log files:
506  // ------------------------------------
507  if (!m_options.LOG_OUTPUT_DIR.empty())
508  {
509  mrpt::system::deleteFilesInDirectory( m_options.LOG_OUTPUT_DIR );
510  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR );
511  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMAP_txt" );
512  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMAP_3D" );
513  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/LSLAM_3D" );
514  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/ASSO" );
515  mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMTSLAM_state" );
516  }
517 
518 }
519 
520 /*---------------------------------------------------------------
521  generateUniqueAreaLabel
522  ---------------------------------------------------------------*/
523 std::string CHMTSLAM::generateUniqueAreaLabel()
524 {
525  return format( "%li", (long int)(m_nextAreaLabel++) );
526 }
527 
528 /*---------------------------------------------------------------
529  generatePoseID
530  ---------------------------------------------------------------*/
531 TPoseID CHMTSLAM::generatePoseID()
532 {
533  return m_nextPoseID++;
534 }
535 
536 /*---------------------------------------------------------------
537  generateHypothesisID
538  ---------------------------------------------------------------*/
539 THypothesisID CHMTSLAM::generateHypothesisID()
540 {
541  return m_nextHypID++;
542 }
543 
544 
545 /*---------------------------------------------------------------
546  getAs3DScene
547  ---------------------------------------------------------------*/
548 void CHMTSLAM::getAs3DScene( COpenGLScene &scene3D )
549 {
550  MRPT_UNUSED_PARAM(scene3D);
551 }
552 
553 
554 /*---------------------------------------------------------------
555  abortedDueToErrors
556  ---------------------------------------------------------------*/
557 bool CHMTSLAM::abortedDueToErrors()
558 {
559  return m_terminationFlag_LSLAM ||
560  m_terminationFlag_TBI ||
561  m_terminationFlag_3D_viewer;
562 }
563 
564 /*---------------------------------------------------------------
565  registerLoopClosureDetector
566  ---------------------------------------------------------------*/
567 void CHMTSLAM::registerLoopClosureDetector(
568  const std::string &name,
569  CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*)
570  )
571 {
572  m_registeredLCDetectors[name] = ptrCreateObject;
573 }
574 
575 /*---------------------------------------------------------------
576  loopClosureDetector_factory
577  ---------------------------------------------------------------*/
578 CTopLCDetectorBase* CHMTSLAM::loopClosureDetector_factory(const std::string &name)
579 {
580  MRPT_START
581  std::map<std::string,TLopLCDetectorFactory>::const_iterator it=m_registeredLCDetectors.find( name );
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  ---------------------------------------------------------------*/
609 bool CHMTSLAM::loadState( CStream &in )
610 {
611  try
612  {
613  in >> *this;
614  return true;
615  }
616  catch (...)
617  {
618  return false;
619  }
620 }
621 
622 /*---------------------------------------------------------------
623  readFromStream
624  ---------------------------------------------------------------*/
625 void CHMTSLAM::readFromStream(mrpt::utils::CStream &in,int version)
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:
641  in >> m_nextAreaLabel
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  ---------------------------------------------------------------*/
667 void CHMTSLAM::writeToStream(mrpt::utils::CStream &out, int *version) const
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 
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
bool BASE_IMPEXP createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:154
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
#define COMMON_TOPOLOG_HYP
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
#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...
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
double m_log_w
Log-weight of this hypothesis.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:59
GLuint in
Definition: glew.h:7146
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::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define POSEID_INVALID
#define MRPT_LOG_WARN(_STRING)
__int64 int64_t
Definition: rptypes.h:51
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::maps::CMultiMetricMapPtr CMultiMetricMapPtr
Backward compatible typedef.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLhandleARB obj
Definition: glew.h:3276
GLuint const GLchar * name
Definition: glew.h:1721
#define MRPT_LOG_DEBUG(_STRING)
int version
Definition: mrpt_jpeglib.h:898
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
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:216
GLuint res
Definition: glew.h:7143
#define MRPT_START
#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.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
#define MRPT_LOAD_CONFIG_VAR_DEGREES(variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
#define ASSERT_(f)
class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER
#define NODE_ANNOTATION_REF_POSEID
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
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
static CSensoryFramePtr Create()
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.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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
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 BASE_IMPEXP joinThread(const TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:190
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018