Go to the documentation of this file.
29 class CPose3DPDFParticles;
68 CLocalMetricHypothesis,
69 mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList>,
104 std::map<TPoseID, mrpt::obs::CSensoryFrame>
m_SFs;
164 std::map<TPoseID, mrpt::poses::CPose3DPDFParticles>& outList)
const;
183 void dumpAsText(std::vector<std::string>& st)
const;
CLocalMetricHypothesis(CHMTSLAM *parent=nullptr)
Constructor (Default param only used from STL classes)
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
TMapPoseID2Pose3D robotPoses
std::set< CHMHMapNode::TNodeID > TNodeIDSet
TRobotPosesPartitioning(const TRobotPosesPartitioning &o)
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...
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
mrpt::maps::CMultiMetricMap metricMaps
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
ThreadLocks(const ThreadLocks &)
const mrpt::poses::CPose3D * getCurrentPose(size_t particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
struct mrpt::hmtslam::CLocalMetricHypothesis::ThreadLocks threadLocks
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...
mrpt::slam::CIncrementalMapPartitioner partitioner
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
CLSLAMParticleData(const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=nullptr)
Declares a class for storing a collection of robot actions.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
Represents a probabilistic 2D movement of the robot mobile base.
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...
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
TRobotPosesPartitioning()=default
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
void removeAreaFromLMH(const CHMHMapNode::TNodeID areaID)
Removes a given area from the LMH:
void dumpAsText(std::vector< std::string > &st) const
Describes the LMH in text.
This class stores any customizable set of metric maps.
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void changeCoordinateOrigin(const TPoseID &newOrigin)
Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric map...
The virtual base class which provides a unified interface for all persistent objects in MRPT.
This template class declares the array of particles and its internal data, managing some memory-relat...
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses.
TNodeIDSet m_neighbors
The list of all areas sourronding the current one (this includes the current area itself).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
std::mutex lock
CS to access the entire struct.
void clearRobotPoses()
Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their esti...
unsigned int pose2idx(const TPoseID &id) const
Uses idx2pose to perform inverse searches.
double m_log_w
Log-weight of this hypothesis.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Finds partitions in metric maps based on N-cut graph partition theory.
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 prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
std::vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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.
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::vector< TPoseID > TPoseIDList
void getAs3DScene(mrpt::opengl::CSetOfObjects::Ptr &objs) const
Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph,...
TNodeIDList m_areasPendingTBI
The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to s...
std::vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
The configuration of a particle filter.
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
~CLocalMetricHypothesis() override
A wrapper class for pointers that can be safely copied with "=" operator without problems.
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020 | |