Main MRPT website > C++ reference
MRPT logo

dijkstra.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  MRPT_DIJKSTRA_H
00029 #define  MRPT_DIJKSTRA_H
00030 
00031 #include <mrpt/math/graphs.h>
00032 #include <mrpt/utils/stl_extensions.h>
00033 
00034 namespace mrpt
00035 {
00036         namespace math
00037         {
00038             using namespace std;
00039                 using namespace mrpt::utils;
00040 
00041                 /** @name Graph-related classes
00042                     @{ */
00043 
00044                 /** 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.
00045                   *  The constructor takes as input the graph (the set of directed edges) computes all the needed data, then
00046                   *   successive calls to \a getShortestPathTo return the paths efficiently from the root.
00047                   *  The entire generated tree can be also retrieved with \a getTreeGraph.
00048                   *
00049                   *  Input graphs are represented by instances of (or classes derived from) mrpt::math::CDirectedGraph, and node's IDs are uint64_t values,
00050                   *   although the type mrpt::utils::TNodeID is also provided for clarity in the code.
00051                   *
00052                   *  The second template argument MAPS_IMPLEMENTATION allows choosing between a sparse std::map<> representation (using mrpt::utils::map_traits_stdmap)
00053                   *   for several intermediary and final results, and an alternative (using mrpt::utils::map_traits_map_as_vector as argument)
00054                   *   dense implementation which is much faster, but can be only used if the TNodeID's start in 0 or a low value.
00055                   *
00056                   * See <a href="http://www.mrpt.org/Example:Dijkstra_optimal_path_search_in_graphs" > this page </a> for a complete example.
00057                   */
00058                 template<class TYPE_EDGES, class MAPS_IMPLEMENTATION = map_traits_stdmap >
00059                 class CDijkstra
00060                 {
00061                 protected:
00062                         /** Auxiliary struct for topological distances from root node */
00063                         struct TDistance
00064                         {
00065                                 double dist;
00066                                 inline TDistance() : dist( std::numeric_limits<double>::max() ) { }
00067                                 inline TDistance(const double D) : dist(D) { }
00068                                 inline const TDistance & operator =(const double D) { dist = D; return *this;}
00069                         };
00070 
00071                         /** Auxiliary struct for backward paths */
00072                         struct TPrevious
00073                         {
00074                                 inline TPrevious() : id( INVALID_NODEID ) { }
00075                                 TNodeID  id;
00076                         };
00077 
00078                         // Cached input data:
00079                         const mrpt::math::CDirectedGraph<TYPE_EDGES>  & m_cached_graph;
00080                         const TNodeID                                   m_source_node_ID;
00081 
00082                         // Private typedefs:
00083                         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.
00084                         typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs>  id2pairIDs_map_t;
00085                         typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance>     id2dist_map_t;
00086                         typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious>     id2id_map_t;
00087 
00088                         // Intermediary and final results:
00089                         id2dist_map_t                  m_distances; //!< All the distances
00090                         std::map<TNodeID,TDistance>    m_distances_non_visited; // Use a std::map here in all cases.
00091                         id2id_map_t                    m_prev_node;
00092                         id2pairIDs_map_t               m_prev_arc;
00093                         std::set<TNodeID>              m_lstNode_IDs;
00094                         list_all_neighbors_t           m_allNeighbors;
00095 
00096                 public:
00097                         /** @name Useful typedefs 
00098                             @{ */
00099 
00100                         typedef mrpt::math::CDirectedGraph<TYPE_EDGES>  graph_t;        //!< The type of a graph with TYPE_EDGES edges
00101                         typedef TYPE_EDGES                              edge_t;     //!< The type of edge data in graph_t
00102                         typedef std::list<TPairNodeIDs>                 edge_list_t; //!< A list of edges used to describe a path on the graph
00103                         
00104                         /** @} */
00105 
00106                         /** Constructor, which takes the input graph and executes the entire Dijkstra algorithm from the given root node ID.
00107                           *
00108                           *  The graph is given by the set of directed edges, stored in a mrpt::math::CDirectedGraph class.
00109                           *
00110                           *  If a function \a functor_edge_weight is provided, it will be used to compute the weight of edges.
00111                           *  Otherwise, all edges weight the unity.
00112                           *
00113                           *  After construction, call \a getShortestPathTo to get the shortest path to a node or \a getTreeGraph for the tree representation.
00114                           *
00115                           * \sa getShortestPathTo, getTreeGraph
00116                           * \exception std::exception If the source nodeID is not found in the graph
00117                           */
00118                         CDijkstra(
00119                                 const graph_t  &graph,
00120                                 const TNodeID   source_node_ID,
00121                                 double (*functor_edge_weight)(const graph_t& graph, const TNodeID id_from, const TNodeID id_to, const TYPE_EDGES &edge) =  NULL,
00122                                 void   (*functor_on_progress)(const graph_t& graph, size_t visitedCount) = NULL
00123                                 )
00124                                 : m_cached_graph(graph), m_source_node_ID(source_node_ID)
00125                         {
00126                                 MRPT_START
00127                                 /*
00128                                 1  function Dijkstra(G, w, s)
00129                                 2     for each vertex v in V[G]                        // Initializations
00130                                 3           m_distances[v] := infinity
00131                                 4           m_prev_node[v] := undefined
00132                                 5     m_distances[s] := 0
00133                                 6     S := empty set
00134                                 7     Q := V[G]
00135                                 8     while Q is not an empty set                      // The algorithm itself
00136                                 9           u := Extract_Min(Q)
00137                                 10           S := S union {u}
00138                                 11           for each edge (u,v) outgoing from u
00139                                 12                  if m_distances[u] + w(u,v) < m_distances[v]             // Relax (u,v)
00140                                 13                        m_distances[v] := m_distances[u] + w(u,v)
00141                                 14                        m_prev_node[v] := u
00142                                 */
00143 
00144                                 // Makea list of all the nodes in the graph:
00145                                 graph.getAllNodes( m_lstNode_IDs );
00146                                 const size_t nNodes = m_lstNode_IDs.size();
00147 
00148                                 if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() )
00149                                         THROW_EXCEPTION_CUSTOM_MSG1("Cannot find the source node_ID=%u in the graph",static_cast<unsigned int>(source_node_ID));
00150 
00151                                 // Init:
00152                                 // m_distances: already initialized to infinity by default.
00153                                 // m_prev_node: idem
00154                                 // m_prev_arc: idem
00155                                 // m_visited: idem
00156                                 size_t visitedCount = 0;
00157                                 m_distances            [source_node_ID] = 0;
00158                                 m_distances_non_visited[source_node_ID] = 0;
00159 
00160                                 // Precompute all neighbors:
00161                                 graph.getAdjacencyMatrix(m_allNeighbors);
00162 
00163                                 TNodeID u;
00164                                 do  // The algorithm:
00165                                 {
00166                                         // Find the nodeID with the minimum known distance so far considered:
00167                                         double min_d = std::numeric_limits<double>::max();
00168                                         u = INVALID_NODEID;
00169 
00170                                         // No need to check if the min. distance node is not visited yet, since we
00171                                         // keep two lists: m_distances_non_visited & m_distances
00172                                         for (typename std::map<TNodeID,TDistance>::const_iterator itDist=m_distances_non_visited.begin();itDist!=m_distances_non_visited.end();++itDist)
00173                                         {
00174                                                 if (itDist->second.dist < min_d)
00175                                                 {
00176                                                         u = itDist->first;
00177                                                         min_d = itDist->second.dist;
00178                                                 }
00179                                         }
00180                                         ASSERTMSG_(u!=INVALID_NODEID, "Graph is not fully connected!")
00181 
00182                                         // Save distance (for possible future reference...) and remove this node from "non-visited":
00183                                         m_distances[u]=m_distances_non_visited[u];
00184                                         m_distances_non_visited.erase(u);
00185 
00186                                         visitedCount++;
00187 
00188                                         // Let the user know about our progress...
00189                                         if (functor_on_progress) (*functor_on_progress)(graph,visitedCount);
00190 
00191                                         // For each arc from "u":
00192                                         const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u];     //graph.getNeighborsOf(u,neighborsOfU);
00193                                         for (std::set<TNodeID>::const_iterator itNei=neighborsOfU.begin();itNei!=neighborsOfU.end();++itNei)
00194                                         {
00195                                                 const TNodeID i = *itNei;
00196                                                 if (i==u) continue; // ignore self-loops...
00197 
00198                                                 // the "edge_ui" may be searched here or a bit later, so the "bool" var will tell us.
00199                                                 typename graph_t::const_iterator edge_ui;
00200                                                 bool edge_ui_reverse = false;
00201                                                 bool edge_ui_found = false;
00202 
00203                                                 // Get weight of edge u<->i
00204                                                 double edge_ui_weight;
00205                                                 if (!functor_edge_weight)
00206                                                         edge_ui_weight = 1.;
00207                                                 else
00208                                                 {       // edge may be i->u or u->i:
00209                                                         edge_ui = graph.edges.find( make_pair(u,i) );
00210                                                         if ( edge_ui==graph.edges.end() )
00211                                                         {
00212                                                                 edge_ui = graph.edges.find( make_pair(i,u));
00213                                                                 edge_ui_reverse = true;
00214                                                         }
00215                                                         ASSERT_(edge_ui!=graph.edges.end());
00216                                                         edge_ui_weight = (*functor_edge_weight)( graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second );
00217                                                         edge_ui_found = true;
00218                                                 }
00219 
00220                                                 if ( (min_d+edge_ui_weight) < m_distances[i].dist) // the [] creates the entry if needed
00221                                                 {
00222                                                         m_distances[i].dist = m_distances_non_visited[i].dist = min_d+edge_ui_weight;
00223                                                         m_prev_node[i].id = u;
00224                                                         // If still not done above, detect the direction of the arc now:
00225                                                         if (!edge_ui_found)
00226                                                         {
00227                                                                 edge_ui = graph.edges.find( make_pair(u,i) );
00228                                                                 if ( edge_ui==graph.edges.end() ) {
00229                                                                         edge_ui = graph.edges.find( make_pair(i,u));
00230                                                                         edge_ui_reverse = true;
00231                                                                 }
00232                                                                 ASSERT_(edge_ui!=graph.edges.end());
00233                                                         }
00234 
00235                                                         if ( !edge_ui_reverse )
00236                                                                         m_prev_arc[i] = make_pair(u,i); // *u -> *i
00237                                                         else    m_prev_arc[i] = make_pair(i,u); // *i -> *u
00238                                                 }
00239                                         }
00240                                 } while ( visitedCount<nNodes );
00241 
00242                                 MRPT_END
00243                         } // end Dijkstra
00244 
00245 
00246                         /** @name Query Dijkstra results
00247                             @{ */
00248 
00249                         /** Return the distance from the root node to any other node using the Dijkstra-generated tree \exception std::exception On unknown node ID
00250                           */
00251                         inline double getNodeDistanceToRoot(const TNodeID id) const {
00252                                 typename id2dist_map_t::const_iterator it=m_distances.find(id);
00253                                 if (it==m_distances.end()) THROW_EXCEPTION("Node was not found in the graph when running Dijkstra");
00254                                 return it->second.dist;
00255                         }
00256 
00257                         /** Return the set of all known node IDs (actually, a const ref to the internal set object). */
00258                         inline const std::set<TNodeID> & getListOfAllNodes() const {return m_lstNode_IDs;}
00259 
00260                         /** Return the node ID of the tree root, as passed in the constructor */
00261                         inline TNodeID getRootNodeID() const { return m_source_node_ID; }
00262 
00263                         /** 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::math::CDirectedGraph::getAdjacencyMatrix */
00264                         inline const list_all_neighbors_t & getCachedAdjacencyMatrix() const { return m_allNeighbors; }
00265 
00266                         /** Returns the shortest path between the source node passed in the constructor and the given target node.
00267                           * The reconstructed path contains a list of arcs (all of them exist in the graph with the given direction), such as the
00268                           *  the first edge starts at the origin passed in the constructor, and the last one contains the given target.
00269                           *
00270                           * \note An empty list of edges is returned when target equals the source node.
00271                           * \sa getTreeGraph
00272                           */
00273                         void getShortestPathTo(
00274                                 const TNodeID   target_node_ID,
00275                                 edge_list_t &out_path
00276                                 ) const
00277                         {
00278                                 out_path.clear();
00279                                 if (target_node_ID==m_source_node_ID) return;
00280 
00281                                 TNodeID nod = target_node_ID;
00282                                 do
00283                                 {
00284                                         typename id2pairIDs_map_t::const_iterator it = m_prev_arc.find(nod);
00285                                         ASSERT_(it!=m_prev_arc.end())
00286 
00287                                         out_path.push_front( it->second );
00288                                         nod = m_prev_node.find(nod)->second.id;
00289                                 } while (nod!=m_source_node_ID);
00290 
00291                         } // end of getShortestPathTo
00292 
00293 
00294                         /** 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)
00295                           */
00296                         typedef CDirectedTree<const TYPE_EDGES*>  tree_graph_t;
00297 
00298                         /** Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the root node.
00299                           * Note that the annotations on each edge in the tree are "const pointers" to the original graph edge data, so
00300                           * it's mandatory for the original input graph not to be deleted as long as this tree is used.
00301                           * \sa getShortestPathTo
00302                           */
00303                         void getTreeGraph( tree_graph_t &out_tree ) const
00304                         {
00305                                 typedef typename tree_graph_t::TEdgeInfo TreeEdgeInfo;
00306 
00307                                 out_tree.clear();
00308                                 out_tree.root = m_source_node_ID;
00309                                 for (typename id2pairIDs_map_t::const_iterator itArcs=m_prev_arc.begin();itArcs!=m_prev_arc.end();++itArcs)
00310                                 {       // For each saved arc in "m_prev_arc", recover the original data in the input graph and save it to the output tree structure.
00311                                         const TNodeID id      = itArcs->first;
00312                                         const TNodeID id_from = itArcs->second.first;
00313                                         const TNodeID id_to   = itArcs->second.second;
00314 
00315                                         std::list<TreeEdgeInfo> &edges = out_tree.edges_to_children[id==id_from ? id_to : id_from];
00316                                         TreeEdgeInfo newEdge;
00317                                         newEdge.reverse = (id==id_from); // true: root towards leafs.
00318                                         newEdge.id = id;
00319                                         typename graph_t::edges_map_t::const_iterator itEdgeData = m_cached_graph.edges.find(make_pair(id_from,id_to));
00320                                         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) ))
00321                                         newEdge.data = & itEdgeData->second;
00322                                         edges.push_back( newEdge );
00323                                 }
00324 
00325                         }// end getTreeGraph
00326 
00327 
00328 
00329                         /** @} */
00330 
00331                 }; // end class
00332 
00333                 /** @} */
00334 
00335         } // End of namespace
00336 } // End of namespace
00337 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011