Main MRPT website > C++ reference for MRPT 1.5.7
dijkstra.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef MRPT_DIJKSTRA_H
10 #define MRPT_DIJKSTRA_H
11 
14 #include <mrpt/utils/traits_map.h>
15 #include <mrpt/math/utils.h>
16 
17 #include <limits>
18 #include <iostream> // TODO - remove me
19 #include <vector>
20 #include <utility>
21 #include <exception>
22 
23 namespace mrpt
24 {
25  namespace graphs
26  {
27 
28  namespace detail {
29 
30  /**\brief Custom exception class that passes information in case an
31  * unconnected graph is passed to a Dijkstra instance
32  */
33  class NotConnectedGraph: public std::exception
34  {
35  public:
36 
38  const std::set<mrpt::utils::TNodeID>& unconnected_nodeIDs,
39  std::string err):
40  m_unconnected_nodeIDs(unconnected_nodeIDs),
41  m_err(err + "\n\n") { }
42 
43  using std::exception::what; // supress clang warning
44  const char* what() {
45  return m_err.c_str();
46  }
47 
48  ~NotConnectedGraph() throw() { }
49 
50  /**\brief Fill set with the nodeIDs Dijkstra algorithm could not reach
51  * starting from the root node.
52  */
53  void getUnconnectedNodeIDs(std::set<mrpt::utils::TNodeID>* set_nodeIDs) const {
54  ASSERTMSG_(set_nodeIDs, "\nSet of nodes pointer is invalid\n");
55 
56  // fil the given set
57  set_nodeIDs->clear();
59  it = m_unconnected_nodeIDs.begin();
60  it != m_unconnected_nodeIDs.end();
61  ++it) {
62  set_nodeIDs->insert(*it);
63  }
64 
65  }
66 
67  private:
68  std::set<mrpt::utils::TNodeID> m_unconnected_nodeIDs;
70  };
71  } // end of namespace - detail
72 
73  /** The Dijkstra algorithm for finding the shortest path between a given
74  * source node in a (weighted) directed graph and all other nodes in the
75  * form of a tree.
76  *
77  * The constructor takes as input the graph (the set of directed edges)
78  * computes all the needed data, then successive calls to \a
79  * getShortestPathTo return the paths efficiently from the root.
80  *
81  * The entire generated tree can be also retrieved with \a getTreeGraph.
82  *
83  * Input graphs are represented by instances of (or classes derived from)
84  * mrpt::graphs::CDirectedGraph, and node's IDs are uint64_t values,
85  * although the type mrpt::utils::TNodeID is also provided for clarity in
86  * the code.
87  *
88  * The second template argument MAPS_IMPLEMENTATION allows choosing
89  * between a sparse std::map<> representation (using *
90  * mrpt::utils::map_traits_stdmap) for several intermediary and final
91  * results, and an alternative (using
92  * mrpt::utils::map_traits_map_as_vector as argument) dense
93  * implementation which is much faster, but can be only used if the
94  * TNodeID's start in 0 or a low value.
95  *
96  * See <a
97  * href="http://www.mrpt.org/Example:Dijkstra_optimal_path_search_in_graphs"
98  * > this page </a> for a complete example.
99  *
100  * \ingroup mrpt_graphs_grp
101  */
102  template<class TYPE_GRAPH, class MAPS_IMPLEMENTATION = mrpt::utils::map_traits_stdmap >
103  class CDijkstra
104  {
105  protected:
106  /** Auxiliary struct for topological distances from root node */
107  struct TDistance
108  {
109  double dist;
110  inline TDistance() : dist(std::numeric_limits<double>::max()) { }
111  inline TDistance(const double D) : dist(D) { }
112  inline const TDistance & operator =(const double D) { dist = D; return *this;}
113  };
114 
115  /** Auxiliary struct for backward paths */
116  struct TPrevious
117  {
118  inline TPrevious() : id(INVALID_NODEID) { }
120  };
121 
122  // Cached input data:
123  const TYPE_GRAPH & m_cached_graph;
125 
126  // Private typedefs:
127  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.
128  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs> id2pairIDs_map_t;
129  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance> id2dist_map_t;
130  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious> id2id_map_t;
131 
132  // Intermediary and final results:
133  id2dist_map_t m_distances; //!< All the distances
134  std::map<TNodeID,TDistance> m_distances_non_visited; // Use a std::map here in all cases.
137  std::set<TNodeID> m_lstNode_IDs;
139 
140  public:
141  /** @name Useful typedefs
142  @{ */
143 
144  typedef TYPE_GRAPH graph_t; //!< The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class
145  typedef typename graph_t::edge_t edge_t; //!< The type of edge data in graph_t
146  typedef std::list<TPairNodeIDs> edge_list_t; //!< A list of edges used to describe a path on the graph
147 
148  /** @} */
149 
150  /** Constructor which takes the input graph and executes the entire
151  * Dijkstra algorithm from the given root node ID.
152  *
153  * The graph is given by the set of directed edges, stored in a
154  * mrpt::graphs::CDirectedGraph class.
155  *
156  * If a function \a functor_edge_weight is provided, it will be used to
157  * compute the weight of edges. Otherwise, all edges weight the unity.
158  *
159  * After construction, call \a getShortestPathTo to get the shortest
160  * path to a node or \a getTreeGraph for the tree representation.
161  *
162  * \sa getShortestPathTo, getTreeGraph
163  *
164  * \exception std::exception If the source nodeID is not found in the
165  * graph
166  */
168  const graph_t &graph,
169  const TNodeID source_node_ID,
170  double (*functor_edge_weight)(const graph_t& graph, const TNodeID id_from, const TNodeID id_to, const edge_t &edge) = NULL,
171  void (*functor_on_progress)(const graph_t& graph, size_t visitedCount) = NULL
172  )
173  : m_cached_graph(graph), m_source_node_ID(source_node_ID)
174  {
175  /*
176  1 function Dijkstra(G, w, s)
177  2 for each vertex v in V[G] // Initializations
178  3 m_distances[v] := infinity
179  4 m_prev_node[v] := undefined
180  5 m_distances[s] := 0
181  6 S := empty set
182  7 Q := V[G]
183  8 while Q is not an empty set // The algorithm itself
184  9 u := Extract_Min(Q)
185  10 S := S union {u}
186  11 for each edge (u,v) outgoing from u
187  12 if m_distances[u] + w(u,v) < m_distances[v] // Relax (u,v)
188  13 m_distances[v] := m_distances[u] + w(u,v)
189  14 m_prev_node[v] := u
190  */
191 
192  // Make a list of all the nodes in the graph:
193  graph.getAllNodes(m_lstNode_IDs);
194  const size_t nNodes = m_lstNode_IDs.size();
195 
196  if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() ) {
197  THROW_EXCEPTION_FMT("Cannot find the source node_ID=%lu in the graph",static_cast<unsigned long>(source_node_ID));
198  }
199 
200  // Init:
201  // m_distances: already initialized to infinity by default.
202  // m_prev_node: idem
203  // m_prev_arc: idem
204  // m_visited: idem
205  size_t visitedCount = 0;
206  m_distances[source_node_ID] = 0;
207  m_distances_non_visited[source_node_ID] = 0;
208 
209  // Precompute all neighbors of all the nodes in the given graph:
210  graph.getAdjacencyMatrix(m_allNeighbors);
211 
212  using namespace std;
213 
214  TNodeID u;
215  // as long as there are nodes not yet visited.
216  do { // The algorithm:
217  // Find the nodeID with the minimum known distance so far considered:
218  double min_d = std::numeric_limits<double>::max();
219  u = INVALID_NODEID;
220 
221  // No need to check if the min. distance node is not visited yet, since we
222  // keep two lists: m_distances_non_visited & m_distances
224  itDist=m_distances_non_visited.begin();
225  itDist != m_distances_non_visited.end();
226  ++itDist) {
227 
228  // TODO - remove these
229  //cout << "Current min distance: " << min_d << endl;
230  //cout << "itDist->first: " << itDist->first << endl;
231  //cout << "itDist->second (distance to this node): " << itDist->second.dist << endl;
232 
233  if (itDist->second.dist < min_d) {
234  u = itDist->first;
235  min_d = itDist->second.dist;
236  //cout << "updating u, min_d... " << endl;
237  }
238  //cout << "finished for." << endl;
239 
240  }
241 
242  // make sure we have found the next nodeID from the available non-visited distances
243  if (u == INVALID_NODEID) {
244  std::set<TNodeID> nodeIDs_unconnected;
245 
246  // for all the nodes in the graph
248  n_it = graph.nodes.begin();
249  n_it != graph.nodes.end();
250  ++n_it) {
251 
252  // have I already visited this node in Dijkstra?
253  bool have_traversed = false;
254  for (typename id2dist_map_t::const_iterator
255  d_it = m_distances.begin();
256  d_it != m_distances.end();
257  ++d_it) {
258  if (n_it->first == d_it->first) {
259  have_traversed = true;
260  break;
261  }
262  }
263 
264  if (!have_traversed) {
265  nodeIDs_unconnected.insert(n_it->first);
266  }
267  }
268 
269  std::string err_str = mrpt::format("Graph is not fully connected!");
271  nodeIDs_unconnected, err_str);
272  }
273 
274  // Update distance (for possible future reference...) and remove this node from "non-visited":
276  m_distances_non_visited.erase(u);
277 
278  visitedCount++;
279 
280  // Let the user know about our progress...
281  if (functor_on_progress) (*functor_on_progress)(graph, visitedCount);
282 
283  // For each arc from "u":
284  const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u]; //graph.getNeighborsOf(u,neighborsOfU);
285  for (std::set<TNodeID>::const_iterator itNei = neighborsOfU.begin(); itNei != neighborsOfU.end(); ++itNei) {
286  const TNodeID i = *itNei;
287  if (i == u) continue; // ignore self-loops...
288 
289  // the "edge_ui" may be searched here or a bit later, so the "bool" var will tell us.
290  typename graph_t::const_iterator edge_ui;
291  bool edge_ui_reverse = false;
292  bool edge_ui_found = false;
293 
294  // Get weight of edge u<->i
295  double edge_ui_weight;
296  if (!functor_edge_weight)
297  edge_ui_weight = 1.;
298  else { // edge may be i->u or u->i:
299  edge_ui = graph.edges.find(std::make_pair(u,i));
300  if (edge_ui==graph.edges.end()) {
301  edge_ui = graph.edges.find(std::make_pair(i,u));
302  edge_ui_reverse = true;
303  }
304  ASSERT_(edge_ui!=graph.edges.end());
305  edge_ui_weight = (*functor_edge_weight)(graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second);
306  edge_ui_found = true;
307  }
308 
309  if ((min_d+edge_ui_weight) < m_distances[i].dist) { // the [] creates the entry if needed
310  // update m_distances, m_distances_non_visited
311  m_distances_non_visited[i].dist = min_d + edge_ui_weight;
312  m_distances[i].dist = min_d + edge_ui_weight;
313 
314  m_prev_node[i].id = u;
315  // If still not done above, detect the direction of the arc now:
316  if (!edge_ui_found) {
317  edge_ui = graph.edges.find(std::make_pair(u,i));
318  if (edge_ui==graph.edges.end()) {
319  edge_ui = graph.edges.find(std::make_pair(i,u));
320  edge_ui_reverse = true;
321  }
322  ASSERT_(edge_ui!=graph.edges.end());
323  }
324 
325  if (!edge_ui_reverse)
326  m_prev_arc[i] = std::make_pair(u,i); // *u -> *i
327  else m_prev_arc[i] = std::make_pair(i,u); // *i -> *u
328  }
329  }
330  } while (visitedCount < nNodes);
331 
332  } // end Dijkstra
333 
334 
335  /** @name Query Dijkstra results
336  @{ */
337 
338  /** Return the distance from the root node to any other node using the
339  * Dijkstra-generated tree
340  *
341  * \exception std::exception On unknown node ID
342  */
343  inline double getNodeDistanceToRoot(const TNodeID id) const {
344  typename id2dist_map_t::const_iterator it=m_distances.find(id);
345  if (it==m_distances.end()) THROW_EXCEPTION("Node was not found in the graph when running Dijkstra");
346  return it->second.dist;
347  }
348 
349  /** Return the set of all known node IDs (actually, a const ref to the internal set object). */
350  inline const std::set<TNodeID> & getListOfAllNodes() const {return m_lstNode_IDs;}
351 
352  /** Return the node ID of the tree root, as passed in the constructor */
353  inline TNodeID getRootNodeID() const { return m_source_node_ID; }
354 
355  /** Return the adjacency matrix of the input graph, which is cached at
356  * construction so if needed later just use this copy to avoid
357  * recomputing it
358  *
359  * \sa mrpt::graphs::CDirectedGraph::getAdjacencyMatrix
360  * */
362  return m_allNeighbors; }
363 
364  /** Returns the shortest path between the source node passed in the
365  * constructor and the given target node. The reconstructed path
366  * contains a list of arcs (all of them exist in the graph with the given
367  * direction), such as the the first edge starts at the origin passed in
368  * the constructor, and the last one contains the given target.
369  *
370  * \note An empty list of edges is returned when target equals the source
371  * node.
372  *
373  * \sa getTreeGraph
374  */
376  const TNodeID target_node_ID,
377  edge_list_t &out_path) const {
378 
379  out_path.clear();
380  if (target_node_ID==m_source_node_ID) return;
381 
382  TNodeID nod = target_node_ID;
383  do {
384  typename id2pairIDs_map_t::const_iterator it = m_prev_arc.find(nod);
385  ASSERT_(it!=m_prev_arc.end())
386 
387  out_path.push_front(it->second);
388  nod = m_prev_node.find(nod)->second.id;
389  } while (nod!=m_source_node_ID);
390 
391  } // end of getShortestPathTo
392 
393 
394  /** Type for graph returned by \a getTreeGraph: a graph like the original
395  * input graph, but with edge data being pointers to the original data
396  * (to save copy time & memory)
397  */
399 
400  /** Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the root node.
401  * Note that the annotations on each edge in the tree are "const pointers" to the original graph edge data, so
402  * it's mandatory for the original input graph not to be deleted as long as this tree is used.
403  * \sa getShortestPathTo
404  */
405  void getTreeGraph(tree_graph_t &out_tree) const
406  {
407  typedef typename tree_graph_t::TEdgeInfo TreeEdgeInfo;
408 
409  out_tree.clear();
410  out_tree.root = m_source_node_ID;
411  // For each saved arc in "m_prev_arc", recover the original data in the input graph and save it to the output tree structure.
412  for (typename id2pairIDs_map_t::const_iterator itArcs=m_prev_arc.begin();
413  itArcs!=m_prev_arc.end();
414  ++itArcs) {
415  const TNodeID id = itArcs->first;
416  const TNodeID id_from = itArcs->second.first;
417  const TNodeID id_to = itArcs->second.second;
418 
419  std::list<TreeEdgeInfo> &edges = out_tree.edges_to_children[id==id_from ? id_to : id_from];
420  TreeEdgeInfo newEdge(id);
421  newEdge.reverse = (id==id_from); // true: root towards leafs.
422  typename graph_t::edges_map_t::const_iterator itEdgeData = m_cached_graph.edges.find(std::make_pair(id_from,id_to));
423  ASSERTMSG_(
424  itEdgeData!=m_cached_graph.edges.end(),format(
425  "Edge %u->%u is in Dijkstra paths but not in original graph!",static_cast<unsigned int>(id_from),static_cast<unsigned int>(id_to)))
426  newEdge.data = & itEdgeData->second;
427  edges.push_back(newEdge);
428  }
429 
430  } // end getTreeGraph
431 
432 
433 
434  /** @} */
435 
436  }; // end class
437 
438  } // End of namespace
439 } // End of namespace
440 #endif
MAPS_IMPLEMENTATION::template map< TNodeID, TDistance > id2dist_map_t
Definition: dijkstra.h:129
Auxiliary struct for topological distances from root node.
Definition: dijkstra.h:107
const list_all_neighbors_t & getCachedAdjacencyMatrix() const
Return the adjacency matrix of the input graph, which is cached at construction so if needed later ju...
Definition: dijkstra.h:361
TMapNode2ListEdges edges_to_children
The edges of each node.
Definition: CDirectedTree.h:67
< Make available this typedef in this namespace too
Definition: CDirectedTree.h:48
TYPE_GRAPH graph_t
The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class...
Definition: dijkstra.h:144
std::map< TNodeID, TDistance > m_distances_non_visited
Definition: dijkstra.h:134
#define THROW_EXCEPTION(msg)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Scalar * iterator
Definition: eigen_plugins.h:23
#define INVALID_NODEID
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
const TDistance & operator=(const double D)
Definition: dijkstra.h:112
TNodeID getRootNodeID() const
Return the node ID of the tree root, as passed in the constructor.
Definition: dijkstra.h:353
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)
Constructor which takes the input graph and executes the entire Dijkstra algorithm from the given roo...
Definition: dijkstra.h:167
id2dist_map_t m_distances
All the distances.
Definition: dijkstra.h:133
MAPS_IMPLEMENTATION::template map< TNodeID, TPairNodeIDs > id2pairIDs_map_t
Definition: dijkstra.h:128
const TNodeID m_source_node_ID
Definition: dijkstra.h:124
void getUnconnectedNodeIDs(std::set< mrpt::utils::TNodeID > *set_nodeIDs) const
Fill set with the nodeIDs Dijkstra algorithm could not reach starting from the root node...
Definition: dijkstra.h:53
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
id2pairIDs_map_t m_prev_arc
Definition: dijkstra.h:136
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
TNodeID root
The root of the tree.
Definition: CDirectedTree.h:66
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
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 ...
Definition: dijkstra.h:127
MAPS_IMPLEMENTATION::template map< TNodeID, TPrevious > id2id_map_t
Definition: dijkstra.h:130
const std::set< TNodeID > & getListOfAllNodes() const
Return the set of all known node IDs (actually, a const ref to the internal set object).
Definition: dijkstra.h:350
list_all_neighbors_t m_allNeighbors
Definition: dijkstra.h:138
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
NotConnectedGraph(const std::set< mrpt::utils::TNodeID > &unconnected_nodeIDs, std::string err)
Definition: dijkstra.h:37
id2id_map_t m_prev_node
Definition: dijkstra.h:135
GLuint id
Definition: glext.h:3770
std::list< TPairNodeIDs > edge_list_t
A list of edges used to describe a path on the graph.
Definition: dijkstra.h:146
#define ASSERT_(f)
void getShortestPathTo(const TNodeID target_node_ID, edge_list_t &out_path) const
Returns the shortest path between the source node passed in the constructor and the given target node...
Definition: dijkstra.h:375
CDirectedTree< const edge_t * > tree_graph_t
Type for graph returned by getTreeGraph: a graph like the original input graph, but with edge data be...
Definition: dijkstra.h:398
std::set< TNodeID > m_lstNode_IDs
Definition: dijkstra.h:137
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:103
const TYPE_GRAPH & m_cached_graph
Definition: dijkstra.h:123
Auxiliary struct for backward paths.
Definition: dijkstra.h:116
void clear()
Empty all edge data and set "root" to INVALID_NODEID.
Definition: CDirectedTree.h:74
graph_t::edge_t edge_t
The type of edge data in graph_t.
Definition: dijkstra.h:145
#define ASSERTMSG_(f, __ERROR_MSG)
Custom exception class that passes information in case an unconnected graph is passed to a Dijkstra i...
Definition: dijkstra.h:33
double getNodeDistanceToRoot(const TNodeID id) const
Return the distance from the root node to any other node using the Dijkstra-generated tree...
Definition: dijkstra.h:343
void getTreeGraph(tree_graph_t &out_tree) const
Returns a tree representation of the graph, as determined by the Dijkstra shortest paths from the roo...
Definition: dijkstra.h:405
std::set< mrpt::utils::TNodeID > m_unconnected_nodeIDs
Definition: dijkstra.h:68



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019