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>
28#include <queue>
29
30namespace 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;
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 {
62 friend class CLSLAM_RBPF_2DLASER;
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 */
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 */
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 */
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 */
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. */
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 */
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.
456 DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CHMTSLAM, mrpt::utils::CSerializable, HMTSLAM_IMPEXP )
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 */
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(
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,
498
499 /** The PF algorithm implementation. */
502 const mrpt::obs::CActionCollection * action,
503 const mrpt::obs::CSensoryFrame * observation,
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 */
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.
aligned_containers< THypothesisID, CLocalMetricHypothesis >::map_t m_LMHs
The list of LMHs at each instant.
Definition: CHMTSLAM.h:380
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.
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
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
CTopLCDetectorBase * loopClosureDetector_factory(const std::string &name)
The class factory for topological loop closure detectors.
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
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
Definition: CHMTSLAM.h:39
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.
struct OBS_IMPEXP CActionCollectionPtr
struct OBS_IMPEXP CSensoryFramePtr
struct OBS_IMPEXP CObservationPtr
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.6 for MRPT 1.4.0 SVN: at Tue Jan 17 22:27:43 UTC 2023