dijkstra.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, 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_DIJKSTRA_H
#define  MRPT_DIJKSTRA_H

#include <mrpt/graphs/CDirectedGraph.h>
#include <mrpt/graphs/CDirectedTree.h>
#include <mrpt/utils/traits_map.h>
#include <limits>

namespace mrpt
{
        namespace graphs
        {
            using namespace std;
                using namespace mrpt::utils;

                /** The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) directed graph and all other nodes in the form of a tree.
                  *  The constructor takes as input the graph (the set of directed edges) computes all the needed data, then
                  *   successive calls to \a getShortestPathTo return the paths efficiently from the root.
                  *  The entire generated tree can be also retrieved with \a getTreeGraph.
                  *
                  *  Input graphs are represented by instances of (or classes derived from) mrpt::graphs::CDirectedGraph, and node's IDs are uint64_t values,
                  *   although the type mrpt::utils::TNodeID is also provided for clarity in the code.
                  *
                  *  The second template argument MAPS_IMPLEMENTATION allows choosing between a sparse std::map<> representation (using mrpt::utils::map_traits_stdmap)
                  *   for several intermediary and final results, and an alternative (using mrpt::utils::map_traits_map_as_vector as argument)
                  *   dense implementation which is much faster, but can be only used if the TNodeID's start in 0 or a low value.
                  *
                  * See <a href="http://www.mrpt.org/Example:Dijkstra_optimal_path_search_in_graphs" > this page </a> for a complete example.
                  * \ingroup mrpt_graphs_grp
                  */
                template<class TYPE_GRAPH, class MAPS_IMPLEMENTATION = mrpt::utils::map_traits_stdmap >
                class CDijkstra
                {
                protected:
                        /** Auxiliary struct for topological distances from root node */
                        struct TDistance
                        {
                                double dist;
                                inline TDistance() : dist( std::numeric_limits<double>::max() ) { }
                                inline TDistance(const double D) : dist(D) { }
                                inline const TDistance & operator =(const double D) { dist = D; return *this;}
                        };

                        /** Auxiliary struct for backward paths */
                        struct TPrevious
                        {
                                inline TPrevious() : id( INVALID_NODEID ) { }
                                TNodeID  id;
                        };

                        // Cached input data:
                        const TYPE_GRAPH & m_cached_graph;
                        const TNodeID      m_source_node_ID;

                        // Private typedefs:
                        typedef typename MAPS_IMPLEMENTATION::template map<TNodeID, std::set<TNodeID> >  list_all_neighbors_t; //!< A std::map (or a similar container according to MAPS_IMPLEMENTATION) with all the neighbors of every node.
                        typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs>  id2pairIDs_map_t;
                        typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance>     id2dist_map_t;
                        typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious>     id2id_map_t;

                        // Intermediary and final results:
                        id2dist_map_t                  m_distances; //!< All the distances
                        std::map<TNodeID,TDistance>    m_distances_non_visited; // Use a std::map here in all cases.
                        id2id_map_t                    m_prev_node;
                        id2pairIDs_map_t               m_prev_arc;
                        std::set<TNodeID>              m_lstNode_IDs;
                        list_all_neighbors_t           m_allNeighbors;

                public:
                        /** @name Useful typedefs
                            @{ */

                        typedef TYPE_GRAPH                 graph_t;     //!< The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class
                        typedef typename graph_t::edge_t   edge_t;          //!< The type of edge data in graph_t
                        typedef std::list<TPairNodeIDs>    edge_list_t; //!< A list of edges used to describe a path on the graph

                        /** @} */

