36 void CHMTSLAM::perform_TLC(
48 logFmt(mrpt::utils::LVL_DEBUG,
"[perform_TLC] TLC of areas: %u <-> %u \n",(
unsigned)Ai, (
unsigned)Ae );
117 m_map.getNodeByID(Ai)->getArcs(lstArcs,LMH.
m_ID);
128 m_map.getNodeByID(Ai)->getArcs(lstArcs);
132 m_map.getNodeByID(Ai).clear();
144 const CPose3D AeRefInLMH = AiRef + Apose_ie;
151 const TPoseID poseId = it->first;
157 for (
size_t i=0;i<pi.
pdf.
size();i++)
161 LMH.
m_particles[i].d->robotPoses[poseId] = AeRefInLMH +
p;
A namespace of pseudo-random numbers genrators of diferent distributions.
A class acquiring a CCriticalSection at its constructor, and releasing it at destructor.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose3D mean
The mean value.
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
mrpt::poses::CPose3DPDFParticles pdf
The robot pose PDF.
const Scalar * const_iterator
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
CParticleList m_particles
The array of particles.
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class stores a set of r...
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
void rebuildMetricMaps()
Rebuild the metric maps of all particles from the observations and their estimated poses...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::obs::CSensoryFrame sf
The observations.
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Information kept for each robot pose used in CRobotPosesGraph.
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 ref...
synch::CCriticalSection lock
CS to access the entire struct.
#define NODE_ANNOTATION_POSES_GRAPH
A class for storing a sequence of arcs (a path).
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...
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.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.