Main MRPT website > C++ reference for MRPT 1.5.9
List of all members | Public Types | Public Member Functions | Protected Attributes
mrpt::hmtslam::CHierarchicalMapMHPartition Class Reference

Detailed Description

Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.

A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap.

See also
CHierarchicalMHMap, CHMHMapArc, CHMHMapNode

Definition at line 34 of file CHierarchicalMapMHPartition.h.

#include <mrpt/hmtslam/CHierarchicalMapMHPartition.h>

Inheritance diagram for mrpt::hmtslam::CHierarchicalMapMHPartition:
Inheritance graph

Public Types

typedef TNodeList::iterator iterator
 
typedef TNodeList::const_iterator const_iterator
 
typedef std::vector< CHMHMapNode::TNodeIDTNodeIDsList
 A type that reprensents a sequence of node IDs. More...
 

Public Member Functions

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...
 
 CHierarchicalMapMHPartition ()
 
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...
 
CHMHMapNodePtr getFirstNode ()
 Returns the first node in the graph, or NULL if it does not exist. More...
 
CHMHMapNodePtr getNodeByID (CHMHMapNode::TNodeID id)
 Returns the node with the given ID, or NULL if it does not exist. More...
 
const CHMHMapNodePtr getNodeByID (CHMHMapNode::TNodeID id) const
 Returns the node with the given ID, or NULL if it does not exist. More...
 
CHMHMapNodePtr getNodeByLabel (const std::string &label, const THypothesisID &hypothesisID)
 Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist. More...
 
const CHMHMapNodePtr getNodeByLabel (const std::string &label, const THypothesisID &hypothesisID) const
 Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL 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::utils::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...
 
CHMHMapArcPtr 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=NULL) const
 Returns whether two nodes are "neightbour", i.e. More...
 
void computeGloballyConsistentNodeCoordinates (std::map< CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian, std::less< CHMHMapNode::TNodeID >, Eigen::aligned_allocator< std::pair< const 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, const bool &showRobotPoseIDs=true) const
 Returns a 3D scene reconstruction of the hierarchical map. More...
 
void dumpAsText (utils::CStringList &s) const
 Return a textual description of the whole graph. More...
 
double computeOverlapProbabilityBetweenNodes (const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, const THypothesisID &hypothesisID, const 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...
 

Protected Attributes

TNodeList m_nodes
 The internal list of nodes and arcs in the whole hierarchical model. More...
 
TArcList m_arcs
 

Member Typedef Documentation

◆ const_iterator

Definition at line 46 of file CHierarchicalMapMHPartition.h.

◆ iterator

Definition at line 45 of file CHierarchicalMapMHPartition.h.

◆ TNodeIDsList

A type that reprensents a sequence of node IDs.

Definition at line 66 of file CHierarchicalMapMHPartition.h.

Constructor & Destructor Documentation

◆ CHierarchicalMapMHPartition()

mrpt::hmtslam::CHierarchicalMapMHPartition::CHierarchicalMapMHPartition ( )
inline

Definition at line 61 of file CHierarchicalMapMHPartition.h.

Member Function Documentation

◆ arcCount()

size_t CHierarchicalMapMHPartition::arcCount ( ) const

Returns the number of arcs in the partition:

Definition at line 49 of file CHierarchicalMapMHPartition.cpp.

◆ areNodesNeightbour()

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

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

have a direct arc between them

Definition at line 1050 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, and MRPT_START.

◆ begin() [1/2]

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

Returns an iterator to the first node in the graph.

Definition at line 49 of file CHierarchicalMapMHPartition.h.

◆ begin() [2/2]

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

Returns an iterator to the first node in the graph.

Definition at line 52 of file CHierarchicalMapMHPartition.h.

◆ 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::utils::DEG2RAD(0.1) 
) const

◆ computeGloballyConsistentNodeCoordinates()

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

