48 resMsg->hypothesisID = LMH_ID;
51 newID != newPoseIDs.end(); ++newID)
56 static_cast<unsigned>(*newID));
61 mrpt::make_aligned_shared<CPose3DPDFParticles>();
69 LMH->
m_SFs.find(*newID);
82 obj->m_options.AA_options;
91 vector<std::vector<uint32_t>> partitions;
98 resMsg->partitions.resize(partitions.size());
101 for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin();
102 itSrc != partitions.end(); itSrc++, itDest++)
104 itDest->resize(itSrc->size());
107 for (it1 = itSrc->begin(), it2 = itDest->begin(); it1 != itSrc->end();
112 resMsg->dumpToConsole();
119 void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole()
const 122 "Hypo ID: %i has %i partitions:\n", (
int)hypothesisID,
123 (
int)partitions.size());
126 it != partitions.end(); ++it)
void updatePartitions(std::vector< std::vector< uint32_t >> &partitions)
Recalculate the map/graph partitions.
TOptions options
Algorithm parameters.
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.
void printf_vector(const char *fmt, const std::vector< T > &V)
Prints a vector in the format [A,B,C,...] to std::cout, and the fmt string for each vector element...
uint32_t addMapFrame(const mrpt::obs::CSensoryFrame &frame, const mrpt::poses::CPose3DPDF &robotPose3D)
Insert a new keyframe to the graph.
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
mrpt::slam::CIncrementalMapPartitioner partitioner
std::vector< TPoseID > TPoseIDList
#define ASSERT_(f)
Defines an assertion mechanism.
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
std::mutex lock
CS to access the entire struct.
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.
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
const Scalar * const_iterator
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...