MRPT  1.9.9
List of all members | Public Types | Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | Friends
mrpt::hmtslam::CHierarchicalMHMap Class Reference

Detailed Description

The most high level class for storing hybrid, multi-hypothesis maps in a graph-based model.

This class is used within the HMT-SLAM implementation in CHMTSLAM.

See also
CHMTSLAM, CHMHMapArc, CHMHMapNode, CHierarchicalMHMapPartition

Definition at line 26 of file CHierarchicalMHMap.h.

#include <mrpt/hmtslam/CHierarchicalMHMap.h>

Inheritance diagram for mrpt::hmtslam::CHierarchicalMHMap:

Public Types

using iterator = TNodeList::iterator
 
using const_iterator = TNodeList::const_iterator
 
using TNodeIDsList = std::vector< CHMHMapNode::TNodeID >
 A type that reprensents a sequence of node IDs. More...
 

Public Member Functions

 CHierarchicalMHMap ()
 Default constructor. More...
 
void dumpAsXMLfile (std::string fileName) const
 Destructor. More...
 
void loadFromXMLfile (std::string fileName)
 Load a graph from a XML file. More...
 
 ~CHierarchicalMHMap () override
 
void clear ()
 Erase all the contents of map (It delete all nodes/arcs objects) More...
 
virtual mxArraywriteToMatlab () const
 Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class. More...
 
const_iterator begin () const
 Returns an iterator to the first node in the graph. More...
 
iterator begin ()
 Returns an iterator to the first node in the graph. More...
 
const_iterator end () const
 Returns an iterator to the end of the list of nodes in the graph. More...
 
iterator end ()
 Returns an iterator to the end of the list of nodes in the graph. More...
 
size_t nodeCount () const
 Returns the number of nodes in the partition: More...
 
size_t arcCount () const
 Returns the number of arcs in the partition: More...
 
CHMHMapNode::Ptr getFirstNode ()
 Returns the first node in the graph, or nullptr if it does not exist. More...
 
CHMHMapNode::Ptr getNodeByID (CHMHMapNode::TNodeID id)
 Returns the node with the given ID, or nullptr if it does not exist. More...
 
const CHMHMapNode::Ptr getNodeByID (CHMHMapNode::TNodeID id) const
 Returns the node with the given ID, or nullptr if it does not exist. More...
 
CHMHMapNode::Ptr getNodeByLabel (const std::string &label, const THypothesisID &hypothesisID)
 Returns the node with the given label (case insensitive) for some given hypothesis ID, or nullptr if it does not exist. More...
 
const CHMHMapNode::Ptr getNodeByLabel (const std::string &label, const THypothesisID &hypothesisID) const
 Returns the node with the given label (case insensitive) for some given hypothesis ID, or nullptr if it does not exist. More...
 
void saveAreasDiagramForMATLAB (const std::string &filName, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID) const
 Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc) More...
 
void saveAreasDiagramWithEllipsedForMATLAB (const std::string &filName, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, float uncertaintyExagerationFactor=1.0f, bool drawArcs=false, unsigned int numberOfIterationsForOptimalGlobalPoses=4) const
 Saves a MATLAB script that represents graphically the nodes with type="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference, and drawing the ellipses of the localization uncertainty for each node. More...
 
void saveGlobalMapForMATLAB (const std::string &filName, const THypothesisID &hypothesisID, const CHMHMapNode::TNodeID &idReferenceNode) const
 Saves a MATLAB script that represents graphically the reconstructed "global map" ADDITIONAL NOTES: More...
 
void findPathBetweenNodes (const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, const THypothesisID &hypothesisID, TArcList &out_path, bool direction=false) const
 The Dijkstra algorithm for finding the shortest path between a pair of nodes. More...
 
void computeCoordinatesTransformationBetweenNodes (const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, mrpt::poses::CPose3DPDFParticles &posePDF, const THypothesisID &hypothesisID, unsigned int particlesCount=100, float additionalNoiseXYratio=0.02, float additionalNoisePhiRad=mrpt::DEG2RAD(0.1)) const
 Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes. More...
 
float computeMatchProbabilityBetweenNodes (const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, float &maxMatchProb, mrpt::poses::CPose3DPDFSOG &estimatedRelativePose, const THypothesisID &hypothesisID, unsigned int monteCarloSamplesPose=300)
 Computes the probability [0,1] of two areas' gridmaps to "match" (loop closure), according to the grid maps and pose uncertainty from information in arcs (uses a Monte Carlo aproximation) If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose". More...
 
void findArcsBetweenNodes (const CHMHMapNode::TNodeID &node1, const CHMHMapNode::TNodeID &node2, const THypothesisID &hypothesisID, TArcList &out_listArcs) const
 Returns all the arcs between a pair of nodes: More...
 
void findArcsOfTypeBetweenNodes (const CHMHMapNode::TNodeID &node1id, const CHMHMapNode::TNodeID &node2id, const THypothesisID &hypothesisID, const std::string &arcType, TArcList &ret) const
 Returns the arcs between a pair of nodes of a given type. More...
 
CHMHMapArc::Ptr findArcOfTypeBetweenNodes (const CHMHMapNode::TNodeID &node1id, const CHMHMapNode::TNodeID &node2id, const THypothesisID &hypothesisID, const std::string &arcType, bool &isInverted) const
 Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction. More...
 