                        /** Constructor, which takes the input graph and executes the entire Dijkstra algorithm from the given root node ID.
                          *
                          *  The graph is given by the set of directed edges, stored in a mrpt::graphs::CDirectedGraph class.
                          *
                          *  If a function \a functor_edge_weight is provided, it will be used to compute the weight of edges.
                          *  Otherwise, all edges weight the unity.
                          *
                          *  After construction, call \a getShortestPathTo to get the shortest path to a node or \a getTreeGraph for the tree representation.
                          *
                          * \sa getShortestPathTo, getTreeGraph
                          * \exception std::exception If the source nodeID is not found in the graph
                          */
                        CDijkstra(
                                const graph_t  &graph,
                                const TNodeID   source_node_ID,
                                double (*functor_edge_weight)(const graph_t& graph, const TNodeID id_from, const TNodeID id_to, const edge_t &edge) =  NULL,
                                void   (*functor_on_progress)(const graph_t& graph, size_t visitedCount) = NULL
                                )
                                : m_cached_graph(graph), m_source_node_ID(source_node_ID)
                        {
                                MRPT_START
                                /*
                                1  function Dijkstra(G, w, s)
                                2     for each vertex v in V[G]                        // Initializations
                                3           m_distances[v] := infinity
                                4           m_prev_node[v] := undefined
                                5     m_distances[s] := 0
                                6     S := empty set
                                7     Q := V[G]
                                8     while Q is not an empty set                      // The algorithm itself
                                9           u := Extract_Min(Q)
                                10           S := S union {u}
                                11           for each edge (u,v) outgoing from u
                                12                  if m_distances[u] + w(u,v) < m_distances[v]             // Relax (u,v)
                                13                        m_distances[v] := m_distances[u] + w(u,v)
                                14                        m_prev_node[v] := u
                                */

                                // Makea list of all the nodes in the graph:
                                graph.getAllNodes( m_lstNode_IDs );
                                const size_t nNodes = m_lstNode_IDs.size();

                                if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() )
                                        THROW_EXCEPTION_CUSTOM_MSG1("Cannot find the source node_ID=%u in the graph",static_cast<unsigned int>(source_node_ID));

                                // Init:
                                // m_distances: already initialized to infinity by default.
                                // m_prev_node: idem
                                // m_prev_arc: idem
                                // m_visited: idem
                                size_t visitedCount = 0;
                                m_distances            [source_node_ID] = 0;
                                m_distances_non_visited[source_node_ID] = 0;

                                // Precompute all neighbors:
                                graph.getAdjacencyMatrix(m_allNeighbors);

                                TNodeID u;
                                do  // The algorithm:
                                {
                                        // Find the nodeID with the minimum known distance so far considered:
                                        double min_d = std::numeric_limits<double>::max();
                                        u = INVALID_NODEID;

                                        // No need to check if the min. distance node is not visited yet, since we
                                        // keep two lists: m_distances_non_visited & m_distances
                                        for (typename std::map<TNodeID,TDistance>::const_iterator itDist=m_distances_non_visited.begin();itDist!=m_distances_non_visited.end();++itDist)
                                        {
                                                if (itDist->second.dist < min_d)
                                                {
                                                        u = itDist->first;
                                                        min_d = itDist->second.dist;
                                                }
                                        }
                                        ASSERTMSG_(u!=INVALID_NODEID, "Graph is not fully connected!")

                                        // Save distance (for possible future reference...) and remove this node from "non-visited":
                                        m_distances[u]=m_distances_non_visited[u];
                                        m_distances_non_visited.erase(u);

                                        visitedCount++;

                                        // Let the user know about our progress...
                                        if (functor_on_progress) (*functor_on_progress)(graph,visitedCount);

                                        // For each arc from "u":
                                        const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u];     //graph.getNeighborsOf(u,neighborsOfU);
                                        for (std::set<TNodeID>::const_iterator itNei=neighborsOfU.begin();itNei!=neighborsOfU.end();++itNei)
                                        {
                                                const TNodeID i = *itNei;
                                                if (i==u) continue; // ignore self-loops...

                                                // the "edge_ui" may be searched here or a bit later, so the "bool" var will tell us.
                                                typename graph_t::const_iterator edge_ui;
                                                bool edge_ui_reverse = false;
                                                bool edge_ui_found = false;

                                                // Get weight of edge u<->i
                                                double edge_ui_weight;
                                                if (!functor_edge_weight)
                                                        edge_ui_weight = 1.;
                                                else
                                                {       // edge may be i->u or u->i:
                                                        edge_ui = graph.edges.find( make_pair(u,i) );
                                                        if ( edge_ui==graph.edges.end() )
                                                        {
                                                                edge_ui = graph.edges.find( make_pair(i,u));
                                                                edge_ui_reverse = true;
                                                        }
                                                        ASSERT_(edge_ui!=graph.edges.end());
                                                        edge_ui_weight = (*functor_edge_weight)( graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second );
                                                        edge_ui_found = true;
                                                }

                                                if ( (min_d+edge_ui_weight) < m_distances[i].dist) // the [] creates the entry if needed
                                                {
                                                        m_distances[i].dist = m_distances_non_visited[i].dist = min_d+edge_ui_weight;
                                                        m_prev_node[i].id = u;
                                                        // If still not done above, detect the direction of the arc now:
                                                        if (!edge_ui_found)
                                                        {
                                                                edge_ui = graph.edges.find( make_pair(u,i) );
                                                                if ( edge_ui==graph.edges.end() ) {
                                                                        edge_ui = graph.edges.find( make_pair(i,u));
                                                                        edge_ui_reverse = true;
                                                                }
                                                                ASSERT_(edge_ui!=graph.edges.end());
                                                        }

                                                        if ( !edge_ui_reverse )
                                                                        m_prev_arc[i] = make_pair(u,i); // *u -> *i
                                                        else    m_prev_arc[i] = make_pair(i,u); // *i -> *u
                                                }
                                        }
                                } while ( visitedCount<nNodes );

