Main MRPT website > C++ reference
MRPT logo
dijkstra.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef MRPT_DIJKSTRA_H
29 #define MRPT_DIJKSTRA_H
30 
31 #include <mrpt/graphs/CDirectedGraph.h>
32 #include <mrpt/graphs/CDirectedTree.h>
33 #include <mrpt/utils/stl_extensions.h>
34 
35 namespace mrpt
36 {
37  namespace graphs
38  {
39  using namespace std;
40  using namespace mrpt::utils;
41 
42  /** 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.
43  * The constructor takes as input the graph (the set of directed edges) computes all the needed data, then
44  * successive calls to \a getShortestPathTo return the paths efficiently from the root.
45  * The entire generated tree can be also retrieved with \a getTreeGraph.
46  *
47  * Input graphs are represented by instances of (or classes derived from) mrpt::graphs::CDirectedGraph, and node's IDs are uint64_t values,
48  * although the type mrpt::utils::TNodeID is also provided for clarity in the code.
49  *
50  * The second template argument MAPS_IMPLEMENTATION allows choosing between a sparse std::map<> representation (using mrpt::utils::map_traits_stdmap)
51  * for several intermediary and final results, and an alternative (using mrpt::utils::map_traits_map_as_vector as argument)
52  * dense implementation which is much faster, but can be only used if the TNodeID's start in 0 or a low value.
53  *
54  * See <a href="http://www.mrpt.org/Example:Dijkstra_optimal_path_search_in_graphs" > this page </a> for a complete example.
55  * \ingroup mrpt_graphs_grp
56  */
57  //Was: template<class TYPE_EDGES, class MAPS_IMPLEMENTATION = map_traits_stdmap >
58  template<class TYPE_GRAPH, class MAPS_IMPLEMENTATION = map_traits_stdmap >
59  class CDijkstra
60  {
61  protected:
62  /** Auxiliary struct for topological distances from root node */
63  struct TDistance
64  {
65  double dist;
66  inline TDistance() : dist( std::numeric_limits<double>::max() ) { }
67  inline TDistance(const double D) : dist(D) { }
68  inline const TDistance & operator =(const double D) { dist = D; return *this;}
69  };
70 
71  /** Auxiliary struct for backward paths */
72  struct TPrevious
73  {
74  inline TPrevious() : id( INVALID_NODEID ) { }
75  TNodeID id;
76  };
77 
78  // Cached input data:
79  const TYPE_GRAPH & m_cached_graph;
80  const TNodeID m_source_node_ID;
81 
82  // Private typedefs:
83  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.
84  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs> id2pairIDs_map_t;
85  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance> id2dist_map_t;
86  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious> id2id_map_t;
87 
88  // Intermediary and final results:
89  id2dist_map_t m_distances; //!< All the distances
90  std::map<TNodeID,TDistance> m_distances_non_visited; // Use a std::map here in all cases.
91  id2id_map_t m_prev_node;
92  id2pairIDs_map_t m_prev_arc;
93  std::set<TNodeID> m_lstNode_IDs;
94  list_all_neighbors_t m_allNeighbors;
95 
96  public:
97  /** @name Useful typedefs
98  @{ */
99 
100  typedef TYPE_GRAPH graph_t; //!< The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class
101  typedef typename graph_t::edge_t edge_t; //!< The type of edge data in graph_t
102  typedef std::list<TPairNodeIDs> edge_list_t; //!< A list of edges used to describe a path on the graph
103 
104  /** @} */
105 
106  /** Constructor, which takes the input graph and executes the entire Dijkstra algorithm from the given root node ID.
107  *
108  * The graph is given by the set of directed edges, stored in a mrpt::graphs::CDirectedGraph class.
109  *
110  * If a function \a functor_edge_weight is provided, it will be used to compute the weight of edges.
111  * Otherwise, all edges weight the unity.
112  *
113  * After construction, call \a getShortestPathTo to get the shortest path to a node or \a getTreeGraph for the tree representation.
114  *
115  * \sa getShortestPathTo, getTreeGraph
116  * \exception std::exception If the source nodeID is not found in the graph
117  */
118  CDijkstra(
119  const graph_t &graph,
120  const TNodeID source_node_ID,
121  double (*functor_edge_weight)(const graph_t& graph, const TNodeID id_from, const TNodeID id_to, const edge_t &edge) = NULL,
122  void (*functor_on_progress)(const graph_t& graph, size_t visitedCount) = NULL
123  )
124  : m_cached_graph(graph), m_source_node_ID(source_node_ID)
125  {
126  MRPT_START
127  /*
128  1 function Dijkstra(G, w, s)
129  2 for each vertex v in V[G] // Initializations
130  3 m_distances[v] := infinity
131  4 m_prev_node[v] := undefined
132  5 m_distances[s] := 0
133  6 S := empty set
134  7 Q := V[G]
135  8 while Q is not an empty set // The algorithm itself
136  9 u := Extract_Min(Q)
137  10 S := S union {u}
138  11 for each edge (u,v) outgoing from u
139  12 if m_distances[u] + w(u,v) < m_distances[v] // Relax (u,v)
140  13 m_distances[v] := m_distances[u] + w(u,v)
141  14 m_prev_node[v] := u
142  */
143 
144  // Makea list of all the nodes in the graph:
145  graph.getAllNodes( m_lstNode_IDs );
146  const size_t nNodes = m_lstNode_IDs.size();
147 
148  if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() )
149  THROW_EXCEPTION_CUSTOM_MSG1("Cannot find the source node_ID=%u in the graph",static_cast<unsigned int>(source_node_ID));
150 
151  // Init:
152  // m_distances: already initialized to infinity by default.
153  // m_prev_node: idem
154  // m_prev_arc: idem
155  // m_visited: idem
156  size_t visitedCount = 0;
157  m_distances [source_node_ID] = 0;
158  m_distances_non_visited[source_node_ID] = 0;
159 
160  // Precompute all neighbors:
161  graph.getAdjacencyMatrix(m_allNeighbors);
162 
163  TNodeID u;
164  do // The algorithm:
165  {
166  // Find the nodeID with the minimum known distance so far considered:
167  double min_d = std::numeric_limits<double>::max();
168  u = INVALID_NODEID;
169 
170  // No need to check if the min. distance node is not visited yet, since we
171  // keep two lists: m_distances_non_visited & m_distances
172  for (typename std::map<TNodeID,TDistance>::const_iterator itDist=m_distances_non_visited.begin();itDist!=m_distances_non_visited.end();++itDist)
173  {
174  if (itDist->second.dist < min_d)
175  {
176  u = itDist->first;
177  min_d = itDist->second.dist;
178  }
179  }
180  ASSERTMSG_(u!=INVALID_NODEID, "Graph is not fully connected!")
181 
182  // Save distance (for possible future reference...) and remove this node from "non-visited":
183  m_distances[u]=m_distances_non_visited[u];
184  m_distances_non_visited.erase(u);
185 
186  visitedCount++;
187 
188  // Let the user know about our progress...
189  if (functor_on_progress) (*functor_on_progress)(graph,visitedCount);
190 
191  // For each arc from "u":
192  const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u]; //graph.getNeighborsOf(u,neighborsOfU);
193  for (std::set<TNodeID>::const_iterator itNei=neighborsOfU.begin();itNei!=neighborsOfU.end();++itNei)
194  {
195  const TNodeID i = *itNei;
196  if (i==u) continue; // ignore self-loops...
197 
198  // the "edge_ui" may be searched here or a bit later, so the "bool" var will tell us.
199  typename graph_t::const_iterator edge_ui;
200  bool edge_ui_reverse = false;
201  bool edge_ui_found = false;
202 
203  // Get weight of edge u<->i
204  double edge_ui_weight;
205  if (!functor_edge_weight)
206  edge_ui_weight = 1.;
207  else
208  { // edge may be i->u or u->i:
209  edge_ui = graph.edges.find( make_pair(u,i) );
210  if ( edge_ui==graph.edges.end() )
211  {
212  edge_ui = graph.edges.find( make_pair(i,u));
213  edge_ui_reverse = true;
214  }
215  ASSERT_(edge_ui!=graph.edges.end());
216  edge_ui_weight = (*functor_edge_weight)( graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second );
217  edge_ui_found = true;
218  }
219 
220  if ( (min_d+edge_ui_weight) < m_distances[i].dist) // the [] creates the entry if needed
221  {
222  m_distances[i].dist = m_distances_non_visited[i].dist = min_d+edge_ui_weight;
223  m_prev_node[i].id = u;
224  // If still not done above, detect the direction of the arc now:
225  if (!edge_ui_found)
226  {
227  edge_ui = graph.edges.find( make_pair(u,i) );
228  if ( edge_ui==graph.edges.end() ) {
229  edge_ui = graph.edges.find( make_pair(i,u));
230  edge_ui_reverse = true;
231  }
232  ASSERT_(edge_ui!=graph.edges.end());
233  }
234 
235  if ( !edge_ui_reverse )
236  m_prev_arc[i] = make_pair(u,i); // *u -> *i
237  else m_prev_arc[i] = make_pair(i,u); // *i -> *u
238  }
239  }
240  } while ( visitedCount<nNodes );
241 
242  MRPT_END
243  } // end Dijkstra
244 
245 
246  /** @name Query Dijkstra results
247  @{ */
248 
249  /** Return the distance from the root node to any other node using the Dijkstra-generated tree \exception std::exception On unknown node ID
250  */
251  inline double getNodeDistanceToRoot(const TNodeID id) const {
252  typename id2dist_map_t::const_iterator it=m_distances.find(id);
253  if (it==m_distances.end()) THROW_EXCEPTION("Node was not found in the graph when running Dijkstra");
254  return it->second.dist;
255  }
256 
257  /** Return the set of all known node IDs (actually, a const ref to the internal set object). */
258  inline const std::set<TNodeID> & getListOfAllNodes() const {return m_lstNode_IDs;}
259 
260  /** Return the node ID of the tree root, as passed in the constructor */
261  inline TNodeID getRootNodeID() const { return m_source_node_ID; }
262 
263  /** 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 */
264  inline const list_all_neighbors_t & getCachedAdjacencyMatrix() const { return m_allNeighbors; }
265 
266  /** Returns the shortest path between the source node passed in the constructor and the given target node.
267  * The reconstructed path contains a list of arcs (all of them exist in the graph with the given direction), such as the
268  * the first edge starts at the origin passed in the constructor, and the last one contains the given target.
269  *
270  * \note An empty list of edges is returned when target equals the source node.
271  * \sa getTreeGraph
272  */
273  void getShortestPathTo(
274  const TNodeID target_node_ID,
275  edge_list_t &out_path
276  ) const
277  {
278  out_path.clear();
279  if (target_node_ID==m_source_node_ID) return;
280 
281  TNodeID nod = target_node_ID;
282  do
283  {
284  typename id2pairIDs_map_t::const_iterator it = m_prev_arc.find(nod);
285  ASSERT_(it!=m_prev_arc.end())
286 
287  out_path.push_front( it->second );
288  nod = m_prev_node.find(nod)->second.id;
289  } while (nod!=m_source_node_ID);
290 
291  } // end of getShortestPathTo
292 
293 
294  /** 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)
295  */
296  typedef CDirectedTree<const edge_t *> tree_graph_t;
297 
298  /** Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the root node.
299  * Note that the annotations on each edge in the tree are "const pointers" to the original graph edge data, so
300  * it's mandatory for the original input graph not to be deleted as long as this tree is used.
301  * \sa getShortestPathTo
302  */
303  void getTreeGraph( tree_graph_t &out_tree ) const
304  {
305  typedef typename tree_graph_t::TEdgeInfo TreeEdgeInfo;
306 
307  out_tree.clear();
308  out_tree.root = m_source_node_ID;
309  for (typename id2pairIDs_map_t::const_iterator itArcs=m_prev_arc.begin();itArcs!=m_prev_arc.end();++itArcs)
310  { // For each saved arc in "m_prev_arc", recover the original data in the input graph and save it to the output tree structure.
311  const TNodeID id = itArcs->first;
312  const TNodeID id_from = itArcs->second.first;
313  const TNodeID id_to = itArcs->second.second;
314 
315  std::list<TreeEdgeInfo> &edges = out_tree.edges_to_children[id==id_from ? id_to : id_from];
316  TreeEdgeInfo newEdge;
317  newEdge.reverse = (id==id_from); // true: root towards leafs.
318  newEdge.id = id;
319  typename graph_t::edges_map_t::const_iterator itEdgeData = m_cached_graph.edges.find(make_pair(id_from,id_to));
320  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) ))
321  newEdge.data = & itEdgeData->second;
322  edges.push_back( newEdge );
323  }
324 
325  }// end getTreeGraph
326 
327 
328 
329  /** @} */
330 
331  }; // end class
332 
333  } // End of namespace
334 } // End of namespace
335 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013