51 resMsg->hypothesisID = LMH_ID;
56 obj->logFmt(mrpt::utils::LVL_DEBUG,
"[thread_AA] Processing new pose ID: %u\n", static_cast<unsigned>( *newID ) );
60 CPose3DPDFParticlesPtr posePDF = CPose3DPDFParticles::Create();
87 vector< vector_uint > partitions;
96 resMsg->partitions.resize( partitions.size() );
99 for (itDest = resMsg->partitions.begin(), itSrc = partitions.begin(); itSrc!=partitions.end(); itSrc++, itDest++)
101 itDest->resize( itSrc->size() );
104 for (it1=itSrc->begin(), it2=itDest->begin(); it1!=itSrc->end(); it1++,it2++)
108 resMsg->dumpToConsole( );
117 void CHMTSLAM::TMessageLSLAMfromAA::dumpToConsole( )
const 119 cout <<
format(
"Hypo ID: %i has %i partitions:\n",(
int)hypothesisID,(
int)partitions.size());
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.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
void markAllNodesForReconsideration()
Mark all nodes for reconsideration in the next call to "updatePartitions", instead of considering jus...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
std::shared_ptr< TMessageLSLAMfromAA > TMessageLSLAMfromAAPtr
const Scalar * const_iterator
GLsizei GLsizei GLuint * obj
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
mrpt::slam::CIncrementalMapPartitioner partitioner
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
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...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::slam::CIncrementalMapPartitioner::TOptions options
This namespace provides multitask, synchronization utilities.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning m_robotPosesGraph
std::map< uint32_t, TPoseID > idx2pose
For the poses in "partitioner".
std::vector< TPoseID > TPoseIDList
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...
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
synch::CCriticalSection lock
CS to access the entire struct.
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...