MRPT  2.0.4
CHMTSLAM.h
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 #pragma once
10 
13 
19 #include <mrpt/maps/CPointsMap.h>
23 #include <mrpt/slam/CICP.h>
24 #include <mrpt/slam/TKLDParams.h>
25 #include <map>
26 
27 #include <queue>
28 #include <thread>
29 
30 namespace mrpt
31 {
32 /** Classes related to the implementation of Hybrid Metric Topological (HMT)
33  * SLAM. \ingroup mrpt_hmtslam_grp */
34 namespace hmtslam
35 {
36 class CHMTSLAM;
37 class CLSLAMAlgorithmBase;
38 class CLSLAM_RBPF_2DLASER;
39 
40 /** An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
41  *
42  * The main entry points for a user are pushAction() and pushObservations().
43  *Several parameters can be modified
44  * through m_options.
45  *
46  * The mathematical models of this approach have been reported in:
47  * - Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J., "Towards a
48  *Unified Bayesian Approach to Hybrid Metric-Topological SLAM", in IEEE
49  *Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
50  * - ...
51  *
52  * More information in the wiki page: https://www.mrpt.org/HMT-SLAM . A
53  *complete working application can be found in "MRPT/apps/hmt-slam".
54  *
55  * The complete state of the SLAM framework is serializable, so it can be saved
56  *and restore to/from a binary dump. This class implements
57  *mrpt::serialization::CSerializable, so
58  * it can be saved with "stream << slam_object;" and restored with "stream >>
59  *slam_object;". Alternatively, the methods CHMTSLAM::saveState and
60  *CHMTSLAM::loadState
61  * can be invoked, which in turn call internally to the CSerializable
62  *interface.
63  *
64  * \sa CHierarchicalMHMap
65  * \ingroup mrpt_hmtslam_grp
66  */
69 {
70  friend class CLocalMetricHypothesis;
71  friend class CLSLAM_RBPF_2DLASER;
73  friend class CTopLCDetector_FabMap;
74 
76 
77  protected:
78  /** @name Inter-thread communication queues:
79  @{ */
80  /** Message definition:
81  - From: LSLAM
82  - To: AA
83  - Meaning: Reconsider the graph partition for the given LMH. Compute SSO
84  for the new node(s) "newPoseIDs".
85  */
86  /*struct TMessageAA
87  {
88  CLocalMetricHypothesis *LMH;
89  TPoseIDList newPoseIDs;
90  };*/
91 
92  /** Message definition:
93  - From: AA
94  - To: LSLAM
95  - Meaning: The partitioning of robot poses.
96  */
98  {
99  using Ptr = std::shared_ptr<TMessageLSLAMfromAA>;
100  /** The hypothesis ID (LMH) for these results. */
102  std::vector<TPoseIDList> partitions;
103 
104  /** for debugging only */
105  void dumpToConsole() const;
106  };
107 
108  /** Message definition:
109  - From: LSLAM
110  - To: TBI
111  - Meaning: One or more areas are to be considered by the TBI engines.
112  */
114  {
115  using Ptr = std::shared_ptr<TMessageLSLAMtoTBI>;
116  /** The LMH */
118  /** The areas to consider. */
120  };
121 
122  /** Message definition:
123  - From: TBI
124  - To: LSLAM
125  - Meaning:
126  */
128  {
129  using Ptr = std::shared_ptr<TMessageLSLAMfromTBI>;
130  /** The hypothesis ID (LMH) for these results. */
132 
133  /** The area for who the loop-closure has been computed. */
135 
136  struct TBI_info
137  {
138  TBI_info() = default;
139  /** Log likelihood for this loop-closure. */
140  double log_lik{0};
141 
142  /** Depending on the loop-closure engine, an guess of the relative
143  * pose of "area_new" relative to "cur_area" is given here.
144  * If the SOG contains 0 modes, then the engine does not provide
145  * this information.
146  */
148  };
149 
150  /** The meat is here: only feasible loop closures from "cur_area" are
151  * included here, with associated data.
152  */
153  std::map<CHMHMapNode::TNodeID, TBI_info> loopClosureData;
154 
155  //
156  };
157 
158  /** LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA
159  */
162 
164 
165  /** @} */
166 
167  /** The Area Abstraction (AA) method, invoked from LSLAM.
168  * \param LMH (IN) The LMH which to this query applies.
169  * \param newPoseIDs (IN) The new poseIDs to be added to the graph
170  * partitioner.
171  * \return A structure with all return data. Memory to be freed by user.
172  * \note The critical section for LMH must be locked BEFORE calling this
173  * method (it does NOT lock any critical section).
174  */
176  CLocalMetricHypothesis* LMH, const TPoseIDList& newPoseIDs);
177 
178  /** The entry point for Topological Bayesian Inference (TBI) engines,
179  * invoked from LSLAM.
180  * \param LMH (IN) The LMH which to this query applies.
181  * \param areaID (IN) The area ID to consider for potential loop-closures.
182  * \note The critical section for LMH must be locked BEFORE calling this
183  * method (it does NOT lock any critical section).
184  */
186  CLocalMetricHypothesis* LMH, const CHMHMapNode::TNodeID& areaID);
187 
188  /** @name Related to the input queue:
189  @{ */
190  public:
191  /** Empty the input queue. */
192  void clearInputQueue();
193 
194  /** Returns true if the input queue is empty (Note that the queue must not
195  * be empty to the user to enqueue more actions/observaitions)
196  * \sa pushAction,pushObservations, inputQueueSize
197  */
198  bool isInputQueueEmpty();
199 
200  /** Returns the number of objects waiting for processing in the input queue.
201  * \sa pushAction,pushObservations, isInputQueueEmpty
202  */
203  size_t inputQueueSize();
204 
205  /** Here the user can enter an action into the system (will go to the SLAM
206  * process).
207  * This class will delete the passed object when required, so DO NOT
208  * DELETE the passed object after calling this.
209  * \sa pushObservations,pushObservation
210  */
212 
213  /** Here the user can enter observations into the system (will go to the
214  * SLAM process).
215  * This class will delete the passed object when required, so DO NOT
216  * DELETE the passed object after calling this.
217  * \sa pushAction,pushObservation
218  */
220 
221  /** Here the user can enter an observation into the system (will go to the
222  * SLAM process).
223  * This class will delete the passed object when required, so DO NOT
224  * DELETE the passed object after calling this.
225  * \sa pushAction,pushObservation
226  */
228 
230  {
231  lsmRBPF_2DLASER = 1
232  };
233 
234  protected:
235  /** Used from the LSLAM thread to retrieve the next object from the queue.
236  * \return The object, or nullptr if empty.
237  */
239 
240  /** The queue of pending actions/observations supplied by the user waiting
241  * for being processed. */
242  std::queue<mrpt::serialization::CSerializable::Ptr> m_inputQueue;
243 
244  /** Critical section for accessing m_inputQueue */
245  mutable std::mutex m_inputQueue_cs;
246 
247  /** Critical section for accessing m_map */
248  mutable std::mutex m_map_cs;
249 
250  /** Critical section for accessing m_LMHs */
251  mutable std::mutex m_LMHs_cs;
252 
253  /** @} */
254 
255  /** @name Threads stuff
256  @{ */
257 
258  /** The function for the "Local SLAM" thread. */
259  void thread_LSLAM();
260 
261  /** The function for the "TBI" thread. */
262  void thread_TBI();
263 
264  /** The function for the "3D viewer" thread. */
265  void thread_3D_viewer();
266  /** Threads handles */
268  /** @} */
269 
270  /** @name HMT-SLAM sub-processes.
271  @{ */
272  /** Auxiliary method within thread_LSLAM */
274 
275  /** No critical section locks are assumed at the entrance of this method.
276  */
278 
279  /** No critical section locks are assumed at the entrance of this method.
280  */
282 
283  /** Topological Loop Closure: Performs all the required operations
284  to close a loop between two areas which have been determined
285  to be the same.
286  */
287  void perform_TLC(
288  CLocalMetricHypothesis& LMH, const CHMHMapNode::TNodeID areaInLMH,
289  const CHMHMapNode::TNodeID areaLoopClosure,
290  const mrpt::poses::CPose3DPDFGaussian& pose1wrt2);
291 
292  /** @} */
293 
294  /** @name The different SLAM algorithms that can be invoked from the LSLAM
295  thread.
296  @{ */
297 
298  /** An instance of a local SLAM method, to be applied to each LMH -
299  * initialized by "initializeEmptyMap" or "loadState".
300  */
302 
303  /** @} */
304 
305  /** @name The different Loop-Closure modules that are to be executed in the
306  TBI thread.
307  @{ */
308  protected:
310 
311  std::map<std::string, TLopLCDetectorFactory> m_registeredLCDetectors;
312 
313  /** The list of LC modules in operation - initialized by
314  * "initializeEmptyMap" or "loadState". */
315  std::deque<CTopLCDetectorBase*> m_topLCdets;
316 
317  /** The critical section for accessing m_topLCdets */
318  std::mutex m_topLCdets_cs;
319 
320  public:
321  /** Must be invoked before calling initializeEmptyMap, so LC objects can be
322  * created. */
324  const std::string& name,
325  CTopLCDetectorBase* (*ptrCreateObject)(CHMTSLAM*));
326 
327  /** The class factory for topological loop closure detectors.
328  * Possible values are enumerated in TOptions::TLC_detectors
329  *
330  * \exception std::exception On unknown name.
331  */
332  CTopLCDetectorBase* loopClosureDetector_factory(const std::string& name);
333 
334  /** @} */
335  protected:
336  /** Termination flag for signaling all threads to terminate */
337  std::atomic_bool m_terminateThreads;
338 
339  /** Threads termination flags:
340  */
343 
344  /** Generates a new and unique area textual label (currently this generates
345  * "0","1",...) */
346  static std::string generateUniqueAreaLabel();
347 
348  /** Generates a new and unique pose ID */
349  static TPoseID generatePoseID();
350 
351  /** Generates a new and unique hypothesis ID */
353 
354  static int64_t m_nextAreaLabel;
357 
358  public:
359  /** Default constructor
360  * \param debug_out_stream If debug output messages should be redirected
361  * to any other stream apart from std::cout
362  */
363  CHMTSLAM();
364 
365  CHMTSLAM(const CHMTSLAM&) : mrpt::system::COutputLogger()
366  {
367  THROW_EXCEPTION("This object cannot be copied.");
368  }
369  const CHMTSLAM& operator=(const CHMTSLAM&)
370  {
371  THROW_EXCEPTION("This object cannot be copied.");
372  }
373 
374  /** Destructor
375  */
376  ~CHMTSLAM() override;
377 
378  /** Return true if an exception has been caught in any thread leading to the
379  * end of the mapping application: no more actions/observations will be
380  * processed from now on.
381  */
382  bool abortedDueToErrors();
383 
384  /** @name High-level map management
385  @{ */
386 
387  /** Loads the options from a config file. */
388  void loadOptions(const std::string& configFile);
389  /** Loads the options from a config source */
390  void loadOptions(const mrpt::config::CConfigFileBase& cfgSource);
391 
392  /** Initializes the whole HMT-SLAM framework, reseting to an empty map (It
393  * also clears the logs directory) - this must be called AFTER loading the
394  * options with CHMTSLAM::loadOptions. */
395  void initializeEmptyMap();
396 
397  /** Save the state of the whole HMT-SLAM framework to some binary stream
398  * (e.g. a file).
399  * \return true if everything goes OK.
400  * \sa loadState
401  */
403 
404  /** Load the state of the whole HMT-SLAM framework from some binary stream
405  * (e.g. a file).
406  * \return true if everything goes OK.
407  * \sa saveState
408  */
410  /** @} */
411 
412  /** @name The important data.
413  @{ */
414  /** The hiearchical, multi-hypothesis graph-based map. */
416  /** The list of LMHs at each instant. */
417  std::map<THypothesisID, CLocalMetricHypothesis> m_LMHs;
418  /** @} */
419 
420  /** Called from LSLAM thread when log files must be created.
421  */
422  void generateLogFiles(unsigned int nIteration);
423 
424  /** Gets a 3D representation of the current state of the whole mapping
425  * framework.
426  */
428 
429  protected:
430  /** A variety of options and configuration params (private, use
431  * loadOptions).
432  */
434  {
435  /** Initialization of default params
436  */
437  TOptions();
438 
439  void loadFromConfigFile(
440  const mrpt::config::CConfigFileBase& source,
441  const std::string& section) override; // See base docs
442  void dumpToTextStream(
443  std::ostream& out) const override; // See base docs
444 
445  /** [LOGGING] If it is not an empty string (""), a directory with that
446  * name will be created and log files save there. */
447  std::string LOG_OUTPUT_DIR;
448  /** [LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log
449  * file will be generated. */
451 
452  /** [LSLAM] The method to use for local SLAM
453  */
455 
456  /** [LSLAM] Minimum distance (and heading) difference between
457  * observations inserted in the map.
458  */
460 
461  /** [LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for
462  * odometry increments (Default=0) */
464 
465  /** [LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry
466  * increments (Default=0) */
468 
469  /** [VIEW3D] The height of the areas' spheres.
470  */
472 
473  /** [VIEW3D] The radius of the areas' spheres.
474  */
476 
477  /** A 3-length vector with the std. deviation of the transition model in
478  * (x,y,phi) used only when there is no odometry (if there is odo, its
479  * uncertainty values will be used instead); x y: In meters, phi:
480  * radians (but in degrees when loading from a configuration ini-file!)
481  */
483 
484  /** [AA] The options for the partitioning algorithm
485  */
487 
488  /** The default set of maps to be created in each particle */
490  /** These params are used from every LMH object. */
493 
494  /** 0 means randomize, use any other value to have repetitive
495  * experiments. */
497 
498  /** A list of topological loop-closure detectors to use: can be one or
499  * more from this list:
500  * 'gridmaps': Occupancy Grid matching.
501  * 'fabmap': Mark Cummins' image matching framework.
502  */
503  std::vector<std::string> TLC_detectors;
504 
505  /** Options passed to this TLC constructor */
507  /** Options passed to this TLC constructor */
509 
511 
512 }; // End of class CHMTSLAM.
513 
514 /** Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
515  */
517 {
519 
520  protected:
522 
523  public:
524  /** Constructor
525  */
526  CLSLAMAlgorithmBase(CHMTSLAM* parent) : m_parent(parent) {}
527  /** Destructor
528  */
529  virtual ~CLSLAMAlgorithmBase() = default;
530  /** Main entry point from HMT-SLAM: process some actions & observations.
531  * The passed action/observation will be deleted, so a copy must be made
532  * if necessary.
533  * This method must be in charge of updating the robot pose estimates and
534  * also to update the
535  * map when required.
536  *
537  * \param LMH The local metric hypothesis which must be updated by this
538  * SLAM algorithm.
539  * \param act The action to process (or nullptr).
540  * \param sf The observations to process (or nullptr).
541  */
542  virtual void processOneLMH(
545  const mrpt::obs::CSensoryFrame::Ptr& sf) = 0;
546 
547  /** The PF algorithm implementation.
548  */
551  const mrpt::obs::CSensoryFrame* observation,
552  const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;
553 
554  /** The PF algorithm implementation. */
557  const mrpt::obs::CSensoryFrame* observation,
558  const bayes::CParticleFilter::TParticleFilterOptions& PF_options) = 0;
559 
560 }; // end of class CLSLAMAlgorithmBase
561 
562 /** Implements a 2D local SLAM method based on a RBPF over an occupancy grid
563  * map.
564  * This class is used internally in mrpt::slam::CHMTSLAM
565  */
567 {
569 
570  public:
571  /** Constructor
572  */
573  CLSLAM_RBPF_2DLASER(CHMTSLAM* parent);
574 
575  /** Destructor
576  */
578 
579  /** Main entry point from HMT-SLAM: process some actions & observations.
580  * The passed action/observation will be deleted, so a copy must be made
581  * if necessary.
582  * This method must be in charge of updating the robot pose estimates and
583  * also to update the
584  * map when required.
585  *
586  * \param LMH The local metric hypothesis which must be updated by this
587  * SLAM algorithm.
588  * \param act The action to process (or nullptr).
589  * \param sf The observations to process (or nullptr).
590  */
591  void processOneLMH(
594  const mrpt::obs::CSensoryFrame::Ptr& sf) override;
595 
596  /** The PF algorithm implementation. */
599  const mrpt::obs::CSensoryFrame* observation,
601  override;
602 
603  /** The PF algorithm implementation. */
606  const mrpt::obs::CSensoryFrame* observation,
608  override;
609 
610  protected:
611  /** For use within PF callback methods */
613 
614  /** Auxiliary structure
615  */
616  struct TPathBin
617  {
618  TPathBin() : x(), y(), phi() {}
619  std::vector<int> x, y, phi;
620 
621  /** For debugging purposes!
622  */
623  void dumpToStdOut() const;
624  };
625 
626  /** Fills out a "TPathBin" variable, given a path hypotesis and (if not set
627  * to nullptr) a new pose appended at the end, using the KLD params in
628  * "options".
629  */
631  TPathBin& outBin, TMapPoseID2Pose3D* path = nullptr,
632  mrpt::poses::CPose2D* newPose = nullptr);
633 
634  /** Checks if a given "TPathBin" element is already into a set of them, and
635  * return its index (first one is 0), or -1 if not found.
636  */
637  int findTPathBinIntoSet(TPathBin& desiredBin, std::deque<TPathBin>& theSet);
638 
639  /** Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal"
640  */
641  static double particlesEvaluator_AuxPFOptimal(
643  const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
644  const void* action, const void* observation);
645 
646  /** Auxiliary function that evaluates the likelihood of an observation,
647  * given a robot pose, and according to the options in
648  * "CPosePDFParticles::options".
649  */
653  size_t particleIndexForMap, const mrpt::obs::CSensoryFrame* observation,
654  const mrpt::poses::CPose2D* x);
655 
656 }; // end class CLSLAM_RBPF_2DLASER
657 
658 } // namespace hmtslam
659 } // namespace mrpt
mrpt::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
Definition: CPose3DPDFSOG.h:33
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::hmtslam::CTopLCDetector_GridMatching::TOptions
Options for a TLC-detector of type gridmap-matching, used from CHMTSLAM.
Definition: CTopLCDetector_GridMatching.h:55
mrpt::hmtslam::CHMTSLAM::TOptions::defaultMapsInitializers
mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers
The default set of maps to be created in each particle.
Definition: CHMTSLAM.h:489
mrpt::hmtslam::CHMTSLAM::m_terminationFlag_3D_viewer
std::atomic_bool m_terminationFlag_3D_viewer
Definition: CHMTSLAM.h:342
CHierarchicalMHMap.h
mrpt::slam::CIncrementalMapPartitioner::TOptions
Configuration parameters.
Definition: CIncrementalMapPartitioner.h:68
mrpt::hmtslam::CLocalMetricHypothesis
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Definition: CLocalMetricHypothesis.h:71
mrpt::hmtslam::CHMTSLAM::generateHypothesisID
static THypothesisID generateHypothesisID()
Generates a new and unique hypothesis ID.
Definition: CHMTSLAM_main.cpp:530
mrpt::hmtslam::CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Definition: CHMTSLAM.h:567
mrpt::hmtslam::CHMTSLAM::m_LSLAM_method
CLSLAMAlgorithmBase * m_LSLAM_method
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" o...
Definition: CHMTSLAM.h:301
mrpt::hmtslam::CHMTSLAM::TOptions::MIN_ODOMETRY_STD_XY
float MIN_ODOMETRY_STD_XY
[LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0)
Definition: CHMTSLAM.h:463
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI
Message definition:
Definition: CHMTSLAM.h:128
mrpt::hmtslam::CHMTSLAM::TOptions::AA_options
mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options
[AA] The options for the partitioning algorithm
Definition: CHMTSLAM.h:486
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMtoTBI::areaIDs
TNodeIDList areaIDs
The areas to consider.
Definition: CHMTSLAM.h:119
mrpt::hmtslam::CHMTSLAM::thread_TBI
void thread_TBI()
The function for the "TBI" thread.
Definition: CHMTSLAM_TBI.cpp:33
mrpt::hmtslam::CTopLCDetector_GridMatching
Definition: CTopLCDetector_GridMatching.h:19
mrpt::hmtslam::CHMTSLAM::LSLAM_process_message_from_TBI
void LSLAM_process_message_from_TBI(const TMessageLSLAMfromTBI &myMsg)
No critical section locks are assumed at the entrance of this method.
Definition: CHMTSLAM_LSLAM.cpp:1894
mrpt::serialization::CSerializable::Ptr
std::shared_ptr< CSerializable > Ptr
Definition: CSerializable.h:36
mrpt::hmtslam::CHMTSLAM::registerLoopClosureDetector
void registerLoopClosureDetector(const std::string &name, CTopLCDetectorBase *(*ptrCreateObject)(CHMTSLAM *))
Must be invoked before calling initializeEmptyMap, so LC objects can be created.
Definition: CHMTSLAM_main.cpp:548
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::hmtslam::CHMTSLAM::TOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CHMTSLAM_main.cpp:289
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::cur_area
CHMHMapNode::TNodeID cur_area
The area for who the loop-closure has been computed.
Definition: CHMTSLAM.h:134
CPointsMap.h
mrpt::hmtslam::CHMTSLAM::abortedDueToErrors
bool abortedDueToErrors()
Return true if an exception has been caught in any thread leading to the end of the mapping applicati...
Definition: CHMTSLAM_main.cpp:539
mrpt::hmtslam::CHMTSLAM::TBI_main_method
static TMessageLSLAMfromTBI::Ptr TBI_main_method(CLocalMetricHypothesis *LMH, const CHMHMapNode::TNodeID &areaID)
The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.
Definition: CHMTSLAM_TBI.cpp:102
mrpt::hmtslam::CTopLCDetector_FabMap
Definition: CTopLCDetector_FabMap.h:17
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::hmtslam::CHMTSLAM
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:69
mrpt::hmtslam::CHMTSLAM::generatePoseID
static TPoseID generatePoseID()
Generates a new and unique pose ID.
Definition: CHMTSLAM_main.cpp:526
mrpt::hmtslam::CHMTSLAM::m_topLCdets
std::deque< CTopLCDetectorBase * > m_topLCdets
The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".
Definition: CHMTSLAM.h:315
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::findTPathBinIntoSet
int findTPathBinIntoSet(TPathBin &desiredBin, std::deque< TPathBin > &theSet)
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one i...
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:753
mrpt::hmtslam::CHMTSLAM::m_hThread_TBI
std::thread m_hThread_TBI
Definition: CHMTSLAM.h:267
CTopLCDetector_FabMap.h
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:57
mrpt::hmtslam::CHMTSLAM::TOptions::random_seed
int random_seed
0 means randomize, use any other value to have repetitive experiments.
Definition: CHMTSLAM.h:496
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:18
CThreadSafeQueue.h
mrpt::hmtslam::CLSLAMAlgorithmBase::processOneLMH
virtual void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf)=0
Main entry point from HMT-SLAM: process some actions & observations.
mrpt::hmtslam::CHMTSLAM::TOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CHMTSLAM_main.cpp:329
CTopLCDetector_GridMatching.h
mrpt::hmtslam::CHMTSLAM::TOptions::stds_Q_no_odo
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
Definition: CHMTSLAM.h:482
HMT_SLAM_common.h
mrpt::hmtslam::CHMTSLAM::thread_LSLAM
void thread_LSLAM()
The function for the "Local SLAM" thread.
Definition: CHMTSLAM_LSLAM.cpp:47
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::phi
std::vector< int > phi
Definition: CHMTSLAM.h:619
mrpt::hmtslam::CHMTSLAM::generateLogFiles
void generateLogFiles(unsigned int nIteration)
Called from LSLAM thread when log files must be created.
Definition: CHMTSLAM_LOG.cpp:41
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info
Definition: CHMTSLAM.h:137
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::~CLSLAM_RBPF_2DLASER
~CLSLAM_RBPF_2DLASER() override
Destructor.
mrpt::hmtslam::CHMTSLAM::generateUniqueAreaLabel
static std::string generateUniqueAreaLabel()
Generates a new and unique area textual label (currently this generates "0","1",.....
Definition: CHMTSLAM_main.cpp:518
CICP.h
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfAuxiliaryPFOptimal
void prediction_and_update_pfAuxiliaryPFOptimal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:222
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::hmtslam::CHMTSLAM::lsmRBPF_2DLASER
@ lsmRBPF_2DLASER
Definition: CHMTSLAM.h:231
mrpt::hmtslam::CLSLAMAlgorithmBase::prediction_and_update_pfAuxiliaryPFOptimal
virtual void prediction_and_update_pfAuxiliaryPFOptimal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)=0
The PF algorithm implementation.
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:27
mrpt::hmtslam::CHMTSLAM::m_terminationFlag_LSLAM
std::atomic_bool m_terminationFlag_LSLAM
Threads termination flags:
Definition: CHMTSLAM.h:341
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:16
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::Ptr
std::shared_ptr< TMessageLSLAMfromTBI > Ptr
Definition: CHMTSLAM.h:129
mrpt::hmtslam::CHMTSLAM::getNextObjectFromInputQueue
mrpt::serialization::CSerializable::Ptr getNextObjectFromInputQueue()
Used from the LSLAM thread to retrieve the next object from the queue.
Definition: CHMTSLAM_main.cpp:384
mrpt::hmtslam::CHMTSLAM::m_topLCdets_cs
std::mutex m_topLCdets_cs
The critical section for accessing m_topLCdets.
Definition: CHMTSLAM.h:318
mrpt::hmtslam::CHMTSLAM::m_nextPoseID
static TPoseID m_nextPoseID
Definition: CHMTSLAM.h:355
mrpt::hmtslam::CHMTSLAM::pushAction
void pushAction(const mrpt::obs::CActionCollection::Ptr &acts)
Here the user can enter an action into the system (will go to the SLAM process).
Definition: CHMTSLAM_main.cpp:171
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::hmtslam::CHMTSLAM::m_nextAreaLabel
static int64_t m_nextAreaLabel
Definition: CHMTSLAM.h:354
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::dumpToStdOut
void dumpToStdOut() const
For debugging purposes!
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:678
CActionCollection.h
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::loopClosureData
std::map< CHMHMapNode::TNodeID, TBI_info > loopClosureData
The meat is here: only feasible loop closures from "cur_area" are included here, with associated data...
Definition: CHMTSLAM.h:153
mrpt::hmtslam::CHMTSLAM::loadState
bool loadState(mrpt::serialization::CArchive &in)
Load the state of the whole HMT-SLAM framework from some binary stream (e.g.
Definition: CHMTSLAM_main.cpp:582
mrpt::hmtslam::CHMTSLAM::TOptions::LOG_OUTPUT_DIR
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:447
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:55
mrpt::hmtslam::CHMTSLAM::m_map
CHierarchicalMHMap m_map
The hiearchical, multi-hypothesis graph-based map.
Definition: CHMTSLAM.h:415
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:90
mrpt::hmtslam::CHMTSLAM::m_LMHs
std::map< THypothesisID, CLocalMetricHypothesis > m_LMHs
The list of LMHs at each instant.
Definition: CHMTSLAM.h:417
mrpt::hmtslam::CHMTSLAM::TOptions::KLD_params
mrpt::slam::TKLDParams KLD_params
Definition: CHMTSLAM.h:492
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::auxiliarComputeObservationLikelihood
static double auxiliarComputeObservationLikelihood(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame *observation, const mrpt::poses::CPose2D *x)
Auxiliary function that evaluates the likelihood of an observation, given a robot pose,...
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:662
CLocalMetricHypothesis.h
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:52
mrpt::hmtslam
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Definition: CHierarchicalMapMHPartition.h:28
mrpt::serialization::CMessage
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:28
mrpt::hmtslam::CHMTSLAM::perform_TLC
void perform_TLC(CLocalMetricHypothesis &LMH, const CHMHMapNode::TNodeID areaInLMH, const CHMHMapNode::TNodeID areaLoopClosure, const mrpt::poses::CPose3DPDFGaussian &pose1wrt2)
Topological Loop Closure: Performs all the required operations to close a loop between two areas whic...
Definition: CHMTSLAM_perform_TLC.cpp:34
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info::delta_new_cur
mrpt::poses::CPose3DPDFSOG delta_new_cur
Depending on the loop-closure engine, an guess of the relative pose of "area_new" relative to "cur_ar...
Definition: CHMTSLAM.h:147
mrpt::hmtslam::CHMTSLAM::getAs3DScene
void getAs3DScene(mrpt::opengl::COpenGLScene &outScene)
Gets a 3D representation of the current state of the whole mapping framework.
Definition: CHMTSLAM_main.cpp:534
mrpt::hmtslam::CHMTSLAM::loadOptions
void loadOptions(const std::string &configFile)
Loads the options from a config file.
Definition: CHMTSLAM_main.cpp:251
mrpt::hmtslam::CHMTSLAM::TOptions::TLC_detectors
std::vector< std::string > TLC_detectors
A list of topological loop-closure detectors to use: can be one or more from this list: 'gridmaps': O...
Definition: CHMTSLAM.h:503
mrpt::hmtslam::CHMTSLAM::loopClosureDetector_factory
CTopLCDetectorBase * loopClosureDetector_factory(const std::string &name)
The class factory for topological loop closure detectors.
Definition: CHMTSLAM_main.cpp:557
mrpt::hmtslam::CHMTSLAM::isInputQueueEmpty
bool isInputQueueEmpty()
Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueu...
Definition: CHMTSLAM_main.cpp:357
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole
void dumpToConsole() const
for debugging only
Definition: CHMTSLAM_AA.cpp:115
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::partitions
std::vector< TPoseIDList > partitions
Definition: CHMTSLAM.h:102
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:32
mrpt::hmtslam::CLSLAMAlgorithmBase::prediction_and_update_pfOptimalProposal
virtual void prediction_and_update_pfOptimalProposal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)=0
The PF algorithm implementation.
mrpt::hmtslam::CHMTSLAM::TOptions::TLC_fabmap_options
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:508
mrpt::hmtslam::CHMTSLAM::LSLAM_process_message
void LSLAM_process_message(const mrpt::serialization::CMessage &msg)
Auxiliary method within thread_LSLAM.
Definition: CHMTSLAM_LSLAM.cpp:269
COutputLogger.h
COpenGLScene.h
mrpt::hmtslam::CHMTSLAM::TOptions::MIN_ODOMETRY_STD_PHI
float MIN_ODOMETRY_STD_PHI
[LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0)
Definition: CHMTSLAM.h:467
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:45
mrpt::hmtslam::CHMTSLAM::m_map_cs
std::mutex m_map_cs
Critical section for accessing m_map.
Definition: CHMTSLAM.h:248
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:41
mrpt::hmtslam::CHMTSLAM::pushObservations
void pushObservations(const mrpt::obs::CSensoryFrame::Ptr &sf)
Here the user can enter observations into the system (will go to the SLAM process).
Definition: CHMTSLAM_main.cpp:189
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
mrpt::hmtslam::CHMTSLAM::initializeEmptyMap
void initializeEmptyMap()
Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory...
Definition: CHMTSLAM_main.cpp:402
mrpt::hmtslam::CHMTSLAM::TOptions::LOG_FREQUENCY
int LOG_FREQUENCY
[LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
Definition: CHMTSLAM.h:450
mrpt::hmtslam::CHMTSLAM::areaAbstraction
static TMessageLSLAMfromAA::Ptr areaAbstraction(CLocalMetricHypothesis *LMH, const TPoseIDList &newPoseIDs)
The Area Abstraction (AA) method, invoked from LSLAM.
Definition: CHMTSLAM_AA.cpp:32
mrpt::hmtslam::CLSLAMAlgorithmBase::m_parent
mrpt::safe_ptr< CHMTSLAM > m_parent
Definition: CHMTSLAM.h:521
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:27
mrpt::serialization::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:31
mrpt::hmtslam::CHMTSLAM::TOptions::TOptions
TOptions()
Initialization of default params.
Definition: CHMTSLAM_main.cpp:261
mrpt::hmtslam::CHMTSLAM::m_LMHs_cs
std::mutex m_LMHs_cs
Critical section for accessing m_LMHs.
Definition: CHMTSLAM.h:251
mrpt::hmtslam::CLSLAMAlgorithmBase
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
Definition: CHMTSLAM.h:517
mrpt::hmtslam::CHMTSLAM::clearInputQueue
void clearInputQueue()
Empty the input queue.
Definition: CHMTSLAM_main.cpp:154
mrpt::hmtslam::CTopLCDetector_FabMap::TOptions
Options for a TLC-detector of type FabMap, used from CHMTSLAM.
Definition: CTopLCDetector_FabMap.h:54
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::Ptr
std::shared_ptr< TMessageLSLAMfromAA > Ptr
Definition: CHMTSLAM.h:99
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::y
std::vector< int > y
Definition: CHMTSLAM.h:619
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::TPathBin
TPathBin()
Definition: CHMTSLAM.h:618
mrpt::hmtslam::CHMTSLAM::saveState
bool saveState(mrpt::serialization::CArchive &out) const
Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
Definition: CHMTSLAM_main.cpp:569
mrpt::hmtslam::CHMTSLAM::TOptions::SLAM_MIN_DIST_BETWEEN_OBS
float SLAM_MIN_DIST_BETWEEN_OBS
[LSLAM] Minimum distance (and heading) difference between observations inserted in the map.
Definition: CHMTSLAM.h:459
mrpt::math::CVectorDynamic
Template for column vectors of dynamic size, compatible with Eigen.
Definition: CVectorDynamic.h:32
mrpt::hmtslam::CHMTSLAM::m_registeredLCDetectors
std::map< std::string, TLopLCDetectorFactory > m_registeredLCDetectors
Definition: CHMTSLAM.h:311
mrpt::hmtslam::CHMTSLAM::m_options
mrpt::hmtslam::CHMTSLAM::TOptions m_options
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:118
mrpt::hmtslam::CLSLAMAlgorithmBase::~CLSLAMAlgorithmBase
virtual ~CLSLAMAlgorithmBase()=default
Destructor.
mrpt::hmtslam::CHMTSLAM::TOptions::pf_options
bayes::CParticleFilter::TParticleFilterOptions pf_options
These params are used from every LMH object.
Definition: CHMTSLAM.h:491
mrpt::hmtslam::CHMTSLAM::TOptions::VIEW3D_AREA_SPHERES_RADIUS
float VIEW3D_AREA_SPHERES_RADIUS
[VIEW3D] The radius of the areas' spheres.
Definition: CHMTSLAM.h:475
mrpt::hmtslam::CHMTSLAM::pushObservation
void pushObservation(const mrpt::obs::CObservation::Ptr &obs)
Here the user can enter an observation into the system (will go to the SLAM process).
Definition: CHMTSLAM_main.cpp:207
TKLDParams.h
mrpt::hmtslam::CHMTSLAM::TOptions::TLC_grid_options
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:506
mrpt::hmtslam::TPoseID
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
Definition: HMT_SLAM_common.h:63
mrpt::hmtslam::CHMTSLAM::m_LSLAM_queue
CMessageQueue m_LSLAM_queue
Definition: CHMTSLAM.h:163
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:58
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA::hypothesisID
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:101
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin::x
std::vector< int > x
Definition: CHMTSLAM.h:619
mrpt::hmtslam::CLSLAMAlgorithmBase::CLSLAMAlgorithmBase
CLSLAMAlgorithmBase(CHMTSLAM *parent)
Constructor.
Definition: CHMTSLAM.h:526
mrpt::hmtslam::CHMTSLAM::CHMTSLAM
CHMTSLAM()
Default constructor.
Definition: CHMTSLAM_main.cpp:56
mrpt::hmtslam::CHMTSLAM::m_nextHypID
static THypothesisID m_nextHypID
Definition: CHMTSLAM.h:356
mrpt::hmtslam::CHMTSLAM::m_inputQueue_cs
std::mutex m_inputQueue_cs
Critical section for accessing m_inputQueue.
Definition: CHMTSLAM.h:245
mrpt::hmtslam::CHMTSLAM::TOptions::SLAM_METHOD
TLSlamMethod SLAM_METHOD
[LSLAM] The method to use for local SLAM
Definition: CHMTSLAM.h:454
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH
void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf) override
Main entry point from HMT-SLAM: process some actions & observations.
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:58
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAA
Message definition:
Definition: CHMTSLAM.h:98
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info::log_lik
double log_lik
Log likelihood for this loop-closure.
Definition: CHMTSLAM.h:140
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::TPathBin
Auxiliary structure.
Definition: CHMTSLAM.h:617
mrpt::hmtslam::CHMTSLAM::LSLAM_process_message_from_AA
void LSLAM_process_message_from_AA(const TMessageLSLAMfromAA &myMsg)
No critical section locks are assumed at the entrance of this method.
Definition: CHMTSLAM_LSLAM.cpp:298
mrpt::hmtslam::CHMTSLAM::m_hThread_LSLAM
std::thread m_hThread_LSLAM
Threads handles.
Definition: CHMTSLAM.h:267
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMtoTBI::Ptr
std::shared_ptr< TMessageLSLAMtoTBI > Ptr
Definition: CHMTSLAM.h:115
mrpt::containers::list_searchable< CHMHMapNode::TNodeID >
mrpt::hmtslam::CHMTSLAM::~CHMTSLAM
~CHMTSLAM() override
Destructor.
Definition: CHMTSLAM_main.cpp:87
mrpt::hmtslam::CHMTSLAM::TOptions
A variety of options and configuration params (private, use loadOptions).
Definition: CHMTSLAM.h:434
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal
void prediction_and_update_pfOptimalProposal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:774
mrpt::hmtslam::CHMTSLAM::TLopLCDetectorFactory
CTopLCDetectorBase *(*)(CHMTSLAM *) TLopLCDetectorFactory
Definition: CHMTSLAM.h:309
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER
CLSLAM_RBPF_2DLASER(CHMTSLAM *parent)
Constructor.
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:37
mrpt::hmtslam::CHMTSLAM::CHMTSLAM
CHMTSLAM(const CHMTSLAM &)
Definition: CHMTSLAM.h:365
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::hypothesisID
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:131
mrpt::hmtslam::CHMTSLAM::TLSlamMethod
TLSlamMethod
Definition: CHMTSLAM.h:230
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMtoTBI::LMH
CLocalMetricHypothesis * LMH
The LMH.
Definition: CHMTSLAM.h:117
mrpt::hmtslam::CHMTSLAM::inputQueueSize
size_t inputQueueSize()
Returns the number of objects waiting for processing in the input queue.
Definition: CHMTSLAM_main.cpp:371
mrpt::hmtslam::CHMTSLAM::TOptions::SLAM_MIN_HEADING_BETWEEN_OBS
float SLAM_MIN_HEADING_BETWEEN_OBS
Definition: CHMTSLAM.h:459
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBI::TBI_info::TBI_info
TBI_info()=default
mrpt::hmtslam::TPoseIDList
std::vector< TPoseID > TPoseIDList
Definition: HMT_SLAM_common.h:67
mrpt::hmtslam::CHMTSLAM::TMessageLSLAMtoTBI
Message definition:
Definition: CHMTSLAM.h:114
mrpt::hmtslam::CHMTSLAM::thread_3D_viewer
void thread_3D_viewer()
The function for the "3D viewer" thread.
Definition: CHMTSLAM_3D_viewer.cpp:31
mrpt::hmtslam::CHierarchicalMHMap
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model.
Definition: CHierarchicalMHMap.h:28
CMessage.h
mrpt::hmtslam::CTopLCDetectorBase
The virtual base class for Topological Loop-closure Detectors; used in HMT-SLAM.
Definition: CTopLCDetectorBase.h:24
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:103
mrpt::hmtslam::CHMTSLAM::m_terminationFlag_TBI
std::atomic_bool m_terminationFlag_TBI
Definition: CHMTSLAM.h:341
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::m_insertNewRobotPose
bool m_insertNewRobotPose
For use within PF callback methods.
Definition: CHMTSLAM.h:612
mrpt::safe_ptr
A wrapper class for pointers that can be safely copied with "=" operator without problems.
Definition: safe_pointers.h:72
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::loadTPathBinFromPath
void loadTPathBinFromPath(TPathBin &outBin, TMapPoseID2Pose3D *path=nullptr, mrpt::poses::CPose2D *newPose=nullptr)
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to nullptr) a new pose append...
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:698
mrpt::hmtslam::TMapPoseID2Pose3D
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
Definition: CLocalMetricHypothesis.h:34
mrpt::hmtslam::CHMTSLAM::TOptions::VIEW3D_AREA_SPHERES_HEIGHT
float VIEW3D_AREA_SPHERES_HEIGHT
[VIEW3D] The height of the areas' spheres.
Definition: CHMTSLAM.h:471
mrpt::system::COutputLogger::COutputLogger
COutputLogger()
Default class constructor.
Definition: COutputLogger.cpp:70
mrpt::hmtslam::CHMTSLAM::m_hThread_3D_viewer
std::thread m_hThread_3D_viewer
Definition: CHMTSLAM.h:267
mrpt::hmtslam::CHMTSLAM::m_terminateThreads
std::atomic_bool m_terminateThreads
Termination flag for signaling all threads to terminate.
Definition: CHMTSLAM.h:337
mrpt::hmtslam::CHMTSLAM::m_inputQueue
std::queue< mrpt::serialization::CSerializable::Ptr > m_inputQueue
The queue of pending actions/observations supplied by the user waiting for being processed.
Definition: CHMTSLAM.h:242
mrpt::hmtslam::CLSLAM_RBPF_2DLASER::particlesEvaluator_AuxPFOptimal
static double particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
Definition: CHMTSLAM_LSLAM_RBPF_2DLASER.cpp:571
mrpt::hmtslam::CHMTSLAM::operator=
const CHMTSLAM & operator=(const CHMTSLAM &)
Definition: CHMTSLAM.h:369
mrpt::containers::CThreadSafeQueue< mrpt::serialization::CMessage >



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020