◆ 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 
)

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 963 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, and THROW_EXCEPTION.

◆ computeOverlapProbabilityBetweenNodes()

double CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes ( const CHMHMapNode::TNodeID nodeFrom,
const CHMHMapNode::TNodeID nodeTo,
const THypothesisID hypothesisID,
const 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.

Exceptions
std::exceptionIf there is not enought 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 1509 of file CHierarchicalMapMHPartition.cpp.

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

◆ dumpAsText()

void CHierarchicalMapMHPartition::dumpAsText ( utils::CStringList s) const

◆ end() [1/2]

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

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

Definition at line 55 of file CHierarchicalMapMHPartition.h.

◆ end() [2/2]

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

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

Definition at line 58 of file CHierarchicalMapMHPartition.h.

◆ findArcOfTypeBetweenNodes()

CHMHMapArcPtr CHierarchicalMapMHPartition::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.

Returns
The arc, or NULL if not found.

Definition at line 1474 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

Returns all the arcs between a pair of nodes:

Definition at line 985 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

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

Definition at line 1016 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

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 721 of file CHierarchicalMapMHPartition.cpp.

References ASSERT_, ASSERTMSG_, MRPT_END, and MRPT_START.

◆ getAs3DScene()

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

◆ getFirstNode()

CHMHMapNodePtr CHierarchicalMapMHPartition::getFirstNode ( )

Returns the first node in the graph, or NULL 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 122 of file CHierarchicalMapMHPartition.cpp.

◆ getNodeByID() [1/2]

CHMHMapNodePtr CHierarchicalMapMHPartition::getNodeByID ( CHMHMapNode::TNodeID  id)

Returns the node with the given ID, or NULL 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 58 of file CHierarchicalMapMHPartition.cpp.

References AREAID_INVALID, MRPT_END, and MRPT_START.

Referenced by mrpt::hmtslam::CHMHMapArc::Create().

◆ getNodeByID() [2/2]

const CHMHMapNodePtr CHierarchicalMapMHPartition::getNodeByID ( CHMHMapNode::TNodeID  id) const

Returns the node with the given ID, or NULL 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 71 of file CHierarchicalMapMHPartition.cpp.

References AREAID_INVALID, MRPT_END, and MRPT_START.

◆ getNodeByLabel() [1/2]

CHMHMapNodePtr CHierarchicalMapMHPartition::getNodeByLabel ( const std::string label,
const THypothesisID hypothesisID 
)

Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL 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 85 of file CHierarchicalMapMHPartition.cpp.

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

◆ getNodeByLabel() [2/2]

const CHMHMapNodePtr CHierarchicalMapMHPartition::getNodeByLabel ( const std::string label,
const THypothesisID hypothesisID 
) const

Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL 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 103 of file CHierarchicalMapMHPartition.cpp.

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

◆ nodeCount()

size_t CHierarchicalMapMHPartition::nodeCount ( ) const

Returns the number of nodes in the partition:

Definition at line 41 of file CHierarchicalMapMHPartition.cpp.

◆ saveAreasDiagramForMATLAB()

void CHierarchicalMapMHPartition::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)

  • 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
    CHMHMapArcSaves 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 132 of file CHierarchicalMapMHPartition.cpp.

References MRPT_UNUSED_PARAM.

◆ 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

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.

References MRPT_UNUSED_PARAM.

◆ saveGlobalMapForMATLAB()

void CHierarchicalMapMHPartition::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:

  • 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 479 of file CHierarchicalMapMHPartition.cpp.

References MRPT_END, MRPT_START, and MRPT_UNUSED_PARAM.

Member Data Documentation

◆ m_arcs

TArcList mrpt::hmtslam::CHierarchicalMapMHPartition::m_arcs
protected

Definition at line 41 of file CHierarchicalMapMHPartition.h.

◆ m_nodes

TNodeList mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes
protected

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 40 of file CHierarchicalMapMHPartition.h.




Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020