CDirectedGraph.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 MRPT_DIRECTEDGRAPH_H
#define MRPT_DIRECTEDGRAPH_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/utils/TTypeName.h>
#include <mrpt/utils/aligned_containers.h>
#include <set>
#include <map>
#include <fstream>

namespace mrpt
{
namespace graphs
{
/** Make available this typedef in this namespace too */
using mrpt::utils::TNodeID;
/** Make available this typedef in this namespace too */
using mrpt::utils::TPairNodeIDs;

/** \addtogroup mrpt_graphs_grp
        @{ */

/** Used in mrpt::graphs export functions to .dot files \sa
 * mrpt::graphs::CDirectedGraph::saveAsDot */
struct TGraphvizExportParams
{
        /** If true (default=false), an "[constraint=false]" will be added to all
         * edges (see Graphviz docs). */
        bool mark_edges_as_not_constraint;
        /** If provided, these textual names will be used for naming the nodes
         * instead of their numeric IDs given in the edges. */
        std::map<TNodeID, std::string> node_names;
        /** If provided, an extra line will be added setting Graphviz properties for
         * each node, e.g. to set a node position, use the string "pos = \"x,y\"" */
        std::map<TNodeID, std::string> node_props;

        TGraphvizExportParams() : mark_edges_as_not_constraint(false) {}
};

namespace detail
{
/** An empty structure */
struct edge_annotations_empty
{
};
}

/** A directed graph with the argument of the template specifying the type of
 * the annotations in the edges.
  *  This class only keeps a list of edges (in the member \a edges), so there is
 * no information stored for each node but its existence referred by a node_ID.
  *
  *  Note that edges are stored as a std::multimap<> to allow <b>multiple
 * edges</b> between the same pair of nodes.
  *
  * \sa mrpt::graphs::CDijkstra, mrpt::graphs::CNetworkOfPoses,
 * mrpt::graphs::CDirectedTree
 * \ingroup mrpt_graphs_grp
  */
template <class TYPE_EDGES,
                  class EDGE_ANNOTATIONS = detail::edge_annotations_empty>
class CDirectedGraph
{
   public:
        /** The type of each global pose in \a nodes: an extension of the \a
         * TYPE_EDGES pose with any optional user-defined data */
        struct edge_t : public TYPE_EDGES, public EDGE_ANNOTATIONS
        {
                // Replicate possible constructors:
                inline edge_t() : TYPE_EDGES() {}
                template <typename ARG1>
                inline edge_t(const ARG1& a1) : TYPE_EDGES(a1)
                {
                }
                template <typename ARG1, typename ARG2>
                inline edge_t(const ARG1& a1, const ARG2& a2) : TYPE_EDGES(a1, a2)
                {
                }
        };

        /** The type of the member \a edges */
        typedef typename mrpt::aligned_containers<TPairNodeIDs, edge_t>::multimap_t
                edges_map_t;
        typedef typename edges_map_t::iterator iterator;
        typedef typename edges_map_t::reverse_iterator reverse_iterator;
        typedef typename edges_map_t::const_iterator const_iterator;
        typedef typename edges_map_t::const_reverse_iterator const_reverse_iterator;
        /**\brief Handy self type */
        typedef CDirectedGraph<TYPE_EDGES, EDGE_ANNOTATIONS> self_t;

        /** The public member with the directed edges in the graph */
        edges_map_t edges;

        /** Copy constructor from a multimap<pair< >, > */
        inline CDirectedGraph(const edges_map_t& obj) : edges(obj) {}
        /** Default constructor */
        inline CDirectedGraph() : edges() {}
        inline iterator begin() { return edges.begin(); }
        inline iterator rbegin() { return edges.rbegin(); }
        inline iterator end() { return edges.end(); }
        inline iterator rend() { return edges.rend(); }
        inline const_iterator begin() const { return edges.begin(); }
        inline const_iterator rbegin() const { return edges.rbegin(); }
        inline const_iterator end() const { return edges.end(); }
        inline const_iterator rend() const { return edges.rend(); }
        /** @name Edges/nodes utility methods
                @{ */

