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



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