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  {
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  {
116  /** The LMH */
118  /** The areas to consider. */
120  };
121 
122  /** Message definition:
123  - From: TBI
124  - To: LSLAM
125  - Meaning:
126  */
128  {
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  */
160  using CMessageQueue =
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  {
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 
510  } m_options;
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  */
577  ~CLSLAM_RBPF_2DLASER() override;
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
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:467
int random_seed
0 means randomize, use any other value to have repetitive experiments.
Definition: CHMTSLAM.h:496
float MIN_ODOMETRY_STD_XY
[LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0) ...
Definition: CHMTSLAM.h:463
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
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:566
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:153
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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:503
int LOG_FREQUENCY
[LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
Definition: CHMTSLAM.h:450
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:508
CTopLCDetectorBase *(*)(CHMTSLAM *) TLopLCDetectorFactory
Definition: CHMTSLAM.h:309
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
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:267
std::atomic_bool m_terminationFlag_3D_viewer
Definition: CHMTSLAM.h:341
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
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:355
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.
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:311
std::atomic_bool m_terminationFlag_TBI
Definition: CHMTSLAM.h:341
Option set for KLD algorithm.
Definition: TKLDParams.h:17
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:101
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:248
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
Definition: CHMTSLAM.h:506
static THypothesisID m_nextHypID
Definition: CHMTSLAM.h:356
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.
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
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
Definition: CHMTSLAM.h:516
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
static int64_t m_nextAreaLabel
Definition: CHMTSLAM.h:354
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
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:526
CHMTSLAM(const CHMTSLAM &)
Definition: CHMTSLAM.h:365
mrpt::serialization::CSerializable::Ptr getNextObjectFromInputQueue()
Used from the LSLAM thread to retrieve the next object from the queue.
~CLSLAM_RBPF_2DLASER() override
Destructor.
CHMHMapNode::TNodeID cur_area
The area for who the loop-closure has been computed.
Definition: CHMTSLAM.h:134
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:492
This class allows loading and storing values and vectors of different types from a configuration text...
std::atomic_bool m_terminationFlag_LSLAM
Threads termination flags:
Definition: CHMTSLAM.h:341
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
std::mutex m_LMHs_cs
Critical section for accessing m_LMHs.
Definition: CHMTSLAM.h:251
std::mutex m_topLCdets_cs
The critical section for accessing m_topLCdets.
Definition: CHMTSLAM.h:318
CMessageQueue m_LSLAM_queue
Definition: CHMTSLAM.h:163
mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers
The default set of maps to be created in each particle.
Definition: CHMTSLAM.h:489
Versatile class for consistent logging and management of output messages.
mrpt::safe_ptr< CHMTSLAM > m_parent
Definition: CHMTSLAM.h:521
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
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
std::vector< TPoseIDList > partitions
Definition: CHMTSLAM.h:102
std::map< THypothesisID, CLocalMetricHypothesis > m_LMHs
The list of LMHs at each instant.
Definition: CHMTSLAM.h:417
std::deque< CTopLCDetectorBase * > m_topLCdets
The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".
Definition: CHMTSLAM.h:315
virtual ~CLSLAMAlgorithmBase()=default
Destructor.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
std::thread m_hThread_3D_viewer
Definition: CHMTSLAM.h:267
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:415
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
Definition: CHMTSLAM.h:131
mrpt::hmtslam::CHMTSLAM::TOptions m_options
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.
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
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.
std::atomic_bool m_terminateThreads
Termination flag for signaling all threads to terminate.
Definition: CHMTSLAM.h:337
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:454
~CHMTSLAM() override
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:491
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:482
A variety of options and configuration params (private, use loadOptions).
Definition: CHMTSLAM.h:433
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:140
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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:27
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:486
float VIEW3D_AREA_SPHERES_HEIGHT
[VIEW3D] The height of the areas&#39; spheres.
Definition: CHMTSLAM.h:471
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
const CHMTSLAM & operator=(const CHMTSLAM &)
Definition: CHMTSLAM.h:369
mrpt::vision::TStereoCalibResults out
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:475
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).
The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model...
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:56
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
TNodeIDList areaIDs
The areas to consider.
Definition: CHMTSLAM.h:119
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void thread_3D_viewer()
The function for the "3D viewer" thread.
CLocalMetricHypothesis * LMH
The LMH.
Definition: CHMTSLAM.h:117
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:612
std::thread m_hThread_LSLAM
Threads handles.
Definition: CHMTSLAM.h:267
float SLAM_MIN_DIST_BETWEEN_OBS
[LSLAM] Minimum distance (and heading) difference between observations inserted in the map...
Definition: CHMTSLAM.h:459
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.
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 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.
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:245
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020