Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 <limits>
16
17namespace 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 >
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.
71 std::set<TNodeID> m_lstNode_IDs;
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 {
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":
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
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 */
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
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:38
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
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
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
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
MAPS_IMPLEMENTATION::template map< TNodeID, TPairNodeIDs > id2pairIDs_map_t
Definition: dijkstra.h:62
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
TYPE_GRAPH graph_t
The type of the graph, typically a mrpt::graphs::CDirectedGraph<> or any other derived class.
Definition: dijkstra.h:78
TNodeID getRootNodeID() const
Return the node ID of the tree root, as passed in the constructor.
Definition: dijkstra.h:239
MAPS_IMPLEMENTATION::template map< TNodeID, TPrevious > id2id_map_t
Definition: dijkstra.h:64
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
id2id_map_t m_prev_node
Definition: dijkstra.h:69
std::map< TNodeID, TDistance > m_distances_non_visited
Definition: dijkstra.h:68
std::set< TNodeID > m_lstNode_IDs
Definition: dijkstra.h:71
id2pairIDs_map_t m_prev_arc
Definition: dijkstra.h:70
id2dist_map_t m_distances
All the distances.
Definition: dijkstra.h:67
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
graph_t::edge_t edge_t
The type of edge data in graph_t.
Definition: dijkstra.h:79
const TYPE_GRAPH & m_cached_graph
Definition: dijkstra.h:57
MAPS_IMPLEMENTATION::template map< TNodeID, TDistance > id2dist_map_t
Definition: dijkstra.h:63
list_all_neighbors_t m_allNeighbors
Definition: dijkstra.h:72
const TNodeID m_source_node_ID
Definition: dijkstra.h:58
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
std::list< TPairNodeIDs > edge_list_t
A list of edges used to describe a path on the graph.
Definition: dijkstra.h:80
< Make available this typedef in this namespace too
Definition: CDirectedTree.h:52
void clear()
Empty all edge data and set "root" to INVALID_NODEID.
Definition: CDirectedTree.h:77
TNodeID root
The root of the tree.
Definition: CDirectedTree.h:69
TMapNode2ListEdges edges_to_children
The edges of each node.
Definition: CDirectedTree.h:70
const Scalar * const_iterator
Definition: eigen_plugins.h:24
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
#define INVALID_NODEID
Definition: types_simple.h:47
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
#define MRPT_END
Definition: mrpt_macros.h:353
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
Auxiliary struct for topological distances from root node.
Definition: dijkstra.h:42
const TDistance & operator=(const double D)
Definition: dijkstra.h:46
Auxiliary struct for backward paths.
Definition: dijkstra.h:51



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:47:55 UTC 2021