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.
Definition at line 34 of file CHierarchicalMapMHPartition.h.
#include <mrpt/hmtslam/CHierarchicalMapMHPartition.h>
Public Types | |
typedef TNodeList::iterator | iterator |
typedef TNodeList::const_iterator | const_iterator |
typedef std::vector< CHMHMapNode::TNodeID > | TNodeIDsList |
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... | |
Static Public Member Functions | |
static void | printf_debug (const char *frmt,...) |
Sends a formated text to "debugOut" if not NULL, or to cout otherwise. More... | |
Protected Attributes | |
TNodeList | m_nodes |
The internal list of nodes and arcs in the whole hierarchical model. More... | |
TArcList | m_arcs |
typedef TNodeList::const_iterator mrpt::hmtslam::CHierarchicalMapMHPartition::const_iterator |
Definition at line 46 of file CHierarchicalMapMHPartition.h.
typedef TNodeList::iterator mrpt::hmtslam::CHierarchicalMapMHPartition::iterator |
Definition at line 45 of file CHierarchicalMapMHPartition.h.
typedef std::vector<CHMHMapNode::TNodeID> mrpt::hmtslam::CHierarchicalMapMHPartition::TNodeIDsList |
A type that reprensents a sequence of node IDs.
Definition at line 66 of file CHierarchicalMapMHPartition.h.
|
inline |
Definition at line 61 of file CHierarchicalMapMHPartition.h.
size_t mrpt::hmtslam::CHierarchicalMapMHPartition::arcCount | ( | ) | const |
Returns the number of arcs in the partition:
bool mrpt::hmtslam::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
|
inline |
Returns an iterator to the first node in the graph.
Definition at line 49 of file CHierarchicalMapMHPartition.h.
|
inline |
Returns an iterator to the first node in the graph.
Definition at line 52 of file CHierarchicalMapMHPartition.h.
void mrpt::hmtslam::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 |
Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.
std::exception | If there is not enought information in arcs to compute the PDF |
void mrpt::hmtslam::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 |
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".
std::exception | If there is any node without a pose arc, invalid (non invertible) matrixes, etc... |
float mrpt::hmtslam::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".
double mrpt::hmtslam::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.
std::exception | If there is not enought information in arcs, etc... |
margin_to_substract | In meters, the area of each gridmap is "eroded" this amount to compensate the area in excess usually found in gridmaps. |
void mrpt::hmtslam::CHierarchicalMapMHPartition::dumpAsText | ( | utils::CStringList & | s | ) | const |
Return a textual description of the whole graph.
|
inline |
Returns an iterator to the end of the list of nodes in the graph.
Definition at line 55 of file CHierarchicalMapMHPartition.h.
|
inline |
Returns an iterator to the end of the list of nodes in the graph.
Definition at line 58 of file CHierarchicalMapMHPartition.h.
CHMHMapArcPtr mrpt::hmtslam::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.
void mrpt::hmtslam::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:
void mrpt::hmtslam::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.
void mrpt::hmtslam::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.
void mrpt::hmtslam::CHierarchicalMapMHPartition::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.
See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
CHMHMapNodePtr mrpt::hmtslam::CHierarchicalMapMHPartition::getFirstNode | ( | ) |
Returns the first node in the graph, or NULL if it does not exist.
CHMHMapNodePtr mrpt::hmtslam::CHierarchicalMapMHPartition::getNodeByID | ( | CHMHMapNode::TNodeID | id | ) |
Returns the node with the given ID, or NULL if it does not exist.
const CHMHMapNodePtr mrpt::hmtslam::CHierarchicalMapMHPartition::getNodeByID | ( | CHMHMapNode::TNodeID | id | ) | const |
Returns the node with the given ID, or NULL if it does not exist.
CHMHMapNodePtr mrpt::hmtslam::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.
const CHMHMapNodePtr mrpt::hmtslam::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.
size_t mrpt::hmtslam::CHierarchicalMapMHPartition::nodeCount | ( | ) | const |
Returns the number of nodes in the partition:
|
staticinherited |
Sends a formated text to "debugOut" if not NULL, or to cout otherwise.
Referenced by mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().
void mrpt::hmtslam::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)
Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
void mrpt::hmtslam::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:
void mrpt::hmtslam::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:
|
protected |
Definition at line 41 of file CHierarchicalMapMHPartition.h.
|
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.15 for MRPT 1.4.0 SVN: at Sun Mar 24 23:21:48 UTC 2019 |