9 #ifndef CLocalMetricHypothesis_H 10 #define CLocalMetricHypothesis_H 27 namespace poses {
class CPose3DPDFParticles; }
49 metricMaps( mapsInitializers ),
72 public
mrpt::utils::CSerializable
82 CLocalMetricHypothesis(
CHMTSLAM * parent = NULL );
86 ~CLocalMetricHypothesis();
88 synch::CCriticalSection m_lock;
95 std::map<TPoseID,
mrpt::obs::CSensoryFrame> m_SFs;
100 std::vector<
std::map<TPoseID,
double> > m_log_w_metric_history;
103 mrpt::obs::CActionRobotMovement2D m_accumRobotMovement;
104 bool m_accumRobotMovementIsValid;
113 unsigned int pose2idx(
const TPoseID &
id)
const;
121 void getAs3DScene( mrpt::opengl::CSetOfObjectsPtr &objs )
const;
126 void getMeans( TMapPoseID2Pose3D &outList )
const;
131 void getPathParticles( std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &outList )
const;
141 void getRelativePose(
152 void changeCoordinateOrigin(
const TPoseID &newOrigin );
155 void rebuildMetricMaps();
162 void clearRobotPoses();
188 void updateAreaFromLMH(
190 bool eraseSFsFromLMH =
false );
201 void prediction_and_update_pfAuxiliaryPFOptimal(
207 void prediction_and_update_pfOptimalProposal(
221 mutable std::vector<double> m_maxLikelihood;
227 mutable unsigned int m_movementDrawsIdx;
This class provides simple critical sections functionality.
Implements a 2D local SLAM method based on a RBPF over an occupancy grid map.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
TMapPoseID2Pose3D robotPoses
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
mrpt::slam::CIncrementalMapPartitioner partitioner
Declares a class for storing a collection of robot actions.
virtual ~CLSLAMParticleData()
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
A class for storing a list of text lines.
#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...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
uint64_t TNodeID
The type for node IDs in graphs of different types.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
std::vector< TPoseID > TPoseIDList
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
This class can be used to make partitions on a map/graph build from observations taken at some poses/...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
The configuration of a particle filter.
std::set< CHMHMapNode::TNodeID > TNodeIDSet
This class stores any customizable set of metric maps.
synch::CCriticalSection lock
CS to access the entire struct.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
A class for representing a node in a hierarchical, multi-hypothesis map.
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
mrpt::maps::CMultiMetricMap metricMaps