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 #ifndef _AIS_GRAPH_HH 00018 #define _AIS_GRAPH_HH 00019 00020 #include <map> 00021 #include <set> 00022 #include <vector> 00023 #include <limits> 00024 00027 namespace AISNavigation{ 00028 00029 00030 struct Graph{ 00031 friend class Dijkstra; 00032 00033 struct Vertex; 00034 struct Edge; 00035 00036 struct VertexIDCompare { 00037 bool operator() (const Vertex* v1, const Vertex* v2) const 00038 { 00039 return v1->id()<v2->id(); 00040 } 00041 }; 00042 00043 typedef std::set<Edge*> EdgeSet; 00044 typedef std::map<int, Vertex*> VertexIDMap; 00045 typedef std::set<Vertex*> VertexSet; 00046 00047 struct Vertex{ 00048 friend class Dijkstra; 00049 friend struct Graph; 00050 friend struct _DijkstraCompare; 00051 virtual ~Vertex(); 00052 inline int id() const {return _id;} 00053 inline const EdgeSet& edges() const {return _edges;} 00054 inline EdgeSet& edges() {return _edges;} 00055 00056 mutable bool _mark; 00057 protected: 00058 Vertex(int id=-1); 00059 int _id; 00060 EdgeSet _edges; 00061 }; 00062 00063 00064 struct Edge{ 00065 friend struct Graph; 00066 virtual ~Edge(); 00067 virtual bool revert(); 00068 inline const Vertex* from() const {return _from;} 00069 inline Vertex* from() {return _from;} 00070 inline const Vertex* to() const {return _to;} 00071 inline Vertex* to() {return _to;} 00072 00073 mutable bool _mark; 00074 protected: 00075 Edge(Vertex* from=0, Vertex* to=0); 00076 Vertex* _from; 00077 Vertex* _to; 00078 }; 00079 00080 00081 Vertex* vertex(int id); 00082 const Vertex* vertex(int id) const; 00083 EdgeSet connectingEdges(const Vertex* v1, const Vertex* v2); 00084 00085 00086 virtual bool removeVertex(Vertex* v); 00087 virtual bool removeEdge(Edge* e); 00088 Graph(); 00089 virtual ~Graph(); 00090 virtual void clear(); 00091 00092 inline const VertexIDMap& vertices() const {return _vertices;} 00093 inline VertexIDMap& vertices() {return _vertices;} 00094 inline const EdgeSet& edges() const {return _edges;} 00095 inline EdgeSet& edges() {return _edges;} 00096 00097 protected: 00098 Vertex* addVertex(Vertex* v); 00099 Edge* addEdge(Edge* e); 00100 00101 VertexIDMap _vertices; 00102 EdgeSet _edges; 00103 }; 00104 }; 00105 00107 00108 #endif