dijkstra.cpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <queue>
00018 #include <vector>
00019 #include <assert.h>
00020 #include "dijkstra.h"
00021 
00022 namespace AISNavigation{
00023 
00024   using namespace std;
00025 
00026   Dijkstra::AdjacencyMapEntry::AdjacencyMapEntry(Graph::Vertex* child_, 
00027                                                  Graph::Vertex* parent_, 
00028                                                  Graph::Edge* edge_, 
00029                                                  double distance_){
00030     _child=child_;
00031     _parent=parent_;
00032     _edge=edge_;
00033     _distance=distance_;
00034     std::set<AdjacencyMapEntry*> choldren;
00035   }
00036 
00037   Dijkstra::Dijkstra(Graph* g): _graph(g){
00038     for (Graph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
00039       AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
00040       _adjacencyMap.insert(make_pair(entry.child(), entry));
00041     }
00042   }
00043 
00044   void Dijkstra::reset(){
00045     for (Graph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
00046       AdjacencyMap::iterator at=_adjacencyMap.find(*it);
00047       assert(at!=_adjacencyMap.end());
00048       at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
00049     }
00050     _visited.clear();
00051   }
00052 
00053 
00054   bool operator<(const Dijkstra::AdjacencyMapEntry& a, const Dijkstra::AdjacencyMapEntry& b){
00055     return a.distance()>b.distance();
00056   }
00057 
00058 
00059   void Dijkstra::shortestPaths(Graph::Vertex* v, 
00060                                Dijkstra::CostFunction* cost, 
00061                                double maxDistance, 
00062                                double comparisonConditioner, 
00063                                bool directed){
00064     reset();
00065     std::priority_queue< AdjacencyMapEntry > frontier;
00066     AdjacencyMap::iterator it=_adjacencyMap.find(v);
00067     assert(it!=_adjacencyMap.end());
00068     it->second._distance=0.;
00069     frontier.push(it->second);
00070     
00071     while(! frontier.empty()){
00072       AdjacencyMapEntry entry=frontier.top();
00073       frontier.pop();
00074       Graph::Vertex* u=entry.child();
00075       AdjacencyMap::iterator ut=_adjacencyMap.find(u);
00076       assert(ut!=_adjacencyMap.end());
00077       double uDistance=ut->second.distance();
00078       
00079       std::pair< Graph::VertexSet::iterator, bool> insertResult=_visited.insert(u);
00080       Graph::EdgeSet::iterator et=u->edges().begin();   
00081       while(et!=u->edges().end()){
00082         Graph::Edge* edge=*et;
00083         et++;
00084         
00085         Graph::Vertex* z=0;
00086         if (edge->from()==u)
00087           z=edge->to();
00088         else if(edge->to()==u)
00089           z=edge->from();
00090         assert(z);
00091         
00092         if (directed && edge->from()!=u)
00093           continue;
00094 
00095         double edgeDistance=(*cost)(edge, u, z);
00096         if (edgeDistance==std::numeric_limits< double >::max())
00097           continue;
00098         double zDistance=uDistance+edgeDistance;
00099         AdjacencyMap::iterator ot=_adjacencyMap.find(z);
00100         assert(ot!=_adjacencyMap.end());
00101         
00102         if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
00103           ot->second._distance=zDistance;
00104           ot->second._parent=u;
00105           ot->second._edge=edge;
00106           frontier.push(ot->second);
00107         }
00108       }
00109     }
00110   }
00111 
00112   void Dijkstra::computeTree(Graph::Vertex* v __attribute__((unused)), AdjacencyMap& amap){
00113    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); it++){
00114       AdjacencyMapEntry& entry(it->second);
00115       entry._children.clear();
00116    }
00117    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); it++){
00118       AdjacencyMapEntry& entry(it->second);
00119       Graph::Vertex* parent=entry.parent();
00120       if (!parent){
00121         continue;
00122       }
00123       Graph::Vertex* v=entry.child();
00124       assert (v==it->first);
00125 
00126       AdjacencyMap::iterator pt=amap.find(parent);
00127       assert(pt!=amap.end());
00128       pt->second._children.insert(v);
00129    }
00130   }
00131 
00132 
00133   void Dijkstra::visitAdjacencyMap(Graph::Vertex* v, AdjacencyMap& amap, TreeAction* action){
00134     typedef std::deque<Graph::Vertex*> Deque;
00135     Deque q;
00136     action->perform(v,0,0);
00137     q.push_back(v);
00138     int count=0;
00139     while (! q.empty()){
00140       Graph::Vertex* parent=q.front();
00141       q.pop_front();
00142       count++;
00143       AdjacencyMap::iterator parentIt=amap.find(parent);
00144       if (parentIt==amap.end())
00145         continue;
00146       Graph::VertexSet& childs(parentIt->second.children());
00147       for (Graph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); childsIt++){
00148         Graph::Vertex* child=*childsIt;
00149         AdjacencyMap::iterator adjacencyIt=amap.find(child);
00150         assert (adjacencyIt!=amap.end());
00151         Graph::Edge* edge=adjacencyIt->second.edge();   
00152 
00153         assert(adjacencyIt->first==child);
00154         assert(adjacencyIt->second.child()==child);
00155         assert(adjacencyIt->second.parent()==parent);
00156         action->perform(child, parent, edge);
00157         q.push_back(child);
00158       } 
00159     }
00160     
00161   }
00162 
00163   void connectedSubset(Graph::VertexSet& connected, Graph::VertexSet& visited, 
00164                                     Graph::VertexSet& startingSet, 
00165                                     Graph* g, Graph::Vertex* v,
00166                                     Dijkstra::CostFunction* cost, double distance, double comparisonConditioner){
00167     typedef std::queue<Graph::Vertex*> VertexDeque;
00168     visited.clear();
00169     connected.clear();
00170     VertexDeque frontier;
00171     Dijkstra dv(g);
00172     connected.insert(v);
00173     frontier.push(v);
00174     while (! frontier.empty()){
00175       Graph::Vertex* v0=frontier.front();
00176       frontier.pop();
00177       dv.shortestPaths(v0,cost,distance,comparisonConditioner);
00178       for (Graph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); it++){
00179         visited.insert(*it);
00180         if (startingSet.find(*it)==startingSet.end())
00181           continue;
00182         std::pair<Graph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
00183         if (insertOutcome.second){ // the node was not in the connectedSet;
00184           frontier.push(dynamic_cast<Graph::Vertex*>(*it));
00185         }
00186       }
00187     }
00188   }
00189 
00190   double UniformCostFunction::operator () (Graph::Edge* edge __attribute__((unused)),
00191       Graph::Vertex* from __attribute__((unused)), Graph::Vertex* to __attribute__((unused))) {
00192     return 1.;
00193   }
00194 
00195 
00196 
00197 };


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58