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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020