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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020