bool areNodesNeightbour (const CHMHMapNode::TNodeID &node1, const CHMHMapNode::TNodeID &node2, const THypothesisID &hypothesisID, const char *requiredAnnotation=nullptr) const
 Returns whether two nodes are "neightbour", i.e. More...
 
void computeGloballyConsistentNodeCoordinates (std::map< CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian > &nodePoses, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, const unsigned int &numberOfIterations=2) const
 This methods implements a Lu&Milios-like globally optimal estimation for the global coordinates of all the nodes in the graph according to all available arcs with relative pose information. More...
 
void getAs3DScene (mrpt::opengl::COpenGLScene &outScene, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, const unsigned int &numberOfIterationsForOptimalGlobalPoses=5, bool showRobotPoseIDs=true) const
 Returns a 3D scene reconstruction of the hierarchical map. More...
 
void dumpAsText (std::vector< std::string > &s) const
 Return a textual description of the whole graph. More...
 
double computeOverlapProbabilityBetweenNodes (const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, const THypothesisID &hypothesisID, size_t monteCarloSamples=100, const float margin_to_substract=6) const
 Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation. More...
 
RTTI classes and functions for polymorphic hierarchies
mrpt::rtti::CObject::Ptr duplicateGetSmartPtr () const
 Makes a deep copy of the object and returns a smart pointer to it. More...
 

Static Public Member Functions

static std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_colors ()
 Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor. More...
 
static std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & logging_levels_to_names ()
 Map from VerbosityLevels to their corresponding names. More...
 

Protected Member Functions

void onNodeAddition (CHMHMapNode::Ptr &node)
 Event handler to be called just after a node has being created: it will be added to the internal list. More...
 
void onArcAddition (CHMHMapArc::Ptr &arc)
 Event handler to be called just after an arc has being created: it will be added to the internal list. More...
 
void onNodeDestruction (CHMHMapNode *node)
 Event handler to be called just before a node is being destroyed: it will be removed from the internal list. More...
 
void onArcDestruction (CHMHMapArc *arc)
 Event handler to be called just before an arc is being destroyed: it will be removed from the internal list. More...
 
CSerializable virtual methods
uint8_t serializeGetVersion () const override
 Must return the current versioning number of the object. More...
 
void serializeTo (mrpt::serialization::CArchive &out) const override
 Pure virtual method for writing (serializing) to an abstract archive. More...
 
void serializeFrom (mrpt::serialization::CArchive &in, uint8_t serial_version) override
 Pure virtual method for reading (deserializing) from an abstract archive. More...
 
CSerializable virtual methods
virtual void serializeTo (CSchemeArchiveBase &out) const
 Virtual method for writing (serializing) to an abstract schema based archive. More...
 
virtual void serializeFrom (CSchemeArchiveBase &in)
 Virtual method for reading (deserializing) from an abstract schema based archive. More...
 

Protected Attributes

TNodeList m_nodes
 The internal list of nodes and arcs in the whole hierarchical model. More...
 
TArcList m_arcs
 
VerbosityLevel m_min_verbosity_level {LVL_INFO}
 Provided messages with VerbosityLevel smaller than this value shall be ignored. More...
 

Friends

class CHMHMapArc
 
class CHMHMapNode
 

RTTI stuff

using Ptr = std::shared_ptr< mrpt::hmtslam ::CHierarchicalMHMap >
 
using ConstPtr = std::shared_ptr< const mrpt::hmtslam ::CHierarchicalMHMap >
 
using UniquePtr = std::unique_ptr< mrpt::hmtslam ::CHierarchicalMHMap >
 
using ConstUniquePtr = std::unique_ptr< const mrpt::hmtslam ::CHierarchicalMHMap >
 
static const mrpt::rtti::TRuntimeClassId runtimeClassId
 
static constexpr const char * className = "mrpt::hmtslam" "::" "CHierarchicalMHMap"
 
static const mrpt::rtti::TRuntimeClassId_GetBaseClass ()
 
static constexpr auto getClassName ()
 
static const mrpt::rtti::TRuntimeClassIdGetRuntimeClassIdStatic ()
 
static std::shared_ptr< CObjectCreateObject ()
 
template<typename... Args>
static Ptr Create (Args &&... args)
 
template<typename Alloc , typename... Args>
static Ptr CreateAlloc (const Alloc &alloc, Args &&... args)
 
template<typename... Args>
static UniquePtr CreateUnique (Args &&... args)
 
virtual const mrpt::rtti::TRuntimeClassIdGetRuntimeClass () const override
 Returns information about the class of an object in runtime. More...
 
virtual mrpt::rtti::CObjectclone () const override
 Returns a deep copy (clone) of the object, indepently of its class. More...
 

Logging methods

void logStr (const VerbosityLevel level, std::string_view msg_str) const
 Main method to add the specified message string to the logger. More...
 
