Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef MRPT_DIJKSTRA_H
00029 #define MRPT_DIJKSTRA_H
00030
00031 #include <mrpt/math/graphs.h>
00032 #include <mrpt/utils/stl_extensions.h>
00033
00034 namespace mrpt
00035 {
00036 namespace math
00037 {
00038 using namespace std;
00039 using namespace mrpt::utils;
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 template<class TYPE_EDGES, class MAPS_IMPLEMENTATION = map_traits_stdmap >
00059 class CDijkstra
00060 {
00061 protected:
00062
00063 struct TDistance
00064 {
00065 double dist;
00066 inline TDistance() : dist( std::numeric_limits<double>::max() ) { }
00067 inline TDistance(const double D) : dist(D) { }
00068 inline const TDistance & operator =(const double D) { dist = D; return *this;}
00069 };
00070
00071
00072 struct TPrevious
00073 {
00074 inline TPrevious() : id( INVALID_NODEID ) { }
00075 TNodeID id;
00076 };
00077
00078
00079 const mrpt::math::CDirectedGraph<TYPE_EDGES> & m_cached_graph;
00080 const TNodeID m_source_node_ID;
00081
00082
00083 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID, std::set<TNodeID> > list_all_neighbors_t;
00084 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPairNodeIDs> id2pairIDs_map_t;
00085 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TDistance> id2dist_map_t;
00086 typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,TPrevious> id2id_map_t;
00087
00088
00089 id2dist_map_t m_distances;
00090 std::map<TNodeID,TDistance> m_distances_non_visited;
00091 id2id_map_t m_prev_node;
00092 id2pairIDs_map_t m_prev_arc;
00093 std::set<TNodeID> m_lstNode_IDs;
00094 list_all_neighbors_t m_allNeighbors;
00095
00096 public:
00097
00098
00099
00100 typedef mrpt::math::CDirectedGraph<TYPE_EDGES> graph_t;
00101 typedef TYPE_EDGES edge_t;
00102 typedef std::list<TPairNodeIDs> edge_list_t;
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 CDijkstra(
00119 const graph_t &graph,
00120 const TNodeID source_node_ID,
00121 double (*functor_edge_weight)(const graph_t& graph, const TNodeID id_from, const TNodeID id_to, const TYPE_EDGES &edge) = NULL,
00122 void (*functor_on_progress)(const graph_t& graph, size_t visitedCount) = NULL
00123 )
00124 : m_cached_graph(graph), m_source_node_ID(source_node_ID)
00125 {
00126 MRPT_START
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145 graph.getAllNodes( m_lstNode_IDs );
00146 const size_t nNodes = m_lstNode_IDs.size();
00147
00148 if ( m_lstNode_IDs.find(source_node_ID)==m_lstNode_IDs.end() )
00149 THROW_EXCEPTION_CUSTOM_MSG1("Cannot find the source node_ID=%u in the graph",static_cast<unsigned int>(source_node_ID));
00150
00151
00152
00153
00154
00155
00156 size_t visitedCount = 0;
00157 m_distances [source_node_ID] = 0;
00158 m_distances_non_visited[source_node_ID] = 0;
00159
00160
00161 graph.getAdjacencyMatrix(m_allNeighbors);
00162
00163 TNodeID u;
00164 do
00165 {
00166
00167 double min_d = std::numeric_limits<double>::max();
00168 u = INVALID_NODEID;
00169
00170
00171
00172 for (typename std::map<TNodeID,TDistance>::const_iterator itDist=m_distances_non_visited.begin();itDist!=m_distances_non_visited.end();++itDist)
00173 {
00174 if (itDist->second.dist < min_d)
00175 {
00176 u = itDist->first;
00177 min_d = itDist->second.dist;
00178 }
00179 }
00180 ASSERTMSG_(u!=INVALID_NODEID, "Graph is not fully connected!")
00181
00182
00183 m_distances[u]=m_distances_non_visited[u];
00184 m_distances_non_visited.erase(u);
00185
00186 visitedCount++;
00187
00188
00189 if (functor_on_progress) (*functor_on_progress)(graph,visitedCount);
00190
00191
00192 const std::set<TNodeID> & neighborsOfU = m_allNeighbors[u];
00193 for (std::set<TNodeID>::const_iterator itNei=neighborsOfU.begin();itNei!=neighborsOfU.end();++itNei)
00194 {
00195 const TNodeID i = *itNei;
00196 if (i==u) continue;
00197
00198
00199 typename graph_t::const_iterator edge_ui;
00200 bool edge_ui_reverse = false;
00201 bool edge_ui_found = false;
00202
00203
00204 double edge_ui_weight;
00205 if (!functor_edge_weight)
00206 edge_ui_weight = 1.;
00207 else
00208 {
00209 edge_ui = graph.edges.find( make_pair(u,i) );
00210 if ( edge_ui==graph.edges.end() )
00211 {
00212 edge_ui = graph.edges.find( make_pair(i,u));
00213 edge_ui_reverse = true;
00214 }
00215 ASSERT_(edge_ui!=graph.edges.end());
00216 edge_ui_weight = (*functor_edge_weight)( graph, edge_ui->first.first,edge_ui->first.second, edge_ui->second );
00217 edge_ui_found = true;
00218 }
00219
00220 if ( (min_d+edge_ui_weight) < m_distances[i].dist)
00221 {
00222 m_distances[i].dist = m_distances_non_visited[i].dist = min_d+edge_ui_weight;
00223 m_prev_node[i].id = u;
00224
00225 if (!edge_ui_found)
00226 {
00227 edge_ui = graph.edges.find( make_pair(u,i) );
00228 if ( edge_ui==graph.edges.end() ) {
00229 edge_ui = graph.edges.find( make_pair(i,u));
00230 edge_ui_reverse = true;
00231 }
00232 ASSERT_(edge_ui!=graph.edges.end());
00233 }
00234
00235 if ( !edge_ui_reverse )
00236 m_prev_arc[i] = make_pair(u,i);
00237 else m_prev_arc[i] = make_pair(i,u);
00238 }
00239 }
00240 } while ( visitedCount<nNodes );
00241
00242 MRPT_END
00243 }
00244
00245
00246
00247
00248
00249
00250
00251 inline double getNodeDistanceToRoot(const TNodeID id) const {
00252 typename id2dist_map_t::const_iterator it=m_distances.find(id);
00253 if (it==m_distances.end()) THROW_EXCEPTION("Node was not found in the graph when running Dijkstra");
00254 return it->second.dist;
00255 }
00256
00257
00258 inline const std::set<TNodeID> & getListOfAllNodes() const {return m_lstNode_IDs;}
00259
00260
00261 inline TNodeID getRootNodeID() const { return m_source_node_ID; }
00262
00263
00264 inline const list_all_neighbors_t & getCachedAdjacencyMatrix() const { return m_allNeighbors; }
00265
00266
00267
00268
00269
00270
00271
00272
00273 void getShortestPathTo(
00274 const TNodeID target_node_ID,
00275 edge_list_t &out_path
00276 ) const
00277 {
00278 out_path.clear();
00279 if (target_node_ID==m_source_node_ID) return;
00280
00281 TNodeID nod = target_node_ID;
00282 do
00283 {
00284 typename id2pairIDs_map_t::const_iterator it = m_prev_arc.find(nod);
00285 ASSERT_(it!=m_prev_arc.end())
00286
00287 out_path.push_front( it->second );
00288 nod = m_prev_node.find(nod)->second.id;
00289 } while (nod!=m_source_node_ID);
00290
00291 }
00292
00293
00294
00295
00296 typedef CDirectedTree<const TYPE_EDGES*> tree_graph_t;
00297
00298
00299
00300
00301
00302
00303 void getTreeGraph( tree_graph_t &out_tree ) const
00304 {
00305 typedef typename tree_graph_t::TEdgeInfo TreeEdgeInfo;
00306
00307 out_tree.clear();
00308 out_tree.root = m_source_node_ID;
00309 for (typename id2pairIDs_map_t::const_iterator itArcs=m_prev_arc.begin();itArcs!=m_prev_arc.end();++itArcs)
00310 {
00311 const TNodeID id = itArcs->first;
00312 const TNodeID id_from = itArcs->second.first;
00313 const TNodeID id_to = itArcs->second.second;
00314
00315 std::list<TreeEdgeInfo> &edges = out_tree.edges_to_children[id==id_from ? id_to : id_from];
00316 TreeEdgeInfo newEdge;
00317 newEdge.reverse = (id==id_from);
00318 newEdge.id = id;
00319 typename graph_t::edges_map_t::const_iterator itEdgeData = m_cached_graph.edges.find(make_pair(id_from,id_to));
00320 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) ))
00321 newEdge.data = & itEdgeData->second;
00322 edges.push_back( newEdge );
00323 }
00324
00325 }
00326
00327
00328
00329
00330
00331 };
00332
00333
00334
00335 }
00336 }
00337 #endif