        /** The number of edges in the graph */
        inline size_t edgeCount() const { return edges.size(); }
        /** Erase all edges */
        inline void clearEdges() { edges.clear(); }
        /** Insert an edge (from -> to) with the given edge value. \sa
         * insertEdgeAtEnd */
        inline void insertEdge(
                TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
        {
                alignas(16) typename edges_map_t::value_type entry(
                        std::make_pair(from_nodeID, to_nodeID), edge_value);
                edges.insert(entry);
        }

        /** Insert an edge (from -> to) with the given edge value (more efficient
         * version to be called if you know that the end will go at the end of the
         * sorted std::multimap). \sa insertEdge */
        inline void insertEdgeAtEnd(
                TNodeID from_nodeID, TNodeID to_nodeID, const edge_t& edge_value)
        {
                alignas(16) typename edges_map_t::value_type entry(
                        std::make_pair(from_nodeID, to_nodeID), edge_value);
                edges.insert(edges.end(), entry);
        }

        /** Test if the given directed edge exists. */
        inline bool edgeExists(TNodeID from_nodeID, TNodeID to_nodeID) const
        {
                return edges.find(std::make_pair(from_nodeID, to_nodeID)) !=
                           edges.end();
        }

        /** Return a reference to the content of a given edge.
          *  If several edges exist between the given nodes, the first one is
         * returned.
          * \exception std::exception if the given edge does not exist
          * \sa getEdges
          */
        edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID)
        {
                iterator it = edges.find(std::make_pair(from_nodeID, to_nodeID));
                if (it == edges.end())
                        THROW_EXCEPTION(
                                format(
                                        "Edge %u->%u does not exist", (unsigned)from_nodeID,
                                        (unsigned)to_nodeID))
                else
                        return it->second;
        }

        /** Return a reference to the content of a given edge.
          *  If several edges exist between the given nodes, the first one is
         * returned.
          * \exception std::exception if the given edge does not exist
          * \sa getEdges
          */
        const edge_t& getEdge(TNodeID from_nodeID, TNodeID to_nodeID) const
        {
                const_iterator it = edges.find(std::make_pair(from_nodeID, to_nodeID));
                if (it == edges.end())
                        THROW_EXCEPTION(
                                format(
                                        "Edge %u->%u does not exist", (unsigned)from_nodeID,
                                        (unsigned)to_nodeID))
                else
                        return it->second;
        }

        /** Return a pair<first,last> of iterators to the range of edges between two
         * given nodes. \sa getEdge  */
        std::pair<iterator, iterator> getEdges(
                TNodeID from_nodeID, TNodeID to_nodeID)
        {
                return edges.equal_range(std::make_pair(from_nodeID, to_nodeID));
        }
        /** Return a pair<first,last> of const iterators to the range of edges
         * between two given nodes.  \sa getEdge */
        std::pair<const_iterator, const_iterator> getEdges(
                TNodeID from_nodeID, TNodeID to_nodeID) const
        {
                return edges.equal_range(std::make_pair(from_nodeID, to_nodeID));
        }

        /** Erase all edges between the given nodes (it has no effect if no edge
         * existed)
          */
        inline void eraseEdge(TNodeID from_nodeID, TNodeID to_nodeID)
        {
                edges.erase(std::make_pair(from_nodeID, to_nodeID));
        }

        /** Return a list of all the node_ID's of the graph, generated from all the
         * nodes that appear in the list of edges
          */
        void getAllNodes(std::set<TNodeID>& lstNode_IDs) const
        {
                lstNode_IDs.clear();
                for (typename edges_map_t::const_iterator it = edges.begin();
                         it != edges.end(); ++it)
                {
                        lstNode_IDs.insert(it->first.first);
                        lstNode_IDs.insert(it->first.second);
                }
        }

        /** Less efficient way to get all nodes that returns a copy of the set
         * object \sa getAllNodes( std::set<TNodeID> &lstNode_IDs) */
        inline std::set<TNodeID> getAllNodes() const
        {
                std::set<TNodeID> lst;
                getAllNodes(lst);
                return lst;
        }

        /** Count how many different node IDs appear in the graph edges.
          */
        size_t countDifferentNodesInEdges() const
        {
                std::set<TNodeID> aux;
                for (typename edges_map_t::const_iterator it = edges.begin();
                         it != edges.end(); ++it)
                {
                        aux.insert(it->first.first);
                        aux.insert(it->first.second);
                }
                return aux.size();
        }

        /** Return the list of all neighbors of "nodeID", by creating a list of
         * their node IDs. \sa getAdjacencyMatrix */
        void getNeighborsOf(
                const TNodeID nodeID, std::set<TNodeID>& neighborIDs) const
        {
                neighborIDs.clear();
                for (typename edges_map_t::const_iterator it = edges.begin();
                         it != edges.end(); ++it)
                {
                        if (it->first.first == nodeID)
                                neighborIDs.insert(it->first.second);
                        else if (it->first.second == nodeID)
                                neighborIDs.insert(it->first.first);
                }
        }
        /** Return the list of all neighbors of "nodeID", by creating a list of
         * their node IDs. \sa getAdjacencyMatrix */
        std::set<TNodeID> getNeighborsOf(const TNodeID nodeID) const
        {
                std::set<TNodeID> neighborIDs;
                this->getNeighborsOf(nodeID, neighborIDs);
                return neighborIDs;
        }

