CHierarchicalMapMHPartition.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/hmtslam/CHMHMapArc.h>
#include <mrpt/hmtslam/CHMHMapNode.h>
#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/opengl/opengl_frwds.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/system/COutputLogger.h>
#include <map>

namespace mrpt
{
namespace poses
{
class CPose3DPDFParticles;
}

namespace hmtslam
{
/** 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.
 * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
 * \ingroup mrpt_hmtslam_grp
 */
class CHierarchicalMapMHPartition : public mrpt::system::COutputLogger
{
   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.
     */
    TNodeList m_nodes;
    TArcList m_arcs;

   public:
    using iterator = TNodeList::iterator;
    using const_iterator = TNodeList::const_iterator;

    /** Returns an iterator to the first node in the graph. */
    const_iterator begin() const { return m_nodes.begin(); }
    /** Returns an iterator to the first node in the graph. */
    iterator begin() { return m_nodes.begin(); }
    /** Returns an iterator to the end of the list of nodes in the graph. */
    const_iterator end() const { return m_nodes.end(); }
    /** Returns an iterator to the end of the list of nodes in the graph. */
    iterator end() { return m_nodes.end(); }
    CHierarchicalMapMHPartition() : m_nodes(), m_arcs() {}
    /** A type that reprensents a sequence of node IDs
     */
    using TNodeIDsList = std::vector<CHMHMapNode::TNodeID>;

    /** Returns the number of nodes in the partition:
     */
    size_t nodeCount() const;

    /** Returns the number of arcs in the partition:
     */
    size_t arcCount() const;

    /** Returns the first node in the graph, or nullptr if it does not exist.
     * \return 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 getFirstNode();

    /** Returns the node with the given ID, or nullptr if it does not exist.
     * \return 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.
     * \return 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 label (case insensitive) for some given
     * hypothesis ID, or nullptr if it does not exist.
     * \return 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.
     * \return 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 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.
     * \sa CHMHMapArc
     */
    // CHierarchicalMapMHPartition   getPartitionByHiearchyLevel( unsigned int
    // level );

    /** Saves a MATLAB script that represents graphically the nodes with
     *<i>type</i>="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.
     */
    void saveAreasDiagramForMATLAB(
        const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
        const THypothesisID& hypothesisID) const;

    /** Saves a MATLAB script that represents graphically the nodes with
     *<i>type</i>="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 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 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 saveGlobalMapForMATLAB(
        const std::string& filName, const THypothesisID& hypothesisID,
        const CHMHMapNode::TNodeID& idReferenceNode) const;

    /** The Dijkstra algorithm for finding the shortest path between a pair of
     * nodes.
     * \return 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 findPathBetweenNodes(
        const CHMHMapNode::TNodeID& nodeFrom,
        const CHMHMapNode::TNodeID& nodeTo, const THypothesisID& hypothesisID,
        TArcList& out_path, bool direction = false) const;

    /** Draw a number of samples according to the PDF of the coordinates
     * transformation between a pair of "Area"'s nodes.
     * \exception std::exception If there is not enough information in arcs to
     * compute the PDF
     * \sa computeGloballyConsistentNodeCoordinates
     */
    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;

    /** 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".
     */
    float computeMatchProbabilityBetweenNodes(
        const CHMHMapNode::TNodeID& nodeFrom,
        const CHMHMapNode::TNodeID& nodeTo, float& maxMatchProb,
        mrpt::poses::CPose3DPDFSOG& estimatedRelativePose,
        const THypothesisID& hypothesisID,
        unsigned int monteCarloSamplesPose = 300);

    /** Returns all the arcs between a pair of nodes:
     */
    void findArcsBetweenNodes(
        const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
        const THypothesisID& hypothesisID, TArcList& out_listArcs) const;

    /** Returns the arcs between a pair of nodes of a given type.
     */
    void findArcsOfTypeBetweenNodes(
        const CHMHMapNode::TNodeID& node1id,
        const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
        const std::string& arcType, TArcList& ret) const;

    /** Returns the first arc between a pair of nodes of a given type, and if it
     * is in the opposite direction.
     * \return The arc, or nullptr if not found.
     */
    CHMHMapArc::Ptr findArcOfTypeBetweenNodes(
        const CHMHMapNode::TNodeID& node1id,
        const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
        const std::string& arcType, bool& isInverted) const;

    /** Returns whether two nodes are "neightbour", i.e. have a direct arc
     * between them  */
    bool areNodesNeightbour(
        const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
        const THypothesisID& hypothesisID,
        const char* requiredAnnotation = nullptr) 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".
     * \exception std::exception If there is any node without a pose arc,
     * invalid (non invertible) matrixes, etc...
     * \sa computeCoordinatesTransformationBetweenNodes
     */
    void computeGloballyConsistentNodeCoordinates(
        std::map<CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian>&
            nodePoses,
        const CHMHMapNode::TNodeID& idReferenceNode,
        const THypothesisID& hypothesisID,
        const unsigned int& numberOfIterations = 2) const;

    /** Returns a 3D scene reconstruction of the hierarchical map.
     *  See "computeGloballyConsistentNodeCoordinates" for the meaning of
     * "numberOfIterationsForOptimalGlobalPoses"
     */
    void getAs3DScene(
        mrpt::opengl::COpenGLScene& outScene,
        const CHMHMapNode::TNodeID& idReferenceNode,
        const THypothesisID& hypothesisID,
        const unsigned int& numberOfIterationsForOptimalGlobalPoses = 5,
        bool showRobotPoseIDs = true) const;

    /** Return a textual description of the whole graph */
    void dumpAsText(std::vector<std::string>& s) const;

    /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a
     * Monte Carlo aproximation.
     * \exception std::exception If there is not enough information in arcs,
     * etc...
     * \param margin_to_substract In meters, the area of each gridmap is
     * "eroded" this amount to compensate the area in excess usually found in
     * gridmaps.
     */
    double computeOverlapProbabilityBetweenNodes(
        const CHMHMapNode::TNodeID& nodeFrom,
        const CHMHMapNode::TNodeID& nodeTo, const THypothesisID& hypothesisID,
        size_t monteCarloSamples = 100,
        const float margin_to_substract = 6) const;

   protected:
};  // End of class def.
}  // namespace hmtslam
}  // namespace mrpt