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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019