        /** Return a map from node IDs to all its neighbors (that is, connected
         * nodes, regardless of the edge direction)
          *  This is a much more efficient method than calling getNeighborsOf() for
         * each node in the graph.
          *  Possible values for the template argument MAP_NODEID_SET_NODEIDS are:
          *    - std::map<TNodeID, std::set<TNodeID> >
          *    - mrpt::utils::map_as_vector<TNodeID, std::set<TNodeID> >
          * \sa getNeighborsOf
          */
        template <class MAP_NODEID_SET_NODEIDS>
        void getAdjacencyMatrix(MAP_NODEID_SET_NODEIDS& outAdjacency) const
        {
                outAdjacency.clear();
                for (typename edges_map_t::const_iterator it = edges.begin();
                         it != edges.end(); ++it)
                {
                        outAdjacency[it->first.first].insert(it->first.second);
                        outAdjacency[it->first.second].insert(it->first.first);
                }
        }

        /** Just like \a getAdjacencyMatrix but return only the adjacency for those
         * node_ids in the set \a onlyForTheseNodes
          *  (both endings nodes of an edge must be within the set for it to be
         * returned) */
        template <class MAP_NODEID_SET_NODEIDS, class SET_NODEIDS>
        void getAdjacencyMatrix(
                MAP_NODEID_SET_NODEIDS& outAdjacency,
                const SET_NODEIDS& onlyForTheseNodes) const
        {
                outAdjacency.clear();
                const typename SET_NODEIDS::const_iterator setEnd =
                        onlyForTheseNodes.end();
                for (typename edges_map_t::const_iterator it = edges.begin();
                         it != edges.end(); ++it)
                {
                        if (onlyForTheseNodes.find(it->first.first) == setEnd ||
                                onlyForTheseNodes.find(it->first.second) == setEnd)
                                continue;
                        outAdjacency[it->first.first].insert(it->first.second);
                        outAdjacency[it->first.second].insert(it->first.first);
                }
        }

        /** @} */  // end of edge/nodes utilities

        /** @name I/O utilities
                @{ */

        /** Save the graph in a Graphviz (.dot files) text format; useful for
         * quickly rendering the graph with "dot"
          * \return false on any error */
        bool saveAsDot(
                std::ostream& o,
                const TGraphvizExportParams& p = TGraphvizExportParams()) const
        {
                o << "digraph G {\n";
                for (const_iterator it = begin(); it != end(); ++it)
                {
                        const TNodeID id1 = it->first.first;
                        const TNodeID id2 = it->first.second;
                        std::string s1, s2;
                        if (!p.node_names.empty())
                        {
                                std::map<TNodeID, std::string>::const_iterator itNam1 =
                                        p.node_names.find(id1);
                                if (itNam1 != p.node_names.end()) s1 = itNam1->second;
                                std::map<TNodeID, std::string>::const_iterator itNam2 =
                                        p.node_names.find(id2);
                                if (itNam2 != p.node_names.end()) s2 = itNam2->second;
                        }
                        if (s1.empty())
                                s1 = mrpt::format("%u", static_cast<unsigned int>(id1));
                        if (s2.empty())
                                s2 = mrpt::format("%u", static_cast<unsigned int>(id2));
                        if (p.node_props.empty())
                        {
                                std::map<TNodeID, std::string>::const_iterator itP1 =
                                        p.node_props.find(id1);
                                if (itP1 != p.node_props.end())
                                        o << "\"" << s1 << "\""
                                          << " [" << itP1->second << "];\n";
                                std::map<TNodeID, std::string>::const_iterator itP2 =
                                        p.node_props.find(id2);
                                if (itP2 != p.node_props.end())
                                        o << "\"" << s2 << "\""
                                          << " [" << itP2->second << "];\n";
                        }
                        o << " \"" << s1 << "\" -> \"" << s2 << "\"";
                        if (p.mark_edges_as_not_constraint) o << " [constraint=false]";
                        o << ";\n";
                }
                return !((o << "}\n").fail());
        }

        /** \overload */
        bool saveAsDot(
                const std::string& fileName,
                const TGraphvizExportParams& p = TGraphvizExportParams()) const
        {
                std::ofstream f(fileName.c_str());
                if (!f.is_open()) return false;
                return saveAsDot(f, p);
        }
        /** @} */

};  // end class CDirectedGraph

/** @} */
}  // End of namespace

// Specialization of TTypeName must occur in the same namespace:
namespace utils
{
MRPT_DECLARE_TTYPENAME(mrpt::graphs::detail::edge_annotations_empty)
}

}  // End of namespace
#endif