MRPT  2.0.4
CHMTSLAM_main.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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: https://www.mrpt.org/HMT-SLAM
24  *
25  */
26 
27 #include "hmtslam-precomp.h" // Precomp header
28 
30 #include <mrpt/io/CFileStream.h>
31 #include <mrpt/io/CMemoryStream.h>
33 #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::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 (const std::exception& e)
120  {
122  "Ignoring exception at ~CHMTSLAM():\n%s", e.what()));
123  }
124  catch (...)
125  {
126  MRPT_LOG_WARN("Ignoring untyped exception at ~CHMTSLAM()");
127  }
128  }
129 
130  // Delete data structures:
131  // ----------------------------------
132  clearInputQueue();
133 
134  // Others:
135  if (m_LSLAM_method)
136  {
137  delete m_LSLAM_method;
138  m_LSLAM_method = nullptr;
139  }
140 
141  // Delete TLC-detectors
142  {
143  std::lock_guard<std::mutex> lock(m_topLCdets_cs);
144 
145  // Clear old list:
146  for (auto& m_topLCdet : m_topLCdets) delete m_topLCdet;
147  m_topLCdets.clear();
148  }
149 }
150 
151 /*---------------------------------------------------------------
152  clearInputQueue
153  ---------------------------------------------------------------*/
154 void CHMTSLAM::clearInputQueue()
155 {
156  // Wait for critical section
157  {
158  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
159 
160  while (!m_inputQueue.empty())
161  {
162  // delete m_inputQueue.front();
163  m_inputQueue.pop();
164  };
165  }
166 }
167 
168 /*---------------------------------------------------------------
169  pushAction
170  ---------------------------------------------------------------*/
171 void CHMTSLAM::pushAction(const CActionCollection::Ptr& acts)
172 {
173  if (m_terminateThreads)
174  {
175  // Discard it:
176  // delete acts;
177  return;
178  }
179 
180  { // Wait for critical section
181  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
182  m_inputQueue.push(acts);
183  }
184 }
185 
186 /*---------------------------------------------------------------
187  pushObservations
188  ---------------------------------------------------------------*/
189 void CHMTSLAM::pushObservations(const CSensoryFrame::Ptr& sf)
190 {
191  if (m_terminateThreads)
192  {
193  // Discard it:
194  // delete sf;
195  return;
196  }
197 
198  { // Wait for critical section
199  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
200  m_inputQueue.push(sf);
201  }
202 }
203 
204 /*---------------------------------------------------------------
205  pushObservation
206  ---------------------------------------------------------------*/
207 void CHMTSLAM::pushObservation(const CObservation::Ptr& obs)
208 {
209  if (m_terminateThreads)
210  { // Discard it:
211  // delete obs;
212  return;
213  }
214 
215  // Add a CSensoryFrame with the obs:
216  CSensoryFrame::Ptr sf = std::make_shared<CSensoryFrame>();
217  sf->insert(
218  obs); // memory will be freed when deleting the SF in other thread
219 
220  { // Wait for critical section
221  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
222  m_inputQueue.push(sf);
223  }
224 }
225 
226 /*---------------------------------------------------------------
227  loadOptions
228  ---------------------------------------------------------------*/
229 void CHMTSLAM::loadOptions(const mrpt::config::CConfigFileBase& cfg)
230 {
231  m_options.loadFromConfigFile(cfg, "HMT-SLAM");
232 
233  m_options.defaultMapsInitializers.loadFromConfigFile(cfg, "MetricMaps");
234 
235  m_options.pf_options.loadFromConfigFile(cfg, "PARTICLE_FILTER");
236 
237  m_options.KLD_params.loadFromConfigFile(cfg, "KLD");
238 
239  m_options.AA_options.loadFromConfigFile(cfg, "GRAPH_CUT");
240 
241  // Topological Loop Closure detector options:
242  m_options.TLC_grid_options.loadFromConfigFile(cfg, "TLC_GRIDMATCHING");
243  m_options.TLC_fabmap_options.loadFromConfigFile(cfg, "TLC_FABMAP");
244 
245  m_options.dumpToConsole();
246 }
247 
248 /*---------------------------------------------------------------
249  loadOptions
250  ---------------------------------------------------------------*/
251 void CHMTSLAM::loadOptions(const std::string& configFile)
252 {
253  ASSERT_(mrpt::system::fileExists(configFile));
254  CConfigFile cfg(configFile);
255  loadOptions(cfg);
256 }
257 
258 /*---------------------------------------------------------------
259  TOptions
260  ---------------------------------------------------------------*/
261 CHMTSLAM::TOptions::TOptions()
262 {
263  LOG_OUTPUT_DIR = "";
264  LOG_FREQUENCY = 1;
265 
266  SLAM_METHOD = lsmRBPF_2DLASER;
267 
268  SLAM_MIN_DIST_BETWEEN_OBS = 1.0f;
269  SLAM_MIN_HEADING_BETWEEN_OBS = DEG2RAD(25.0f);
270 
271  MIN_ODOMETRY_STD_XY = 0;
272  MIN_ODOMETRY_STD_PHI = 0;
273 
274  VIEW3D_AREA_SPHERES_HEIGHT = 10.0f;
275  VIEW3D_AREA_SPHERES_RADIUS = 1.0f;
276 
277  random_seed = 1234;
278 
279  TLC_detectors.clear();
280 
281  stds_Q_no_odo.resize(3);
282  stds_Q_no_odo[0] = stds_Q_no_odo[1] = 0.10f;
283  stds_Q_no_odo[2] = DEG2RAD(4.0f);
284 }
285 
286 /*---------------------------------------------------------------
287  loadFromConfigFile
288  ---------------------------------------------------------------*/
289 void CHMTSLAM::TOptions::loadFromConfigFile(
290  const mrpt::config::CConfigFileBase& source, const std::string& section)
291 {
292  MRPT_LOAD_CONFIG_VAR(LOG_OUTPUT_DIR, string, source, section);
293  MRPT_LOAD_CONFIG_VAR(LOG_FREQUENCY, int, source, section);
294 
296  SLAM_METHOD, int, TLSlamMethod, source, section);
297 
298  MRPT_LOAD_CONFIG_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float, source, section);
299  MRPT_LOAD_CONFIG_VAR_DEGREES(SLAM_MIN_HEADING_BETWEEN_OBS, source, section);
300 
301  MRPT_LOAD_CONFIG_VAR(MIN_ODOMETRY_STD_XY, float, source, section);
302  MRPT_LOAD_CONFIG_VAR_DEGREES(MIN_ODOMETRY_STD_PHI, source, section);
303 
304  MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_HEIGHT, float, source, section);
305  MRPT_LOAD_CONFIG_VAR(VIEW3D_AREA_SPHERES_RADIUS, float, source, section);
306 
307  MRPT_LOAD_CONFIG_VAR(random_seed, int, source, section);
308 
309  stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
310  source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
311  ASSERT_(stds_Q_no_odo.size() == 3);
312 
313  stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);
314 
315  std::string sTLC_detectors =
316  source.read_string(section, "TLC_detectors", "", true);
317 
318  mrpt::system::tokenize(sTLC_detectors, ", ", TLC_detectors);
319 
320  std::cout << "TLC_detectors: " << TLC_detectors.size() << std::endl;
321 
322  // load other sub-classes:
323  AA_options.loadFromConfigFile(source, section);
324 }
325 
326 /*---------------------------------------------------------------
327  dumpToTextStream
328  ---------------------------------------------------------------*/
329 void CHMTSLAM::TOptions::dumpToTextStream(std::ostream& out) const
330 {
331  out << "\n----------- [CHMTSLAM::TOptions] ------------ \n\n";
332 
333  LOADABLEOPTS_DUMP_VAR(LOG_OUTPUT_DIR, string);
334  LOADABLEOPTS_DUMP_VAR(LOG_FREQUENCY, int);
335 
336  LOADABLEOPTS_DUMP_VAR(SLAM_METHOD, int);
337 
338  LOADABLEOPTS_DUMP_VAR(SLAM_MIN_DIST_BETWEEN_OBS, float);
339  LOADABLEOPTS_DUMP_VAR_DEG(SLAM_MIN_HEADING_BETWEEN_OBS);
340 
341  LOADABLEOPTS_DUMP_VAR(MIN_ODOMETRY_STD_XY, float);
342  LOADABLEOPTS_DUMP_VAR_DEG(MIN_ODOMETRY_STD_PHI);
343 
344  LOADABLEOPTS_DUMP_VAR(random_seed, int);
345 
346  AA_options.dumpToTextStream(out);
347  pf_options.dumpToTextStream(out);
348  KLD_params.dumpToTextStream(out);
349  defaultMapsInitializers.dumpToTextStream(out);
350  TLC_grid_options.dumpToTextStream(out);
351  TLC_fabmap_options.dumpToTextStream(out);
352 }
353 
354 /*---------------------------------------------------------------
355  isInputQueueEmpty
356  ---------------------------------------------------------------*/
357 bool CHMTSLAM::isInputQueueEmpty()
358 {
359  bool res;
360 
361  { // Wait for critical section
362  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
363  res = m_inputQueue.empty();
364  }
365  return res;
366 }
367 
368 /*---------------------------------------------------------------
369  inputQueueSize
370  ---------------------------------------------------------------*/
371 size_t CHMTSLAM::inputQueueSize()
372 {
373  size_t res;
374  { // Wait for critical section
375  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
376  res = m_inputQueue.size();
377  }
378  return res;
379 }
380 
381 /*---------------------------------------------------------------
382  getNextObjectFromInputQueue
383  ---------------------------------------------------------------*/
384 CSerializable::Ptr CHMTSLAM::getNextObjectFromInputQueue()
385 {
386  CSerializable::Ptr obj;
387 
388  { // Wait for critical section
389  std::lock_guard<std::mutex> lock(m_inputQueue_cs);
390  if (!m_inputQueue.empty())
391  {
392  obj = m_inputQueue.front();
393  m_inputQueue.pop();
394  }
395  }
396  return obj;
397 }
398 
399 /*---------------------------------------------------------------
400  initializeEmptyMap
401  ---------------------------------------------------------------*/
402 void CHMTSLAM::initializeEmptyMap()
403 {
404  THypothesisIDSet LMH_hyps;
405  THypothesisID newHypothID = generateHypothesisID();
406 
407  LMH_hyps.insert(COMMON_TOPOLOG_HYP);
408  LMH_hyps.insert(newHypothID);
409 
410  // ------------------------------------
411  // CLEAR HIERARCHICAL MAP
412  // ------------------------------------
413  CHMHMapNode::TNodeID firstAreaID;
414  {
415  std::lock_guard<std::mutex> lock(m_map_cs);
416 
417  // Initialize hierarchical structures:
418  // -----------------------------------------------------
419  m_map.clear();
420 
421  // Create a single node for the starting area:
422  CHMHMapNode::Ptr firstArea = std::make_shared<CHMHMapNode>(&m_map);
423  firstAreaID = firstArea->getID();
424 
425  firstArea->m_hypotheses = LMH_hyps;
426  CMultiMetricMap::Ptr emptyMap =
427  CMultiMetricMap::Create(m_options.defaultMapsInitializers);
428 
429  firstArea->m_nodeType = "Area";
430  firstArea->m_label = generateUniqueAreaLabel();
431  firstArea->m_annotations.set(
432  NODE_ANNOTATION_METRIC_MAPS, emptyMap, newHypothID);
433  firstArea->m_annotations.setElemental(
435  } // end of lock m_map_cs
436 
437  // ------------------------------------
438  // CLEAR LIST OF HYPOTHESES
439  // ------------------------------------
440  {
441  std::lock_guard<std::mutex> lock(m_LMHs_cs);
442 
443  // Add to the list:
444  m_LMHs.clear();
445  CLocalMetricHypothesis& newLMH = m_LMHs[newHypothID];
446  newLMH.m_parent = this;
447 
448  newLMH.m_currentRobotPose =
449  POSEID_INVALID; // Special case: map is empty
450  newLMH.m_log_w = 0;
451  newLMH.m_ID = newHypothID;
452 
453  newLMH.m_neighbors.clear();
454  newLMH.m_neighbors.insert(firstAreaID);
455 
456  newLMH.clearRobotPoses();
457  } // end of cs
458 
459  // ------------------------------------------
460  // Create the local SLAM algorithm object
461  // -----------------------------------------
462  switch (m_options.SLAM_METHOD)
463  {
464  case lsmRBPF_2DLASER:
465  {
466  if (m_LSLAM_method) delete m_LSLAM_method;
467  m_LSLAM_method = new CLSLAM_RBPF_2DLASER(this);
468  }
469  break;
470  default:
472  "Invalid selection for LSLAM method: %i",
473  (int)m_options.SLAM_METHOD);
474  };
475 
476  // ------------------------------------
477  // Topological LC detectors:
478  // ------------------------------------
479  {
480  std::lock_guard<std::mutex> lock(m_topLCdets_cs);
481 
482  // Clear old list:
483  for (auto& m_topLCdet : m_topLCdets) delete m_topLCdet;
484  m_topLCdets.clear();
485 
486  // Create new list:
487  // 1: Occupancy Grid matching.
488  // 2: Cummins' image matching.
489  for (auto d = m_options.TLC_detectors.begin();
490  d != m_options.TLC_detectors.end(); ++d)
491  m_topLCdets.push_back(loopClosureDetector_factory(*d));
492  }
493 
494  // ------------------------------------
495  // Other variables:
496  // ------------------------------------
497  m_LSLAM_queue.clear();
498 
499  // ------------------------------------
500  // Delete log files:
501  // ------------------------------------
502  if (!m_options.LOG_OUTPUT_DIR.empty())
503  {
504  mrpt::system::deleteFilesInDirectory(m_options.LOG_OUTPUT_DIR);
505  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR);
506  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_txt");
507  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/HMAP_3D");
508  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/LSLAM_3D");
509  mrpt::system::createDirectory(m_options.LOG_OUTPUT_DIR + "/ASSO");
511  m_options.LOG_OUTPUT_DIR + "/HMTSLAM_state");
512  }
513 }
514 
515 /*---------------------------------------------------------------
516  generateUniqueAreaLabel
517  ---------------------------------------------------------------*/
518 std::string CHMTSLAM::generateUniqueAreaLabel()
519 {
520  return format("%li", (long int)(m_nextAreaLabel++));
521 }
522 
523 /*---------------------------------------------------------------
524  generatePoseID
525  ---------------------------------------------------------------*/
526 TPoseID CHMTSLAM::generatePoseID() { return m_nextPoseID++; }
527 /*---------------------------------------------------------------
528  generateHypothesisID
529  ---------------------------------------------------------------*/
530 THypothesisID CHMTSLAM::generateHypothesisID() { return m_nextHypID++; }
531 /*---------------------------------------------------------------
532  getAs3DScene
533  ---------------------------------------------------------------*/
534 void CHMTSLAM::getAs3DScene([[maybe_unused]] COpenGLScene& scene3D) {}
535 
536 /*---------------------------------------------------------------
537  abortedDueToErrors
538  ---------------------------------------------------------------*/
539 bool CHMTSLAM::abortedDueToErrors()
540 {
541  return m_terminationFlag_LSLAM || m_terminationFlag_TBI ||
542  m_terminationFlag_3D_viewer;
543 }
544 
545 /*---------------------------------------------------------------
546  registerLoopClosureDetector
547  ---------------------------------------------------------------*/
548 void CHMTSLAM::registerLoopClosureDetector(
549  const std::string& name, CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*))
550 {
551  m_registeredLCDetectors[name] = ptrCreateObject;
552 }
553 
554 /*---------------------------------------------------------------
555  loopClosureDetector_factory
556  ---------------------------------------------------------------*/
557 CTopLCDetectorBase* CHMTSLAM::loopClosureDetector_factory(
558  const std::string& name)
559 {
560  MRPT_START
561  auto it = m_registeredLCDetectors.find(name);
562  if (it == m_registeredLCDetectors.end())
564  "Invalid value for TLC_detectors: %s", name.c_str());
565  return it->second(this);
566  MRPT_END
567 }
568 
569 bool CHMTSLAM::saveState(CArchive& out) const
570 {
571  try
572  {
573  out << *this;
574  return true;
575  }
576  catch (...)
577  {
578  return false;
579  }
580 }
581 
582 bool CHMTSLAM::loadState(CArchive& in)
583 {
584  try
585  {
586  in >> *this;
587  return true;
588  }
589  catch (...)
590  {
591  return false;
592  }
593 }
594 
595 /*---------------------------------------------------------------
596  readFromStream
597  ---------------------------------------------------------------*/
598 void CHMTSLAM::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
599 {
600  switch (version)
601  {
602  case 0:
603  {
604  // Acquire all critical sections before!
605  // -------------------------------------------
606  std::lock_guard<std::mutex> lock_map(m_map_cs);
607 
608  // Data:
609  in >> m_nextAreaLabel >> m_nextPoseID >> m_nextHypID;
610  // The HMT-MAP:
611  in >> m_map;
612  // The LMHs:
613  in >> m_LMHs;
614  // Save options??? Better allow changing them...
615  }
616  break;
617  default:
619  };
620 }
621 
622 uint8_t CHMTSLAM::serializeGetVersion() const { return 0; }
623 void CHMTSLAM::serializeTo(mrpt::serialization::CArchive& out) const
624 {
625  // Acquire all critical sections before!
626  // -------------------------------------------
627  std::lock_guard<std::mutex> lock_map(m_map_cs);
628 
629  // Data:
630  out << m_nextAreaLabel << m_nextPoseID << m_nextHypID;
631  // The HMT-MAP:
632  out << m_map;
633  // The LMHs:
634  out << m_LMHs;
635  // Save options??? Better allow changing them...
636 }
bool createDirectory(const std::string &dirName)
Creates a directory.
Definition: filesystem.cpp:161
#define MRPT_START
Definition: exceptions.h:241
#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:566
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
#define COMMON_TOPOLOG_HYP
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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:128
double m_log_w
Log-weight of this hypothesis.
STL namespace.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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
constexpr double DEG2RAD(const double x)
Degrees to radians.
This namespace contains representation of robot actions and observations.
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
#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:54
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
mrpt::vision::TStereoCalibResults out
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
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:56
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
#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:218
#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.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
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)
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 ...
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020