graph.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 <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 };


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