CHierarchicalMapMHPartition.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CHierarchicalMapMHPartition_H
#define CHierarchicalMapMHPartition_H

#include <mrpt/hmtslam/CLocalMetricHypothesis.h>
#include <mrpt/hmtslam/CHMHMapArc.h>
#include <mrpt/hmtslam/CHMHMapNode.h>

#include <mrpt/poses/CPose3DPDFSOG.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/utils/COutputLogger.h>
#include <mrpt/opengl/opengl_frwds.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::utils::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:
        typedef TNodeList::iterator iterator;
        typedef TNodeList::const_iterator 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
          */
        typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;

        /** 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 enought 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::utils::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,
                                 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;

        /** 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,
                const bool& showRobotPoseIDs = true) const;

        /** Return a textual description of the whole graph */
        void dumpAsText(utils::CStringList& 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 enought 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,
                const size_t& monteCarloSamples = 100,
                const float margin_to_substract = 6) const;

   protected:
};  // End of class def.
}
}

#endif