                                MRPT_END
                        } // end Dijkstra


                        /** @name Query Dijkstra results
                            @{ */

                        /** Return the distance from the root node to any other node using the Dijkstra-generated tree \exception std::exception On unknown node ID
                          */
                        inline double getNodeDistanceToRoot(const TNodeID id) const {
                                typename id2dist_map_t::const_iterator it=m_distances.find(id);
                                if (it==m_distances.end()) THROW_EXCEPTION("Node was not found in the graph when running Dijkstra");
                                return it->second.dist;
                        }

                        /** Return the set of all known node IDs (actually, a const ref to the internal set object). */
                        inline const std::set<TNodeID> & getListOfAllNodes() const {return m_lstNode_IDs;}

                        /** Return the node ID of the tree root, as passed in the constructor */
                        inline TNodeID getRootNodeID() const { return m_source_node_ID; }

                        /** Return the adjacency matrix of the input graph, which is cached at construction so if needed later just use this copy to avoid recomputing it \sa  mrpt::graphs::CDirectedGraph::getAdjacencyMatrix */
                        inline const list_all_neighbors_t & getCachedAdjacencyMatrix() const { return m_allNeighbors; }

                        /** Returns the shortest path between the source node passed in the constructor and the given target node.
                          * The reconstructed path contains a list of arcs (all of them exist in the graph with the given direction), such as the
                          *  the first edge starts at the origin passed in the constructor, and the last one contains the given target.
                          *
                          * \note An empty list of edges is returned when target equals the source node.
                          * \sa getTreeGraph
                          */
                        void getShortestPathTo(
                                const TNodeID   target_node_ID,
                                edge_list_t &out_path
                                ) const
                        {
                                out_path.clear();
                                if (target_node_ID==m_source_node_ID) return;

                                TNodeID nod = target_node_ID;
                                do
                                {
                                        typename id2pairIDs_map_t::const_iterator it = m_prev_arc.find(nod);
                                        ASSERT_(it!=m_prev_arc.end())

                                        out_path.push_front( it->second );
                                        nod = m_prev_node.find(nod)->second.id;
                                } while (nod!=m_source_node_ID);

                        } // end of getShortestPathTo


                        /** Type for graph returned by \a getTreeGraph: a graph like the original input graph, but with edge data being pointers to the original data (to save copy time & memory)
                          */
                        typedef CDirectedTree<const edge_t *>  tree_graph_t;

                        /** Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the root node.
                          * Note that the annotations on each edge in the tree are "const pointers" to the original graph edge data, so
                          * it's mandatory for the original input graph not to be deleted as long as this tree is used.
                          * \sa getShortestPathTo
                          */
                        void getTreeGraph( tree_graph_t &out_tree ) const
                        {
                                typedef typename tree_graph_t::TEdgeInfo TreeEdgeInfo;

                                out_tree.clear();
                                out_tree.root = m_source_node_ID;
                                for (typename id2pairIDs_map_t::const_iterator itArcs=m_prev_arc.begin();itArcs!=m_prev_arc.end();++itArcs)
                                {       // For each saved arc in "m_prev_arc", recover the original data in the input graph and save it to the output tree structure.
                                        const TNodeID id      = itArcs->first;
                                        const TNodeID id_from = itArcs->second.first;
                                        const TNodeID id_to   = itArcs->second.second;

                                        std::list<TreeEdgeInfo> &edges = out_tree.edges_to_children[id==id_from ? id_to : id_from];
                                        TreeEdgeInfo newEdge;
                                        newEdge.reverse = (id==id_from); // true: root towards leafs.
                                        newEdge.id = id;
                                        typename graph_t::edges_map_t::const_iterator itEdgeData = m_cached_graph.edges.find(make_pair(id_from,id_to));
                                        ASSERTMSG_(itEdgeData!=m_cached_graph.edges.end(),format("Edge %u->%u is in Dijkstra paths but not in original graph!",static_cast<unsigned int>(id_from),static_cast<unsigned int>(id_to) ))
                                        newEdge.data = & itEdgeData->second;
                                        edges.push_back( newEdge );
                                }

                        }// end getTreeGraph



                        /** @} */

                }; // end class

        } // End of namespace
} // End of namespace
#endif