void logFmt (const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
 Alternative logging method, which mimics the printf behavior. More...
 
void void logCond (const VerbosityLevel level, bool cond, const std::string &msg_str) const
 Log the given message only if the condition is satisfied. More...
 
void setLoggerName (const std::string &name)
 Set the name of the COutputLogger instance. More...
 
std::string getLoggerName () const
 Return the name of the COutputLogger instance. More...
 
void setMinLoggingLevel (const VerbosityLevel level)
 Set the minimum logging level for which the incoming logs are going to be taken into account. More...
 
void setVerbosityLevel (const VerbosityLevel level)
 alias of setMinLoggingLevel() More...
 
VerbosityLevel getMinLoggingLevel () const
 
bool isLoggingLevelVisible (VerbosityLevel level) const
 
void getLogAsString (std::string &log_contents) const
 Fill the provided string with the contents of the logger's history in std::string representation. More...
 
std::string getLogAsString () const
 Get the history of COutputLogger instance in a string representation. More...
 
void writeLogToFile (const std::string *fname_in=nullptr) const
 Write the contents of the COutputLogger instance to an external file. More...
 
void dumpLogToConsole () const
 Dump the current contents of the COutputLogger instance in the terminal window. More...
 
std::string getLoggerLastMsg () const
 Return the last Tmsg instance registered in the logger history. More...
 
void getLoggerLastMsg (std::string &msg_str) const
 Fill inputtted string with the contents of the last message in history. More...
 
void loggerReset ()
 Reset the contents of the logger instance. More...
 
void logRegisterCallback (output_logger_callback_t userFunc)
 
bool logDeregisterCallback (output_logger_callback_t userFunc)
 
bool logging_enable_console_output {true}
 [Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically. More...
 
bool logging_enable_keep_record {false}
 [Default=false] Enables storing all messages into an internal list. More...
 

Member Typedef Documentation

◆ const_iterator

using mrpt::hmtslam::CHierarchicalMapMHPartition::const_iterator = TNodeList::const_iterator
inherited

Definition at line 48 of file CHierarchicalMapMHPartition.h.

◆ ConstPtr

Definition at line 32 of file CHierarchicalMHMap.h.

◆ ConstUniquePtr

using mrpt::hmtslam::CHierarchicalMHMap::ConstUniquePtr = std::unique_ptr<const mrpt::hmtslam :: CHierarchicalMHMap >

Definition at line 32 of file CHierarchicalMHMap.h.

◆ iterator

using mrpt::hmtslam::CHierarchicalMapMHPartition::iterator = TNodeList::iterator
inherited

Definition at line 47 of file CHierarchicalMapMHPartition.h.

◆ Ptr

A type for the associated smart pointer

Definition at line 32 of file CHierarchicalMHMap.h.

◆ TNodeIDsList

A type that reprensents a sequence of node IDs.

Definition at line 61 of file CHierarchicalMapMHPartition.h.

◆ UniquePtr

using mrpt::hmtslam::CHierarchicalMHMap::UniquePtr = std::unique_ptr< mrpt::hmtslam :: CHierarchicalMHMap >

Definition at line 32 of file CHierarchicalMHMap.h.

Constructor & Destructor Documentation

◆ CHierarchicalMHMap()

CHierarchicalMHMap::CHierarchicalMHMap ( )
default

Default constructor.

◆ ~CHierarchicalMHMap()

CHierarchicalMHMap::~CHierarchicalMHMap ( )
override

Definition at line 32 of file CHierarchicalMHMap.cpp.

References mrpt::containers::clear().

Here is the call graph for this function:

Member Function Documentation

◆ _GetBaseClass()

static const mrpt::rtti::TRuntimeClassId* mrpt::hmtslam::CHierarchicalMHMap::_GetBaseClass ( )
staticprotected

◆ arcCount()

size_t CHierarchicalMapMHPartition::arcCount ( ) const
inherited

Returns the number of arcs in the partition:

Definition at line 45 of file CHierarchicalMapMHPartition.cpp.

◆ areNodesNeightbour()

bool CHierarchicalMapMHPartition::areNodesNeightbour ( const CHMHMapNode::TNodeID node1,
const CHMHMapNode::TNodeID node2,
const THypothesisID hypothesisID,
const char *  requiredAnnotation = nullptr 
) const
inherited

Returns whether two nodes are "neightbour", i.e.

have a direct arc between them

Definition at line 1059 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, and MRPT_START.

◆ begin() [1/2]

const_iterator mrpt::hmtslam::CHierarchicalMapMHPartition::begin ( ) const
inlineinherited

Returns an iterator to the first node in the graph.

Definition at line 51 of file CHierarchicalMapMHPartition.h.

References mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes.

◆ begin() [2/2]

iterator mrpt::hmtslam::CHierarchicalMapMHPartition::begin ( )
inlineinherited

Returns an iterator to the first node in the graph.

Definition at line 53 of file CHierarchicalMapMHPartition.h.

References mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes.

◆ clear()

void CHierarchicalMHMap::clear ( )

Erase all the contents of map (It delete all nodes/arcs objects)

Definition at line 36 of file CHierarchicalMHMap.cpp.

◆ clone()

virtual mrpt::rtti::CObject* mrpt::hmtslam::CHierarchicalMHMap::clone ( ) const
overridevirtual

Returns a deep copy (clone) of the object, indepently of its class.

Implements mrpt::rtti::CObject.

◆ computeCoordinatesTransformationBetweenNodes()

void CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes ( const CHMHMapNode::TNodeID nodeFrom,
const CHMHMapNode::TNodeID nodeTo,
mrpt::poses::CPose3DPDFParticles posePDF,
const THypothesisID hypothesisID,
unsigned int  particlesCount = 100,
float  additionalNoiseXYratio = 0.02,
float  additionalNoisePhiRad = mrpt::DEG2RAD(0.1) 
) const
inherited

Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.

Exceptions
std::exceptionIf there is not enough information in arcs to compute the PDF
See also
computeGloballyConsistentNodeCoordinates

Definition at line 855 of file CHierarchicalMapMHPartition.cpp.

References ARC_ANNOTATION_DELTA, ARC_ANNOTATION_DELTA_SRC_POSEID, ARC_ANNOTATION_DELTA_TRG_POSEID, ASSERT_, mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DPDFGaussian::drawManySamples(), mrpt::random::getRandomGenerator(), mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, MRPT_END, MRPT_START, NODE_ANNOTATION_REF_POSEID, mrpt::bayes::CParticleFilterDataImpl< Derived, particle_list_t >::normalizeWeights(), mrpt::poses::CPose3DPDFParticles::resetDeterministic(), and mrpt::math::size().

Here is the call graph for this function:

◆ computeGloballyConsistentNodeCoordinates()

void CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates ( std::map< CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian > &  nodePoses,
const CHMHMapNode::TNodeID idReferenceNode,
const THypothesisID hypothesisID,
const unsigned int &  numberOfIterations = 2 
) const
inherited

This methods implements a Lu&Milios-like globally optimal estimation for the global coordinates of all the nodes in the graph according to all available arcs with relative pose information.

Global coordinates will be computed relative to the node "idReferenceNode".

Exceptions
std::exceptionIf there is any node without a pose arc, invalid (non invertible) matrixes, etc...
See also
computeCoordinatesTransformationBetweenNodes

Definition at line 1295 of file CHierarchicalMapMHPartition.cpp.

References ARC_ANNOTATION_DELTA, mrpt::poses::CPose3DPDFGaussianInf::copyFrom(), mrpt::poses::CPose3DPDFGaussian::cov, mrpt::graphs::CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::dijkstra_nodes_estimate(), mrpt::graphs::CDirectedGraph< TYPE_EDGES, EDGE_ANNOTATIONS >::insertEdgeAtEnd(), mrpt::poses::CPose3DPDFGaussian::mean, MRPT_END, MRPT_START, mrpt::graphs::CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::nodes, mrpt::graphslam::optimize_graph_spa_levmarq(), mrpt::graphs::CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::root, and mrpt::math::MatrixBase< Scalar, Derived >::setIdentity().

Here is the call graph for this function:

◆ computeMatchProbabilityBetweenNodes()

float CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes ( const CHMHMapNode::TNodeID nodeFrom,
const CHMHMapNode::TNodeID nodeTo,
float &  maxMatchProb,
mrpt::poses::CPose3DPDFSOG estimatedRelativePose,
const THypothesisID hypothesisID,
unsigned int  monteCarloSamplesPose = 300 
)
inherited

Computes the probability [0,1] of two areas' gridmaps to "match" (loop closure), according to the grid maps and pose uncertainty from information in arcs (uses a Monte Carlo aproximation) If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".

Definition at line 984 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, MRPT_START, and THROW_EXCEPTION.

◆ computeOverlapProbabilityBetweenNodes()

double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes ( const CHMHMapNode::TNodeID nodeFrom,
const CHMHMapNode::TNodeID nodeTo,
const THypothesisID hypothesisID,
size_t  monteCarloSamples = 100,
const float  margin_to_substract = 6 
) const
inherited

Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.

Exceptions
std::exceptionIf there is not enough information in arcs, etc...
Parameters
margin_to_substractIn meters, the area of each gridmap is "eroded" this amount to compensate the area in excess usually found in gridmaps.

Definition at line 1541 of file CHierarchicalMapMHPartition.cpp.

References ASSERT_, mrpt::bayes::CParticleFilterData< T, STORAGE >::m_particles, MRPT_END, MRPT_START, NODE_ANNOTATION_METRIC_MAPS, and mrpt::math::RectanglesIntersection().

Here is the call graph for this function:

◆ Create()

template<typename... Args>
static Ptr mrpt::hmtslam::CHierarchicalMHMap::Create ( Args &&...  args)
inlinestatic

Definition at line 32 of file CHierarchicalMHMap.h.

◆ CreateAlloc()

template<typename Alloc , typename... Args>
static Ptr mrpt::hmtslam::CHierarchicalMHMap::CreateAlloc ( const Alloc &  alloc,
Args &&...  args 
)
inlinestatic

Definition at line 32 of file CHierarchicalMHMap.h.

◆ CreateObject()

static std::shared_ptr<CObject> mrpt::hmtslam::CHierarchicalMHMap::CreateObject ( )
static

◆ CreateUnique()

template<typename... Args>
static UniquePtr mrpt::hmtslam::CHierarchicalMHMap::CreateUnique ( Args &&...  args)
inlinestatic

Definition at line 32 of file CHierarchicalMHMap.h.

◆ dumpAsText()

void CHierarchicalMapMHPartition::dumpAsText ( std::vector< std::string > &  s) const
inherited

◆ dumpAsXMLfile()

void CHierarchicalMHMap::dumpAsXMLfile ( std::string  fileName) const

Destructor.

Save the whole graph as a XML file

Definition at line 160 of file CHierarchicalMHMap.cpp.

References THROW_EXCEPTION.

◆ dumpLogToConsole()

void COutputLogger::dumpLogToConsole ( ) const
inherited

Dump the current contents of the COutputLogger instance in the terminal window.

See also
writeToFile

Definition at line 190 of file COutputLogger.cpp.

◆ duplicateGetSmartPtr()

mrpt::rtti::CObject::Ptr CObject::duplicateGetSmartPtr ( ) const
inlineinherited

Makes a deep copy of the object and returns a smart pointer to it.

Definition at line 204 of file CObject.h.

References mrpt::rtti::CObject::clone().

Referenced by mrpt::obs::CRawlog::insert().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ end() [1/2]

const_iterator mrpt::hmtslam::CHierarchicalMapMHPartition::end ( ) const
inlineinherited

Returns an iterator to the end of the list of nodes in the graph.

Definition at line 55 of file CHierarchicalMapMHPartition.h.

References mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes.

◆ end() [2/2]

iterator mrpt::hmtslam::CHierarchicalMapMHPartition::end ( )
inlineinherited

Returns an iterator to the end of the list of nodes in the graph.

Definition at line 57 of file CHierarchicalMapMHPartition.h.

References mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes.

◆ findArcOfTypeBetweenNodes()

CHMHMapArc::Ptr CHierarchicalMapMHPartition::findArcOfTypeBetweenNodes ( const CHMHMapNode::TNodeID node1id,
const CHMHMapNode::TNodeID node2id,
const THypothesisID hypothesisID,
const std::string &  arcType,
bool &  isInverted 
) const
inherited

Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction.

Returns
The arc, or nullptr if not found.

Definition at line 1507 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, and MRPT_START.

◆ findArcsBetweenNodes()

void CHierarchicalMapMHPartition::findArcsBetweenNodes ( const CHMHMapNode::TNodeID node1,
const CHMHMapNode::TNodeID node2,
const THypothesisID hypothesisID,
TArcList out_listArcs 
) const
inherited

Returns all the arcs between a pair of nodes:

Definition at line 1000 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, and MRPT_START.

◆ findArcsOfTypeBetweenNodes()

void CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes ( const CHMHMapNode::TNodeID node1id,
const CHMHMapNode::TNodeID node2id,
const THypothesisID hypothesisID,
const std::string &  arcType,
TArcList ret 
) const
inherited

Returns the arcs between a pair of nodes of a given type.

Definition at line 1029 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, and MRPT_START.

◆ findPathBetweenNodes()

void CHierarchicalMapMHPartition::findPathBetweenNodes ( const CHMHMapNode::TNodeID nodeFrom,
const CHMHMapNode::TNodeID nodeTo,
const THypothesisID hypothesisID,
TArcList out_path,
bool  direction = false 
) const
inherited

The Dijkstra algorithm for finding the shortest path between a pair of nodes.

Returns
The sequence of arcs connecting the nodes.It will be empty if no path is found or when the starting and ending node coincide.

Definition at line 726 of file CHierarchicalMapMHPartition.cpp.

References ASSERT_, ASSERTMSG_, mrpt::format(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ getAs3DScene()

void CHierarchicalMapMHPartition::getAs3DScene ( mrpt::opengl::COpenGLScene outScene,
const CHMHMapNode::TNodeID idReferenceNode,
const THypothesisID hypothesisID,
const unsigned int &  numberOfIterationsForOptimalGlobalPoses = 5,
bool  showRobotPoseIDs = true 
) const
inherited

Returns a 3D scene reconstruction of the hierarchical map.

See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"

Definition at line 1094 of file CHierarchicalMapMHPartition.cpp.

References mrpt::poses::CPose3D::addComponents(), mrpt::opengl::COpenGLScene::clear(), mrpt::opengl::CSimpleLine::Create(), mrpt::opengl::CSetOfObjects::Create(), mrpt::opengl::CSphere::Create(), mrpt::opengl::CGridPlaneXY::Create(), mrpt::opengl::CDisk::Create(), mrpt::opengl::CText::Create(), mrpt::format(), mrpt::opengl::COpenGLScene::insert(), MRPT_END, MRPT_START, NODE_ANNOTATION_METRIC_MAPS, NODE_ANNOTATION_POSES_GRAPH, mrpt::poses::CPose3D::normalizeAngles(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y().

Here is the call graph for this function:

◆ getClassName()

static constexpr auto mrpt::hmtslam::CHierarchicalMHMap::getClassName ( )
inlinestatic

Definition at line 32 of file CHierarchicalMHMap.h.

◆ getFirstNode()

CHMHMapNode::Ptr CHierarchicalMapMHPartition::getFirstNode ( )
inherited

Returns the first node in the graph, or nullptr if it does not exist.

Returns
A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"

Definition at line 117 of file CHierarchicalMapMHPartition.cpp.

◆ getLogAsString() [1/2]

void COutputLogger::getLogAsString ( std::string &  log_contents) const
inherited

Fill the provided string with the contents of the logger's history in std::string representation.

Definition at line 154 of file COutputLogger.cpp.

◆ getLogAsString() [2/2]

std::string COutputLogger::getLogAsString ( ) const
inherited

Get the history of COutputLogger instance in a string representation.

Definition at line 159 of file COutputLogger.cpp.

Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport().

Here is the caller graph for this function:

◆ getLoggerLastMsg() [1/2]

std::string COutputLogger::getLoggerLastMsg ( ) const
inherited

Return the last Tmsg instance registered in the logger history.

Definition at line 195 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::getAsString().

Here is the call graph for this function:

◆ getLoggerLastMsg() [2/2]

void COutputLogger::getLoggerLastMsg ( std::string &  msg_str) const
inherited

Fill inputtted string with the contents of the last message in history.

Definition at line 201 of file COutputLogger.cpp.

◆ getLoggerName()

std::string COutputLogger::getLoggerName ( ) const
inherited

Return the name of the COutputLogger instance.

See also
setLoggerName

Definition at line 143 of file COutputLogger.cpp.

◆ getMinLoggingLevel()

VerbosityLevel mrpt::system::COutputLogger::getMinLoggingLevel ( ) const
inlineinherited

◆ getNodeByID() [1/2]

CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByID ( CHMHMapNode::TNodeID  id)
inherited

Returns the node with the given ID, or nullptr if it does not exist.

Returns
A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"

Definition at line 49 of file CHierarchicalMapMHPartition.cpp.

References AREAID_INVALID, MRPT_END, and MRPT_START.

◆ getNodeByID() [2/2]

const CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByID ( CHMHMapNode::TNodeID  id) const
inherited

Returns the node with the given ID, or nullptr if it does not exist.

Returns
A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"

Definition at line 63 of file CHierarchicalMapMHPartition.cpp.

References AREAID_INVALID, MRPT_END, and MRPT_START.

◆ getNodeByLabel() [1/2]

CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByLabel ( const std::string &  label,
const THypothesisID hypothesisID 
)
inherited

Returns the node with the given label (case insensitive) for some given hypothesis ID, or nullptr if it does not exist.

Returns
A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"

Definition at line 78 of file CHierarchicalMapMHPartition.cpp.

References mrpt::system::os::_strcmpi(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ getNodeByLabel() [2/2]

const CHMHMapNode::Ptr CHierarchicalMapMHPartition::getNodeByLabel ( const std::string &  label,
const THypothesisID hypothesisID 
) const
inherited

Returns the node with the given label (case insensitive) for some given hypothesis ID, or nullptr if it does not exist.

Returns
A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate"

Definition at line 97 of file CHierarchicalMapMHPartition.cpp.

References mrpt::system::os::_strcmpi(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ GetRuntimeClass()

virtual const mrpt::rtti::TRuntimeClassId* mrpt::hmtslam::CHierarchicalMHMap::GetRuntimeClass ( ) const
overridevirtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::serialization::CSerializable.

◆ GetRuntimeClassIdStatic()

static const mrpt::rtti::TRuntimeClassId& mrpt::hmtslam::CHierarchicalMHMap::GetRuntimeClassIdStatic ( )
static

◆ isLoggingLevelVisible()

bool mrpt::system::COutputLogger::isLoggingLevelVisible ( VerbosityLevel  level) const
inlineinherited

Definition at line 202 of file system/COutputLogger.h.

References mrpt::system::COutputLogger::m_min_verbosity_level.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation(), and mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper().

Here is the caller graph for this function:

◆ loadFromXMLfile()

void CHierarchicalMHMap::loadFromXMLfile ( std::string  fileName)

Load a graph from a XML file.

Definition at line 153 of file CHierarchicalMHMap.cpp.

References THROW_EXCEPTION.

◆ logCond()

void COutputLogger::logCond ( const VerbosityLevel  level,
bool  cond,
const std::string &  msg_str 
) const
inherited

Log the given message only if the condition is satisfied.

See also
log, logFmt

Definition at line 131 of file COutputLogger.cpp.

◆ logDeregisterCallback()

bool COutputLogger::logDeregisterCallback ( output_logger_callback_t  userFunc)
inherited
Returns
true if an entry was found and deleted.

Definition at line 291 of file COutputLogger.cpp.

References getAddress(), and mrpt::system::COutputLogger::m_listCallbacks.

Here is the call graph for this function:

◆ logFmt()

void COutputLogger::logFmt ( const VerbosityLevel  level,
const char *  fmt,
  ... 
) const
inherited

◆ loggerReset()

void COutputLogger::loggerReset ( )
inherited

Reset the contents of the logger instance.

Called upon construction.

Definition at line 206 of file COutputLogger.cpp.

References mrpt::system::LVL_INFO.

◆ logging_levels_to_colors()

std::array< mrpt::system::TConsoleColor, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_colors ( )
staticinherited

Map from VerbosityLevels to their corresponding mrpt::system::TConsoleColor.

Handy for coloring the input based on the verbosity of the message

Definition at line 47 of file COutputLogger.cpp.

References logging_levels_to_colors.

Referenced by mrpt::system::COutputLogger::TMsg::dumpToConsole().

Here is the caller graph for this function:

◆ logging_levels_to_names()

std::array< std::string, NUMBER_OF_VERBOSITY_LEVELS > & COutputLogger::logging_levels_to_names ( )
staticinherited

Map from VerbosityLevels to their corresponding names.

Handy for printing the current message VerbosityLevel along with the actual content

Definition at line 60 of file COutputLogger.cpp.

References logging_levels_to_names.

Referenced by mrpt::system::COutputLogger::TMsg::getAsString().

Here is the caller graph for this function:

◆ logRegisterCallback()

void COutputLogger::logRegisterCallback ( output_logger_callback_t  userFunc)
inherited

Definition at line 278 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::m_listCallbacks.

◆ logStr()

void COutputLogger::logStr ( const VerbosityLevel  level,
std::string_view  msg_str 
) const
inherited

Main method to add the specified message string to the logger.

See also
logCond, logFmt

Definition at line 72 of file COutputLogger.cpp.

References mrpt::system::COutputLogger::TMsg::body, mrpt::system::COutputLogger::TMsg::dumpToConsole(), mrpt::system::COutputLogger::TMsg::level, mrpt::system::COutputLogger::TMsg::name, and mrpt::system::COutputLogger::TMsg::timestamp.

Referenced by mrpt::slam::PF_implementation< mrpt::math::TPose3D, CMonteCarloLocalization3D, mrpt::bayes::particle_storage_mode::VALUE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), mrpt::nav::CReactiveNavigationSystem::STEP1_InitPTGs(), mrpt::system::COutputLoggerStreamWrapper::~COutputLoggerStreamWrapper(), and mrpt::system::CTimeLoggerSaveAtDtor::~CTimeLoggerSaveAtDtor().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ nodeCount()

size_t CHierarchicalMapMHPartition::nodeCount ( ) const
inherited

Returns the number of nodes in the partition:

Definition at line 41 of file CHierarchicalMapMHPartition.cpp.

◆ onArcAddition()

void CHierarchicalMHMap::onArcAddition ( CHMHMapArc::Ptr arc)
protected

Event handler to be called just after an arc has being created: it will be added to the internal list.

Definition at line 144 of file CHierarchicalMHMap.cpp.

◆ onArcDestruction()

void CHierarchicalMHMap::onArcDestruction ( CHMHMapArc arc)
protected

Event handler to be called just before an arc is being destroyed: it will be removed from the internal list.

Note
At *addition we use a smart pointer to assure all the implied guys use the same smrt. pnt., but at destructors the objects don't know anything but "this", thus the usage of plain pointers.

Definition at line 112 of file CHierarchicalMHMap.cpp.

◆ onNodeAddition()

void CHierarchicalMHMap::onNodeAddition ( CHMHMapNode::Ptr node)
protected

Event handler to be called just after a node has being created: it will be added to the internal list.

Definition at line 123 of file CHierarchicalMHMap.cpp.

References ASSERT_.

◆ onNodeDestruction()

void CHierarchicalMHMap::onNodeDestruction ( CHMHMapNode node)
protected

Event handler to be called just before a node is being destroyed: it will be removed from the internal list.

Note
At *addition we use a smart pointer to assure all the implied guys use the same smrt. pnt., but at destructors the objects don't know anything but "this", thus the usage of plain pointers.

Definition at line 99 of file CHierarchicalMHMap.cpp.

References mrpt::hmtslam::CHMHMapNode::getID().

Here is the call graph for this function:

◆ saveAreasDiagramForMATLAB()

void CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB ( const std::string &  filName,
const CHMHMapNode::TNodeID idReferenceNode,
const THypothesisID hypothesisID 
) const
inherited

Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc)

  • The partition may be empty if no node fulfills the condition.
  • All arcs STARTING at each node from the partition will be added to the partition as well.
  • Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only.
    See also
    CHMHMapArc Saves a MATLAB script that represents graphically the nodes with type="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference. ADDITIONAL NOTES:
  • Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
  • If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.

Definition at line 128 of file CHierarchicalMapMHPartition.cpp.

◆ saveAreasDiagramWithEllipsedForMATLAB()

void CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB ( const std::string &  filName,
const CHMHMapNode::TNodeID idReferenceNode,
const THypothesisID hypothesisID,
float  uncertaintyExagerationFactor = 1.0f,
bool  drawArcs = false,
unsigned int  numberOfIterationsForOptimalGlobalPoses = 4 
) const
inherited

Saves a MATLAB script that represents graphically the nodes with type="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference, and drawing the ellipses of the localization uncertainty for each node.

ADDITIONAL NOTES:

  • Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
  • If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.

Definition at line 305 of file CHierarchicalMapMHPartition.cpp.

◆ saveGlobalMapForMATLAB()

void CHierarchicalMapMHPartition::saveGlobalMapForMATLAB ( const std::string &  filName,
const THypothesisID hypothesisID,
const CHMHMapNode::TNodeID idReferenceNode 
) const
inherited

Saves a MATLAB script that represents graphically the reconstructed "global map" ADDITIONAL NOTES:

  • Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
  • If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.

Definition at line 481 of file CHierarchicalMapMHPartition.cpp.

◆ serializeFrom() [1/2]

void CHierarchicalMHMap::serializeFrom ( mrpt::serialization::CArchive in,
uint8_t  serial_version 
)
overrideprotectedvirtual

Pure virtual method for reading (deserializing) from an abstract archive.

Users don't call this method directly. Instead, use stream >> object;.

Parameters
inThe input binary stream where the object data must read from.
versionThe version of the object stored in the stream: use this version number in your code to know how to read the incoming data.
Exceptions
std::exceptionOn any I/O error

Implements mrpt::serialization::CSerializable.

Definition at line 56 of file CHierarchicalMHMap.cpp.

References mrpt::containers::clear(), and MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.

Here is the call graph for this function:

◆ serializeFrom() [2/2]

virtual void mrpt::serialization::CSerializable::serializeFrom ( CSchemeArchiveBase in)
inlineprotectedvirtualinherited

Virtual method for reading (deserializing) from an abstract schema based archive.

Definition at line 74 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ serializeGetVersion()

uint8_t CHierarchicalMHMap::serializeGetVersion ( ) const
overrideprotectedvirtual

Must return the current versioning number of the object.

Start in zero for new classes, and increments each time there is a change in the stored format.

Implements mrpt::serialization::CSerializable.

Definition at line 44 of file CHierarchicalMHMap.cpp.

◆ serializeTo() [1/2]

void CHierarchicalMHMap::serializeTo ( mrpt::serialization::CArchive out) const
overrideprotectedvirtual

Pure virtual method for writing (serializing) to an abstract archive.

Users don't call this method directly. Instead, use stream << object;.

Exceptions
std::exceptionOn any I/O error

Implements mrpt::serialization::CSerializable.

Definition at line 45 of file CHierarchicalMHMap.cpp.

References out.

◆ serializeTo() [2/2]

virtual void mrpt::serialization::CSerializable::serializeTo ( CSchemeArchiveBase out) const
inlineprotectedvirtualinherited

Virtual method for writing (serializing) to an abstract schema based archive.

Definition at line 64 of file CSerializable.h.

References mrpt::serialization::CSerializable::GetRuntimeClass(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ setLoggerName()

void COutputLogger::setLoggerName ( const std::string &  name)
inherited

◆ setMinLoggingLevel()

void COutputLogger::setMinLoggingLevel ( const VerbosityLevel  level)
inherited

Set the minimum logging level for which the incoming logs are going to be taken into account.

String messages with specified VerbosityLevel smaller than the min, will not be outputted to the screen and neither will a record of them be stored in by the COutputLogger instance

Definition at line 144 of file COutputLogger.cpp.

Referenced by mrpt::maps::CRandomFieldGridMap2D::enableVerbose(), mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute(), generic_kf_slam_test(), generic_pf_test(), generic_rbpf_slam_test(), mrpt::apps::RawlogGrabberApp::initialize(), mrpt::hwdrivers::CHokuyoURG::initialize(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::loadParams(), mrpt::apps::CGridMapAlignerApp::run(), mrpt::apps::RBPF_SLAM_App_Base::run(), and mrpt::apps::ICP_SLAM_App_Base::run().

Here is the caller graph for this function:

◆ setVerbosityLevel()

void COutputLogger::setVerbosityLevel ( const VerbosityLevel  level)
inherited

◆ writeLogToFile()

void COutputLogger::writeLogToFile ( const std::string *  fname_in = nullptr) const
inherited

Write the contents of the COutputLogger instance to an external file.

Upon call to this method, COutputLogger dumps the contents of all the logged commands so far to the specified external file. By default the filename is set to ${LOGGERNAME}.log except if the fname parameter is provided

See also
dumpToConsole, getAsString

Definition at line 165 of file COutputLogger.cpp.

References ASSERTMSG_, and mrpt::format().

Here is the call graph for this function:

◆ writeToMatlab()

virtual mxArray* mrpt::serialization::CSerializable::writeToMatlab ( ) const
inlinevirtualinherited

Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.

Returns
A new mxArray (caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB.

Definition at line 90 of file CSerializable.h.

Friends And Related Function Documentation

◆ CHMHMapArc

friend class CHMHMapArc
friend

Definition at line 29 of file CHierarchicalMHMap.h.

◆ CHMHMapNode

friend class CHMHMapNode
friend

Definition at line 30 of file CHierarchicalMHMap.h.

Member Data Documentation

◆ className

constexpr const char* mrpt::hmtslam::CHierarchicalMHMap::className = "mrpt::hmtslam" "::" "CHierarchicalMHMap"
static

Definition at line 32 of file CHierarchicalMHMap.h.

◆ logging_enable_console_output

bool mrpt::system::COutputLogger::logging_enable_console_output {true}
inherited

[Default=true] Set it to false in case you don't want the logged messages to be dumped to the output automatically.

Definition at line 240 of file system/COutputLogger.h.

◆ logging_enable_keep_record

bool mrpt::system::COutputLogger::logging_enable_keep_record {false}
inherited

[Default=false] Enables storing all messages into an internal list.

See also
writeLogToFile, getLogAsString

Definition at line 243 of file system/COutputLogger.h.

◆ m_arcs

TArcList mrpt::hmtslam::CHierarchicalMapMHPartition::m_arcs
protectedinherited

Definition at line 44 of file CHierarchicalMapMHPartition.h.

◆ m_min_verbosity_level

VerbosityLevel mrpt::system::COutputLogger::m_min_verbosity_level {LVL_INFO}
protectedinherited

Provided messages with VerbosityLevel smaller than this value shall be ignored.

Definition at line 253 of file system/COutputLogger.h.

Referenced by mrpt::system::COutputLogger::getMinLoggingLevel(), and mrpt::system::COutputLogger::isLoggingLevelVisible().

◆ m_nodes

TNodeList mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes
protectedinherited

The internal list of nodes and arcs in the whole hierarchical model.

The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects.

Definition at line 43 of file CHierarchicalMapMHPartition.h.

Referenced by mrpt::hmtslam::CHierarchicalMapMHPartition::begin(), and mrpt::hmtslam::CHierarchicalMapMHPartition::end().

◆ runtimeClassId

const mrpt::rtti::TRuntimeClassId mrpt::hmtslam::CHierarchicalMHMap::runtimeClassId
staticprotected

Definition at line 32 of file CHierarchicalMHMap.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020