9#ifndef CLocalMetricHypothesis_H
10#define CLocalMetricHypothesis_H
27 namespace poses {
class CPose3DPDFParticles; }
49 metricMaps( mapsInitializers ),
72 public
mrpt::utils::CSerializable
95 std::map<TPoseID,mrpt::obs::CSensoryFrame>
m_SFs;
131 void getPathParticles( std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &outList )
const;
190 bool eraseSFsFromLMH =
false );
#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...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL)
TMapPoseID2Pose3D robotPoses
mrpt::maps::CMultiMetricMap metricMaps
virtual ~CLSLAMParticleData()
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses.
void dumpAsText(utils::CStringList &st) const
Describes the LMH in text.
CLocalMetricHypothesis(CHMTSLAM *parent=NULL)
Constructor (Default param only used from STL classes)
mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx)
Returns the i'th particle hypothesis for the current robot pose.
void getRelativePose(const TPoseID &reference, const TPoseID &pose, mrpt::poses::CPose3DPDFParticles &outPDF) const
Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).
void getAs3DScene(mrpt::opengl::CSetOfObjectsPtr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph,...
double m_log_w
Log-weight of this hypothesis.
synch::CCriticalSection m_lock
Critical section for threads signaling they are working with the LMH.
void getPathParticles(std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &outList) const
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles...
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
~CLocalMetricHypothesis()
Destructor.
void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID)
Removes a given area from the LMH:
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void updateAreaFromLMH(const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false)
The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are...
mrpt::poses::StdVector_CPose2D m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void changeCoordinateOrigin(const TPoseID &newOrigin)
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric map...
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
std::vector< std::map< TPoseID, double > > m_log_w_metric_history
The historic log-weights of the metric observations inserted in this LMH, for each particle.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
void getPoseParticles(const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF) const
Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles...
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
This class stores any customizable set of metric maps.
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.
Represents a probabilistic 2D movement of the robot mobile base.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
This class provides simple critical sections functionality.
The virtual base class which provides a unified interface for all persistent objects in MRPT.
A class for storing a list of text lines.
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
std::set< CHMHMapNode::TNodeID > TNodeIDSet
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
std::vector< TPoseID > TPoseIDList
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
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.
synch::CCriticalSection lock
CS to access the entire struct.
unsigned int pose2idx(const TPoseID &id) const
Uses idx2pose to perform inverse searches.
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
mrpt::slam::CIncrementalMapPartitioner partitioner
A wrapper class for pointers that can be safely copied with "=" operator without problems.