Main MRPT website > C++ reference for MRPT 1.9.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-2018, 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().
13  *Several parameters can be modified
14  * through m_options.
15  *
16  * The mathematical models of this approach have been reported in:
17  * - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J.,
18  * "Towards a Unified Bayesian Approach to Hybrid Metric-Topological
19  *SLAM",
20  * in IEEE Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
21  * - ...
22  *
23  * More information in the wiki page: http://www.mrpt.org/HMT-SLAM
24  *
25  */
26 
27 #include "hmtslam-precomp.h" // Precomp header
28 
29 #include <mrpt/io/CFileStream.h>
32 #include <mrpt/system/filesystem.h>
33 #include <mrpt/io/CMemoryStream.h>
34 
35 #include <mrpt/system/os.h>
36 
37 using namespace mrpt::slam;
38 using namespace mrpt::hmtslam;
39 using namespace mrpt::obs;
40 using namespace mrpt::maps;
41 using namespace mrpt::config;
42 using namespace mrpt::opengl;
43 using namespace mrpt::serialization;
44 using namespace std;
45 
47 
48 // Initialization of static members:
49 int64_t CHMTSLAM::m_nextAreaLabel = 0;
50 TPoseID CHMTSLAM::m_nextPoseID = 0;
51 THypothesisID CHMTSLAM::m_nextHypID = COMMON_TOPOLOG_HYP + 1;
52 
53 /*---------------------------------------------------------------
54  Constructor
55  ---------------------------------------------------------------*/
56 CHMTSLAM::CHMTSLAM()
57 {
58  // Initialize data structures:
59  // ----------------------------
60  m_terminateThreads = false;
61  m_terminationFlag_LSLAM = m_terminationFlag_TBI =
62  m_terminationFlag_3D_viewer = false;
63 
64  // Create threads:
65  // -----------------------
66  m_hThread_LSLAM = std::thread(&CHMTSLAM::thread_LSLAM, this);
67  m_hThread_TBI = std::thread(&CHMTSLAM::thread_TBI, this);
68  m_hThread_3D_viewer = std::thread(&CHMTSLAM::thread_3D_viewer, this);
69 
70  // Other variables:
71  m_LSLAM_method = nullptr;
72 
73  // Register default LC detectors:
74  // --------------------------------
75  registerLoopClosureDetector(
76  "gridmaps", &CTopLCDetector_GridMatching::createNewInstance);
77  registerLoopClosureDetector(
78  "fabmap", &CTopLCDetector_FabMap::createNewInstance);
79 
80  // Prepare an empty map:
81  initializeEmptyMap();
82 }
83 
84 /*---------------------------------------------------------------
85  Destructor
86  ---------------------------------------------------------------*/
87 CHMTSLAM::~CHMTSLAM()
88 {
89  // Signal to threads that we are closing:
90  // -------------------------------------------
91  m_terminateThreads = true;
92 
93  // Wait for threads:
94  // ----------------------------------
95  MRPT_LOG_DEBUG("[CHMTSLAM::destructor] Waiting for threads end...\n");
96 
97  m_hThread_3D_viewer.join();
98  m_hThread_LSLAM.join();
99  m_hThread_TBI.join();
100 
101  MRPT_LOG_DEBUG("[CHMTSLAM::destructor] All threads finished.\n");
102 
103  // Save the resulting H.Map if logging
104  // --------------------------------------
105  if (!m_options.LOG_OUTPUT_DIR.empty())
106  {
107  try
108  {
109  /* // Update the last area(s) in the HMAP:
110  updateHierarchicalMapFromRBPF();
111 
112  // Save:
113  os::sprintf(auxFil,1000,"%s/hierarchicalMap.hmap",m_options.logOutputDirectory.c_str());
114 
115  CFileStream f(auxFil,fomWrite);
116  f << m_knownAreas;
117  */
118  }
119  catch (std::exception& e)
120  {
122  mrpt::format(
123  "Ignoring exception at ~CHMTSLAM():\n%s", e.what()));
124  }
125  catch (...)
126  {
127  MRPT_LOG_WARN("Ignoring untyped exception at ~CHMTSLAM()");
128  }
129  }
130 
131  // Delete data structures:
132  // ----------------------------------
133  clearInputQueue();
134 
135  // Others:
136  if (m_LSLAM_method)
137  {
138  delete m_LSLAM_method;
139  m_LSLAM_method = nullptr;
140  }
141 
142  // Delete TLC-detectors
143  {
144  std::lock_guard<std::mutex> lock(m_topLCdets_cs);
145 
146  // Clear old list:
147  for (std::deque<CTopLCDetectorBase*>::iterator it = m_topLCdets.begin();
148  it != m_topLCdets.end(); ++it)
149  delete *it;
150  m_topLCdets.clear();
151  }
152 }
153 
154 /*---------------------------------------------------------------
155  clearInputQueue
156  ---------------------------------------------------------------*/
157 void CHMTSLAM::clearInputQueue()
158 {
159  // Wait for critical section
160  {
161  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
162 
163  while (!m_inputQueue.empty())
164  {
165  // delete m_inputQueue.front();
166  m_inputQueue.pop();
167  };
168  }
169 }
170 
171 /*---------------------------------------------------------------
172  pushAction
173  ---------------------------------------------------------------*/
174 void CHMTSLAM::pushAction(const CActionCollection::Ptr& acts)
175 {
176  if (m_terminateThreads)
177  {
178  // Discard it:
179  // delete acts;
180  return;
181  }
182 
183  { // Wait for critical section
184  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
185  m_inputQueue.push(acts);
186  }
187 }
188 
189 /*---------------------------------------------------------------
190  pushObservations
191  ---------------------------------------------------------------*/
192 void CHMTSLAM::pushObservations(const CSensoryFrame::Ptr& sf)
193 {
194  if (m_terminateThreads)
195  {
196  // Discard it:
197  // delete sf;
198  return;
199  }
200 
201  { // Wait for critical section
202  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
203  m_inputQueue.push(sf);
204  }
205 }
206 
207 /*---------------------------------------------------------------
208  pushObservation
209  ---------------------------------------------------------------*/
210 void CHMTSLAM::pushObservation(const CObservation::Ptr& obs)
211 {
212  if (m_terminateThreads)
213  { // Discard it:
214  // delete obs;
215  return;
216  }
217 
218  // Add a CSensoryFrame with the obs:
219  CSensoryFrame::Ptr sf = mrpt::make_aligned_shared<CSensoryFrame>();
220  sf->insert(
221  obs); // memory will be freed when deleting the SF in other thread
222 
223  { // Wait for critical section
224  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
225  m_inputQueue.push(sf);
226  }
227 }
228 
229 /*---------------------------------------------------------------
230  loadOptions
231  ---------------------------------------------------------------*/
232 void CHMTSLAM::loadOptions(const mrpt::config::CConfigFileBase& cfg)
233 {
234  m_options.loadFromConfigFile(cfg, "HMT-SLAM");
235 
236  m_options.defaultMapsInitializers.loadFromConfigFile(cfg, "MetricMaps");
237 
238  m_options.pf_options.loadFromConfigFile(cfg, "PARTICLE_FILTER");
239 
240  m_options.KLD_params.loadFromConfigFile(cfg, "KLD");
241 
242  m_options.AA_options.loadFromConfigFile(cfg, "GRAPH_CUT");
243 
244  // Topological Loop Closure detector options:
245  m_options.TLC_grid_options.loadFromConfigFile(cfg, "TLC_GRIDMATCHING");
246  m_options.TLC_fabmap_options.loadFromConfigFile(cfg, "TLC_FABMAP");
247 
248  m_options.dumpToConsole();
249 }
250 
251 /*---------------------------------------------------------------
252  loadOptions
253  ---------------------------------------------------------------*/
254 void CHMTSLAM::loadOptions(const std::string& configFile)
255 {
256  ASSERT_(mrpt::system::fileExists(configFile));
257  CConfigFile cfg(configFile);
258  loadOptions(cfg);
259 }
260 
261 /*---------------------------------------------------------------
262  TOptions
263  ---------------------------------------------------------------*/
264 CHMTSLAM::TOptions::TOptions()
265 {
266  LOG_OUTPUT_DIR = "";
267  LOG_FREQUENCY = 1;
268 
269  SLAM_METHOD = lsmRBPF_2DLASER;
270 
271  SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
272  SLAM_MIN_HEADING_BETWEEN_OBS = DEG2RAD(25.0f);
273 
274  MIN_ODOMETRY_STD_XY = 0;
275  MIN_ODOMETRY_STD_PHI = 0;
276 
277  VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
278  VIEW3D_AREA_SPHERES_RADIUS = 1.0f;
279 
280  random_seed = 1234;
281 
282  TLC_detectors.clear();
283 
284  stds_Q_no_odo.resize(3);
285  stds_Q_no_odo[0] = stds_Q_no_odo[1] = 0.10f;
286  stds_Q_no_odo[2] = DEG2RAD(4.0f);
287 }
288 
289 /*---------------------------------------------------------------
290  loadFromConfigFile
291  ---------------------------------------------------------------*/
292 void CHMTSLAM::TOptions::loadFromConfigFile(
293  const mrpt::config::CConfigFileBase& source, const std::string& section)
294 {
295  MRPT_LOAD_CONFIG_VAR(LOG_OUTPUT_DIR, string, source, section);
296  MRPT_LOAD_CONFIG_VAR(LOG_FREQUENCY, int, source, section);
297 
299  SLAM_METHOD, int, TLSlamMethod, source, section);
300 
301  MRPT_LOAD_CONFIG_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float, source, section);
302  MRPT_LOAD_CONFIG_VAR_DEGREES(SLAM_MIN_HEADING_BETWEEN_OBS, source, section);
303 
304  MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY, float, source, section);
305  MRPT_LOAD_CONFIG_VAR_DEGREES(MIN_ODOMETRY_STD_PHI, source, section);
306 
307  MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_HEIGHT, float, source, section);
308  MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_RADIUS, float, source, section);
309 
310  MRPT_LOAD_CONFIG_VAR(random_seed, int, source, section);
311 
312  stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
313  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
314  ASSERT_(stds_Q_no_odo.size() == 3);
315 
316  stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);
317 
318  std::string sTLC_detectors =
319  source.read_string(section, "TLC_detectors", "", true);
320 
321  mrpt::system::tokenize(sTLC_detectors, ", ", TLC_detectors);
322 
323  std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;
324 
325  // load other sub-classes:
326  AA_options.loadFromConfigFile(source, section);
327 }
328 
329 /*---------------------------------------------------------------
330  dumpToTextStream
331  ---------------------------------------------------------------*/
332 void CHMTSLAM::TOptions::dumpToTextStream(std::ostream& out) const
333 {
334  out << mrpt::format("\n----------- [CHMTSLAM::TOptions] ------------ \n\n");
335 
336  LOADABLEOPTS_DUMP_VAR(LOG_OUTPUT_DIR, string);
337  LOADABLEOPTS_DUMP_VAR(LOG_FREQUENCY, int);
338 
339  LOADABLEOPTS_DUMP_VAR(SLAM_METHOD, int);
340 
341  LOADABLEOPTS_DUMP_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float);
342  LOADABLEOPTS_DUMP_VAR_DEG(SLAM_MIN_HEADING_BETWEEN_OBS);
343 
344  LOADABLEOPTS_DUMP_VAR(MIN_ODOMETRY_STD_XY, float);
345  LOADABLEOPTS_DUMP_VAR_DEG(MIN_ODOMETRY_STD_PHI);
346 
347  LOADABLEOPTS_DUMP_VAR(random_seed, int);
348 
349  AA_options.dumpToTextStream(out);
350  pf_options.dumpToTextStream(out);
351  KLD_params.dumpToTextStream(out);
352  defaultMapsInitializers.dumpToTextStream(out);
353  TLC_grid_options.dumpToTextStream(out);
354  TLC_fabmap_options.dumpToTextStream(out);
355 }
356 
357 /*---------------------------------------------------------------
358  isInputQueueEmpty
359  ---------------------------------------------------------------*/
360 bool CHMTSLAM::isInputQueueEmpty()
361 {
362  bool res;
363 
364  { // Wait for critical section
365  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
366  res = m_inputQueue.empty();
367  }
368  return res;
369 }
370 
371 /*---------------------------------------------------------------
372  inputQueueSize
373  ---------------------------------------------------------------*/
374 size_t CHMTSLAM::inputQueueSize()
375 {
376  size_t res;
377  { // Wait for critical section
378  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
379  res = m_inputQueue.size();
380  }
381  return res;
382 }
383 
384 /*---------------------------------------------------------------
385  getNextObjectFromInputQueue
386  ---------------------------------------------------------------*/
387 CSerializable::Ptr CHMTSLAM::getNextObjectFromInputQueue()
388 {
390 
391  { // Wait for critical section
392  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
393  if (!m_inputQueue.empty())
394  {
395  obj = m_inputQueue.front();
396  m_inputQueue.pop();
397  }
398  }
399  return obj;
400 }
401 
402 /*---------------------------------------------------------------
403  initializeEmptyMap
404  ---------------------------------------------------------------*/
405 void CHMTSLAM::initializeEmptyMap()
406 {
407  THypothesisIDSet LMH_hyps;
408  THypothesisID newHypothID = generateHypothesisID();
409 
410  LMH_hyps.insert(COMMON_TOPOLOG_HYP);
411  LMH_hyps.insert(newHypothID);
412 
413  // ------------------------------------
414  // CLEAR HIERARCHICAL MAP
415  // ------------------------------------
416  CHMHMapNode::TNodeID firstAreaID;
417  {
418  std::lock_guard<std::mutex> lock(m_map_cs);
419 
420  // Initialize hierarchical structures:
421  // -----------------------------------------------------
422  m_map.clear();
423 
424  // Create a single node for the starting area:
425  CHMHMapNode::Ptr firstArea =
426  mrpt::make_aligned_shared<CHMHMapNode>(&m_map);
427  firstAreaID = firstArea->getID();
428 
429  firstArea->m_hypotheses = LMH_hyps;
431  new CMultiMetricMap(&m_options.defaultMapsInitializers));
432 
433  firstArea->m_nodeType = "Area";
434  firstArea->m_label = generateUniqueAreaLabel();
435  firstArea->m_annotations.set(
436  NODE_ANNOTATION_METRIC_MAPS, emptyMap, newHypothID);
437  firstArea->m_annotations.setElemental(
439  } // end of lock m_map_cs
440 
441  // ------------------------------------
442  // CLEAR LIST OF HYPOTHESES
443  // ------------------------------------
444  {
445  std::lock_guard<std::mutex> lock(m_LMHs_cs);
446 
447  // Add to the list:
448  m_LMHs.clear();
449  CLocalMetricHypothesis& newLMH = m_LMHs[newHypothID];
450  newLMH.m_parent = this;
451 
452  newLMH.m_currentRobotPose =
453  POSEID_INVALID; // Special case: map is empty
454  newLMH.m_log_w = 0;
455  newLMH.m_ID = newHypothID;
456 
457  newLMH.m_neighbors.clear();
458  newLMH.m_neighbors.insert(firstAreaID);
459 
460  newLMH.clearRobotPoses();
461  } // end of cs
462 
463  // ------------------------------------------
464  // Create the local SLAM algorithm object
465  // -----------------------------------------
466  switch (m_options.SLAM_METHOD)
467  {
468  case lsmRBPF_2DLASER:
469  {
470  if (m_LSLAM_method) delete m_LSLAM_method;
471  m_LSLAM_method = new CLSLAM_RBPF_2DLASER(this);
472  }
473  break;
474  default:
476  "Invalid selection for LSLAM method: %i",
477  (int)m_options.SLAM_METHOD);
478  };
479 
480  // ------------------------------------
481  // Topological LC detectors:
482  // ------------------------------------
483  {
484  std::lock_guard<std::mutex> lock(m_topLCdets_cs);
485 
486  // Clear old list:
487  for (std::deque<CTopLCDetectorBase*>::iterator it = m_topLCdets.begin();
488  it != m_topLCdets.end(); ++it)
489  delete *it;
490  m_topLCdets.clear();
491 
492  // Create new list:
493  // 1: Occupancy Grid matching.
494  // 2: Cummins' image matching.
496  m_options.TLC_detectors.begin();
497  d != m_options.TLC_detectors.end(); ++d)
498  m_topLCdets.push_back(loopClosureDetector_factory(*d));
499  }
500 
501  // ------------------------------------
502  // Other variables:
503  // ------------------------------------
504  m_LSLAM_queue.clear();
505 
506  // ------------------------------------
507  // Delete log files:
508  // ------------------------------------
509  if (!m_options.LOG_OUTPUT_DIR.empty())
510  {
511  mrpt::system::deleteFilesInDirectory(m_options.LOG_OUTPUT_DIR);
512  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR);
513  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_txt");
514  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_3D");
515  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/LSLAM_3D");
516  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/ASSO");
518  m_options.LOG_OUTPUT_DIR + "/HMTSLAM_state");
519  }
520 }
521 
522 /*---------------------------------------------------------------
523  generateUniqueAreaLabel
524  ---------------------------------------------------------------*/
525 std::string CHMTSLAM::generateUniqueAreaLabel()
526 {
527  return format("%li", (long int)(m_nextAreaLabel++));
528 }
529 
530 /*---------------------------------------------------------------
531  generatePoseID
532  ---------------------------------------------------------------*/
533 TPoseID CHMTSLAM::generatePoseID() { return m_nextPoseID++; }
534 /*---------------------------------------------------------------
535  generateHypothesisID
536  ---------------------------------------------------------------*/
537 THypothesisID CHMTSLAM::generateHypothesisID() { return m_nextHypID++; }
538 /*---------------------------------------------------------------
539  getAs3DScene
540  ---------------------------------------------------------------*/
541 void CHMTSLAM::getAs3DScene(COpenGLScene& scene3D)
542 {
543  MRPT_UNUSED_PARAM(scene3D);
544 }
545 
546 /*---------------------------------------------------------------
547  abortedDueToErrors
548  ---------------------------------------------------------------*/
549 bool CHMTSLAM::abortedDueToErrors()
550 {
551  return m_terminationFlag_LSLAM || m_terminationFlag_TBI ||
552  m_terminationFlag_3D_viewer;
553 }
554 
555 /*---------------------------------------------------------------
556  registerLoopClosureDetector
557  ---------------------------------------------------------------*/
558 void CHMTSLAM::registerLoopClosureDetector(
559  const std::string& name, CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*))
560 {
561  m_registeredLCDetectors[name] = ptrCreateObject;
562 }
563 
564 /*---------------------------------------------------------------
565  loopClosureDetector_factory
566  ---------------------------------------------------------------*/
567 CTopLCDetectorBase* CHMTSLAM::loopClosureDetector_factory(
568  const std::string& name)
569 {
570  MRPT_START
572  m_registeredLCDetectors.find(name);
573  if (it == m_registeredLCDetectors.end())
575  "Invalid value for TLC_detectors: %s", name.c_str());
576  return it->second(this);
577  MRPT_END
578 }
579 
580 bool CHMTSLAM::saveState(CArchive& out) const
581 {
582  try
583  {
584  out << *this;
585  return true;
586  }
587  catch (...)
588  {
589  return false;
590  }
591 }
592 
593 bool CHMTSLAM::loadState(CArchive& in)
594 {
595  try
596  {
597  in >> *this;
598  return true;
599  }
600  catch (...)
601  {
602  return false;
603  }
604 }
605 
606 /*---------------------------------------------------------------
607  readFromStream
608  ---------------------------------------------------------------*/
609 void CHMTSLAM::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
610 {
611  switch (version)
612  {
613  case 0:
614  {
615  // Acquire all critical sections before!
616  // -------------------------------------------
617  std::lock_guard<std::mutex> lock_map(m_map_cs);
618 
619  // Data:
620  in >> m_nextAreaLabel >> m_nextPoseID >> m_nextHypID;
621  // The HMT-MAP:
622  in >> m_map;
623  // The LMHs:
624  in >> m_LMHs;
625  // Save options??? Better allow changing them...
626  }
627  break;
628  default:
630  };
631 }
632 
633 uint8_t CHMTSLAM::serializeGetVersion() const { return 0; }
634 void CHMTSLAM::serializeTo(mrpt::serialization::CArchive& out) const
635 {
636  // Acquire all critical sections before!
637  // -------------------------------------------
638  std::lock_guard<std::mutex> lock_map(m_map_cs);
639 
640  // Data:
641  out << m_nextAreaLabel << m_nextPoseID << m_nextHypID;
642  // The HMT-MAP:
643  out << m_map;
644  // The LMHs:
645  out << m_LMHs;
646  // Save options??? Better allow changing them...
647 }
Scalar * iterator
Definition: eigen_plugins.h:26
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:158
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:567
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
double RAD2DEG(const double x)
Radians to degrees.
#define COMMON_TOPOLOG_HYP
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:47
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:127
double m_log_w
Log-weight of this hypothesis.
STL namespace.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
A set of hypothesis IDs, used for arcs and nodes in multi-hypothesis hybrid maps. ...
#define POSEID_INVALID
__int64 int64_t
Definition: rptypes.h:49
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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 NODE_ANNOTATION_METRIC_MAPS
GLsizei const GLchar ** string
Definition: glext.h:4101
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#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...
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
#define MRPT_END
Definition: exceptions.h:266
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
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:4054
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
#define NODE_ANNOTATION_REF_POSEID
bool 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:215
#define MRPT_LOG_WARN(_STRING)
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
GLuint res
Definition: glext.h:7268
This class stores any customizable set of metric maps.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
const Scalar * const_iterator
Definition: eigen_plugins.h:27
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself)...
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT( variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019