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



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:44:04 UTC 2022