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



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Mon Oct 3 19:22:36 UTC 2016