9#ifndef CHierarchicalMapMHPartition_H
10#define CHierarchicalMapMHPartition_H
25 namespace poses {
class CPose3DPDFParticles; }
115 const std::string &filName,
125 const std::string &filName,
128 float uncertaintyExagerationFactor = 1.0f,
129 bool drawArcs =
false,
130 unsigned int numberOfIterationsForOptimalGlobalPoses = 4
139 const std::string &filName,
152 bool direction=
false)
const;
164 unsigned int particlesCount = 100,
165 float additionalNoiseXYratio = 0.02,
178 unsigned int monteCarloSamplesPose = 300
195 const std::string &arcType,
205 const std::string &arcType,
206 bool &isInverted )
const;
214 const char *requiredAnnotation=NULL )
const;
225 const unsigned int &numberOfIterations = 2)
const;
234 const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5,
235 const bool &showRobotPoseIDs =
true
251 const size_t &monteCarloSamples = 100,
252 const float margin_to_substract = 6
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical,...
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.
iterator begin()
Returns an iterator to the first node in the graph.
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...
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const
Returns the node with the given ID, or NULL if it does not exist.
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.
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 "Ar...
size_t arcCount() const
Returns the number of arcs in the partition:
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.
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,...
const_iterator begin() const
Returns an iterator to the first node in the graph.
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:
const_iterator end() const
Returns an iterator to the end of the list of nodes in the graph.
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.
TNodeList::const_iterator const_iterator
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
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,...
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...
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 al...
void dumpAsText(utils::CStringList &s) const
Return a textual description of the whole graph.
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:
CHMHMapNodePtr getFirstNode()
Returns the first node in the graph, or NULL if it does not exist.
TNodeList::iterator iterator
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.
CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id)
Returns the node with the given ID, or NULL if it does not exist.
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 gri...
CHierarchicalMapMHPartition()
CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID)
Returns the node with the given label (case insensitive) for some given hypothesis ID,...
size_t nodeCount() const
Returns the number of nodes in the partition:
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
A class for storing a sequence of arcs (a path).
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
This base class provides a common printf-like method to send debug information to std::cout,...
A class for storing a list of text lines.
const Scalar * const_iterator
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
double DEG2RAD(const double x)
Degrees to radians.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.