hyper_graph.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include "hyper_graph.h"
28 
29 #include <assert.h>
30 #include <queue>
31 
32 namespace g2o {
33 
34  HyperGraph::Vertex::Vertex(int id) : _id(id)
35  {
36  }
37 
39  {
40  }
41 
43  {
44  }
45 
47  {
48  }
49 
50  void HyperGraph::Edge::resize(size_t size)
51  {
52  _vertices.resize(size, 0);
53  }
54 
56  {
57  _id = id;
58  }
59 
61  {
62  VertexIDMap::iterator it=_vertices.find(id);
63  if (it==_vertices.end())
64  return 0;
65  return it->second;
66  }
67 
69  {
70  VertexIDMap::const_iterator it=_vertices.find(id);
71  if (it==_vertices.end())
72  return 0;
73  return it->second;
74  }
75 
77  {
78  Vertex* vn=vertex(v->id());
79  if (vn)
80  return false;
81  _vertices.insert( std::make_pair(v->id(),v) );
82  return true;
83  }
84 
89  bool HyperGraph::changeId(Vertex* v, int newId){
90  Vertex* v2 = vertex(v->id());
91  if (v != v2)
92  return false;
93  _vertices.erase(v->id());
94  v->setId(newId);
95  _vertices.insert(std::make_pair(v->id(), v));
96  return true;
97  }
98 
100  {
101  std::pair<EdgeSet::iterator, bool> result = _edges.insert(e);
102  if (! result.second)
103  return false;
104  for (std::vector<Vertex*>::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
105  Vertex* v = *it;
106  v->edges().insert(e);
107  }
108  return true;
109  }
110 
112  {
113  VertexIDMap::iterator it=_vertices.find(v->id());
114  if (it==_vertices.end())
115  return false;
116  assert(it->second==v);
117  //remove all edges which are entering or leaving v;
118  EdgeSet tmp(v->edges());
119  for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){
120  if (!removeEdge(*it)){
121  assert(0);
122  }
123  }
124  _vertices.erase(it);
125  delete v;
126  return true;
127  }
128 
130  {
131  EdgeSet::iterator it = _edges.find(e);
132  if (it == _edges.end())
133  return false;
134  _edges.erase(it);
135 
136  for (std::vector<Vertex*>::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
137  Vertex* v = *vit;
138  it = v->edges().find(e);
139  assert(it!=v->edges().end());
140  v->edges().erase(it);
141  }
142 
143  delete e;
144  return true;
145  }
146 
148  {
149  }
150 
152  {
153  for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it)
154  delete (it->second);
155  for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it)
156  delete (*it);
157  _vertices.clear();
158  _edges.clear();
159  }
160 
162  {
163  clear();
164  }
165 
166 } // end namespace
int id() const
returns the id
Definition: hyper_graph.h:103
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:140
virtual bool changeId(Vertex *v, int newId)
Definition: hyper_graph.cpp:89
Vertex * vertex(int id)
returns a vertex id in the hyper-graph, or 0 if the vertex id is not present
Definition: hyper_graph.cpp:60
Vertex(int id=-1)
creates a vertex having an ID specified by the argument
Definition: hyper_graph.cpp:34
virtual bool addEdge(Edge *e)
Definition: hyper_graph.cpp:99
virtual void setId(int newId)
Definition: hyper_graph.h:104
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:90
virtual bool removeVertex(Vertex *v)
removes a vertex from the graph. Returns true on success (vertex was present)
const VertexContainer & vertices() const
Definition: hyper_graph.h:132
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
Definition: hyper_graph.h:106
Edge(int id=-1)
creates and empty edge with no vertices
Definition: hyper_graph.cpp:42
virtual void clear()
clears the graph and empties all structures.
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
virtual bool addVertex(Vertex *v)
Definition: hyper_graph.cpp:76
virtual void resize(size_t size)
Definition: hyper_graph.cpp:50
HyperGraph()
constructs an empty hyper graph
virtual bool removeEdge(Edge *e)
removes a vertex from the graph. Returns true on success (edge was present)
int _id
unique id
Definition: hyper_graph.h:155
virtual ~HyperGraph()
destroys the hyper-graph and all the vertices of the graph
VertexContainer _vertices
Definition: hyper_graph.h:154


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47