36 class CLSLAMAlgorithmBase;
296 const std::string &name,
582 std::deque<TPathBin> &theSet
592 const void *observation );
599 size_t particleIndexForMap,
#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.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
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.
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
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.
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:
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.
CHierarchicalMHMap m_map
The hiearchical, multi-hypothesis graph-based map.
virtual ~CHMTSLAM()
Destructor.
void clearInputQueue()
Empty the input queue.
bool m_terminateThreads
Termination flag for signaling all threads to terminate.
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.
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
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.
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.
void loadOptions(const std::string &configFile)
Loads the options from a config file.
std::map< std::string, TLopLCDetectorFactory > m_registeredLCDetectors
stlplus::smart_ptr< TMessageLSLAMtoTBI > TMessageLSLAMtoTBIPtr
stlplus::smart_ptr< TMessageLSLAMfromTBI > TMessageLSLAMfromTBIPtr
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
static TPoseID m_nextPoseID
bool m_terminationFlag_TBI
CLSLAMAlgorithmBase * m_LSLAM_method
An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" o...
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
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
CHMTSLAM(const CHMTSLAM &)
synch::CCriticalSection m_topLCdets_cs
The critical section for accessing m_topLCdets.
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".
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.
bool m_insertNewRobotPose
For use within PF callback methods.
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.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
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.
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.
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.
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.
A class used to store a 2D pose.
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 .
Option set for KLD algorithm.
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...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
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.
std::vector< int32_t > vector_int
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define THROW_EXCEPTION(msg)
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
class HMTSLAM_IMPEXP CHMTSLAM
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::vector< TPoseID > TPoseIDList
class HMTSLAM_IMPEXP CLSLAM_RBPF_2DLASER
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
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
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
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...
double log_lik
Log likelihood for this loop-closure.
THypothesisID hypothesisID
The hypothesis ID (LMH) for these results.
std::map< CHMHMapNode::TNodeID, TBI_info > loopClosureData
The meat is here: only feasible loop closures from "cur_area" are included here, with associated data...
CHMHMapNode::TNodeID cur_area
The area for who the loop-closure has been computed.
TNodeIDList areaIDs
The areas to consider.
CLocalMetricHypothesis * LMH
The LMH.
A variety of options and configuration params (private, use loadOptions).
mrpt::math::CVectorFloat stds_Q_no_odo
A 3-length vector with the std.
CTopLCDetector_FabMap::TOptions TLC_fabmap_options
Options passed to this TLC constructor.
mrpt::slam::TKLDParams KLD_params
CTopLCDetector_GridMatching::TOptions TLC_grid_options
Options passed to this TLC constructor.
float VIEW3D_AREA_SPHERES_HEIGHT
[VIEW3D] The height of the areas' spheres.
int random_seed
0 means randomize, use any other value to have repetitive experiments.
int LOG_FREQUENCY
[LOGGING] One SLAM iteration out of "LOGGING_logFrequency", a log file will be generated.
mrpt::maps::TSetOfMetricMapInitializers defaultMapsInitializers
The default set of maps to be created in each particle.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) 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
mrpt::slam::CIncrementalMapPartitioner::TOptions AA_options
[AA] The options for the partitioning algorithm
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.
float VIEW3D_AREA_SPHERES_RADIUS
[VIEW3D] The radius of the areas' spheres.
vector_string TLC_detectors
A list of topological loop-closure detectors to use: can be one or more from this list: 'gridmaps': O...
float SLAM_MIN_DIST_BETWEEN_OBS
[LSLAM] Minimum distance (and heading) difference between observations inserted in the map.
float MIN_ODOMETRY_STD_XY
[LSLAM] Minimum uncertainty (1 sigma, meters) in x and y for odometry increments (Default=0)
float MIN_ODOMETRY_STD_PHI
[LSLAM] Minimum uncertainty (1 sigma, rads) in phi for odometry increments (Default=0)
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...
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.
Configuration of the algorithm:
This structure contains the information needed to interface the threads API on each platform:
A wrapper class for pointers that can be safely copied with "=" operator without problems.