class mrpt::hmtslam::CHierarchicalMapMHPartition

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

#include <mrpt/hmtslam/CHierarchicalMapMHPartition.h>

class CHierarchicalMapMHPartition: public mrpt::system::COutputLogger
{
public:
    // typedefs

    typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;

    // construction

    CHierarchicalMapMHPartition();

    //
methods

    const_iterator begin() const;
    iterator begin();
    const_iterator end() const;
    iterator end();
    size_t nodeCount() const;
    size_t arcCount() const;
    CHMHMapNode::Ptr getFirstNode();
    CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id);
    const CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id) const;
    CHMHMapNode::Ptr getNodeByLabel(const std::string& label, const THypothesisID& hypothesisID);
    const CHMHMapNode::Ptr getNodeByLabel(const std::string& label, const THypothesisID& hypothesisID) const;
    void saveAreasDiagramForMATLAB(const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode, const THypothesisID& hypothesisID) const;

    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;

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

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

    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;

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

    void findArcsBetweenNodes(const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2, const THypothesisID& hypothesisID, TArcList& out_listArcs) const;

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

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

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

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

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

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

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

// direct descendants

class CHierarchicalMHMap;

Inherited Members

public:
    // structs

    struct TMsg;

Typedefs

typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList

A type that reprensents a sequence of node IDs.

Methods

const_iterator begin() const

Returns an iterator to the first node in the graph.

iterator begin()

Returns an iterator to the first node in the graph.

const_iterator end() const

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

iterator end()

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

size_t nodeCount() const

Returns the number of nodes in the partition:

size_t arcCount() const

Returns the number of arcs in the partition:

CHMHMapNode::Ptr getFirstNode()

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”

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

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”

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

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”

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.

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”

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.

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”

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)

• 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.

• 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.

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:

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.

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.

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:

• 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.

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.

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.

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.

Parameters:

std::exception

If there is not enough information in arcs to compute the PDF

See also:

computeGloballyConsistentNodeCoordinates

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”.

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:

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.

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.

Returns:

The arc, or nullptr if not found.

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.

have a direct arc between them

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.

Global coordinates will be computed relative to the node “idReferenceNode”.

Parameters:

std::exception

If there is any node without a pose arc, invalid (non invertible) matrixes, etc…

See also:

computeCoordinatesTransformationBetweenNodes

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.

See “computeGloballyConsistentNodeCoordinates” for the meaning of “numberOfIterationsForOptimalGlobalPoses”

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

Return a textual description of the whole graph.

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.

Parameters:

std::exception	If there is not enough 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.