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 <assert.h> 00018 #include <queue> 00019 #include "graph.h" 00020 00021 namespace AISNavigation{ 00022 Graph::Vertex::Vertex(int id){ 00023 _id=id; 00024 } 00025 00026 Graph::Vertex::~Vertex(){ 00027 } 00028 00029 Graph::Edge::Edge(Vertex* from_, Vertex* to_){ 00030 _from=from_; 00031 _to=to_; 00032 } 00033 00034 Graph::Edge::~Edge(){ 00035 } 00036 00037 bool Graph::Edge::revert(){ 00038 Graph::Vertex* ap=_from; 00039 _from=_to; 00040 _to=ap; 00041 return true; 00042 } 00043 00044 Graph::Vertex* Graph::vertex(int id) { 00045 VertexIDMap::iterator it=_vertices.find(id); 00046 if (it==_vertices.end()) 00047 return 0; 00048 return it->second; 00049 } 00050 00051 const Graph::Vertex* Graph::vertex(int id) const { 00052 VertexIDMap::const_iterator it=_vertices.find(id); 00053 if (it==_vertices.end()) 00054 return 0; 00055 return it->second; 00056 } 00057 00058 Graph::EdgeSet Graph::connectingEdges(const Graph::Vertex* from, const Graph::Vertex* to){ 00059 EdgeSet eset; 00060 for (EdgeSet::const_iterator it=from->edges().begin(); 00061 it!=from->edges().end(); 00062 it++){ 00063 Edge* e=*it; 00064 if (e->from()==from && e->to()==to) 00065 eset.insert(e); 00066 } 00067 return eset; 00068 } 00069 00070 Graph::Vertex* Graph::addVertex(Vertex* v){ 00071 Vertex* vn=vertex(v->id()); 00072 if (vn) 00073 return 0; 00074 _vertices.insert( std::make_pair(v->id(),v) ); 00075 return v; 00076 } 00077 00078 Graph::Edge* Graph::addEdge(Edge* e){ 00079 std::pair<EdgeSet::iterator, bool> result=_edges.insert(e); 00080 if (! result.second) 00081 return 0; 00082 e->from()->edges().insert(e); 00083 e->to()->edges().insert(e); 00084 return e; 00085 } 00086 00087 bool Graph::removeVertex(Vertex* v){ 00088 VertexIDMap::iterator it=_vertices.find(v->id()); 00089 if (it==_vertices.end()) 00090 return false; 00091 assert(it->second==v); 00092 //remove all edges which are entering or leaving v; 00093 EdgeSet tmp=v->edges(); 00094 for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); it++){ 00095 if (!removeEdge(*it)){ 00096 assert(0); 00097 } 00098 } 00099 _vertices.erase(it); 00100 delete v; 00101 return true; 00102 } 00103 00104 bool Graph::removeEdge(Edge* e){ 00105 EdgeSet::iterator it=_edges.find(e); 00106 if (it==_edges.end()) 00107 return false; 00108 _edges.erase(it); 00109 00110 it=e->from()->edges().find(e); 00111 assert(it!=e->from()->edges().end()); 00112 e->from()->edges().erase(it); 00113 00114 it=e->to()->edges().find(e); 00115 assert(it!=e->to()->edges().end()); 00116 e->to()->edges().erase(it); 00117 00118 delete e; 00119 return true; 00120 00121 } 00122 00123 Graph::Graph(){ 00124 } 00125 00126 void Graph::clear(){ 00127 for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); it++){ 00128 delete (it->second); 00129 } 00130 for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); it++){ 00131 delete (*it); 00132 } 00133 _vertices.clear(); 00134 _edges.clear(); 00135 } 00136 00137 Graph::~Graph(){ 00138 clear(); 00139 } 00140 00141 };