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 #ifndef G2O_AIS_HYPER_GRAPH_HH
28 #define G2O_AIS_HYPER_GRAPH_HH
29 
30 #include <set>
31 #include <bitset>
32 #include <cassert>
33 #include <vector>
34 #include <cstddef>
35 
36 #include <unordered_map>
37 
38 #include "g2o_core_api.h"
39 
40 /** @addtogroup graph */
41 //@{
42 namespace g2o {
43 
44   /**
45      Class that models a directed  Hyper-Graph. An hyper graph is a graph where an edge
46      can connect one or more nodes. Both Vertices and Edges of an hyper graph
47      derive from the same class HyperGraphElement, thus one can implement generic algorithms
48      that operate transparently on edges or vertices (see HyperGraphAction).
49 
50      The vertices are uniquely identified by an int id, while the edges are
51      identfied by their pointers.
52    */
53   class G2O_CORE_API HyperGraph
54   {
55     public:
56 
57       /**
58        * \brief enum of all the types we have in our graphs
59        */
60       enum G2O_CORE_API HyperGraphElementType {
61         HGET_VERTEX,
62         HGET_EDGE,
63         HGET_PARAMETER,
64         HGET_CACHE,
65         HGET_DATA,
66         HGET_NUM_ELEMS // keep as last elem
67       };
68 
69       static const int UnassignedId = -1;
70       static const int InvalidId = -2;
71 
72       typedef std::bitset<HyperGraph::HGET_NUM_ELEMS> GraphElemBitset;
73 
74       class G2O_CORE_API Data;
75       class G2O_CORE_API DataContainer;
76       class G2O_CORE_API Vertex;
77       class G2O_CORE_API Edge;
78 
79       /**
80        * base hyper graph element, specialized in vertex and edge
81        */
82       struct G2O_CORE_API HyperGraphElement {
~HyperGraphElementHyperGraphElement83         virtual ~HyperGraphElement() {}
84         /**
85          * returns the type of the graph element, see HyperGraphElementType
86          */
87         virtual HyperGraphElementType elementType() const = 0;
88       };
89 
90       /**
91        * \brief data packet for a vertex. Extend this class to store in the vertices
92        * the potential additional information you need (e.g. images, laser scans, ...).
93        */
94       class G2O_CORE_API Data : public HyperGraph::HyperGraphElement {
95         public:
96           Data();
97           ~Data();
98           //! read the data from a stream
99           virtual bool read(std::istream& is) = 0;
100           //! write the data to a stream
101           virtual bool write(std::ostream& os) const = 0;
elementType()102           virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}
next()103           inline const Data* next() const {return _next;}
next()104           inline Data* next() {return _next;}
setNext(Data * next_)105           inline void setNext(Data* next_) { _next = next_; }
dataContainer()106           inline DataContainer* dataContainer() { return _dataContainer;}
dataContainer()107           inline const DataContainer* dataContainer() const { return _dataContainer;}
setDataContainer(DataContainer * dataContainer_)108           inline void setDataContainer(DataContainer * dataContainer_){ _dataContainer = dataContainer_;}
109         protected:
110           Data* _next; // linked list of multiple data;
111           DataContainer* _dataContainer;
112       };
113 
114       /**
115        * \brief Container class that implements an interface for adding/removing Data elements in
116        a linked list
117        */
118       class G2O_CORE_API DataContainer {
119         public:
DataContainer()120           DataContainer() {_userData = 0;}
~DataContainer()121           virtual ~DataContainer() { delete _userData;}
122           //! the user data associated with this vertex
userData()123           const Data* userData() const { return _userData; }
userData()124           Data* userData() { return _userData; }
setUserData(Data * obs)125           void setUserData(Data* obs) { _userData = obs;}
addUserData(Data * obs)126           void addUserData(Data* obs) { if (obs) { obs->setNext(_userData); _userData=obs; } }
127         protected:
128           Data* _userData;
129       };
130 
131 
132       typedef std::set<Edge*>                           EdgeSet;
133       typedef std::set<Vertex*>                         VertexSet;
134 
135       typedef std::unordered_map<int, Vertex*>     VertexIDMap;
136       typedef std::vector<Vertex*>                      VertexContainer;
137 
138       //! abstract Vertex, your types must derive from that one
139       class G2O_CORE_API Vertex : public HyperGraphElement {
140         public:
141           //! creates a vertex having an ID specified by the argument
142           explicit Vertex(int id=InvalidId);
143           virtual ~Vertex();
144           //! returns the id
id()145           int id() const {return _id;}
setId(int newId)146           virtual void setId(int newId) { _id = newId; }
147           //! returns the set of hyper-edges that are leaving/entering in this vertex
edges()148           const EdgeSet& edges() const {return _edges;}
149           //! returns the set of hyper-edges that are leaving/entering in this vertex
edges()150           EdgeSet& edges() {return _edges;}
elementType()151           virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}
152         protected:
153           int _id;
154           EdgeSet _edges;
155       };
156 
157 
158       /**
159        * Abstract Edge class. Your nice edge classes should inherit from that one.
160        * An hyper-edge has pointers to the vertices it connects and stores them in a vector.
161        */
162       class G2O_CORE_API Edge : public HyperGraphElement {
163         public:
164           //! creates and empty edge with no vertices
165           explicit Edge(int id = InvalidId);
166           virtual ~Edge();
167 
168           /**
169            * resizes the number of vertices connected by this edge
170            */
171           virtual void resize(size_t size);
172           /**
173             returns the vector of pointers to the vertices connected by the hyper-edge.
174             */
vertices()175           const VertexContainer& vertices() const { return _vertices;}
176           /**
177             returns the vector of pointers to the vertices connected by the hyper-edge.
178             */
vertices()179           VertexContainer& vertices() { return _vertices;}
180           /**
181             returns the pointer to the ith vertex connected to the hyper-edge.
182             */
vertex(size_t i)183           const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
184           /**
185             returns the pointer to the ith vertex connected to the hyper-edge.
186             */
vertex(size_t i)187           Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
188           /**
189             set the ith vertex on the hyper-edge to the pointer supplied
190             */
setVertex(size_t i,Vertex * v)191           void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
192 
id()193           int id() const {return _id;}
194           void setId(int id);
elementType()195           virtual HyperGraphElementType elementType() const { return HGET_EDGE;}
196 
197           int numUndefinedVertices() const;
198 
199          protected:
200           VertexContainer _vertices;
201           int _id;  ///< unique id
202       };
203 
204     public:
205       //! constructs an empty hyper graph
206       HyperGraph();
207       //! destroys the hyper-graph and all the vertices of the graph
208       virtual ~HyperGraph();
209 
210       //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
211       Vertex* vertex(int id);
212       //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
213       const Vertex* vertex(int id) const;
214 
215       //! removes a vertex from the graph. Returns true on success (vertex was present)
216       virtual bool removeVertex(Vertex* v, bool detach=false);
217       //! removes a vertex from the graph. Returns true on success (edge was present)
218       virtual bool removeEdge(Edge* e);
219       //! clears the graph and empties all structures.
220       virtual void clear();
221 
222       //! @returns the map <i>id -> vertex</i> where the vertices are stored
vertices()223       const VertexIDMap& vertices() const {return _vertices;}
224       //! @returns the map <i>id -> vertex</i> where the vertices are stored
vertices()225       VertexIDMap& vertices() {return _vertices;}
226 
227       //! @returns the set of edges of the hyper graph
edges()228       const EdgeSet& edges() const {return _edges;}
229       //! @returns the set of edges of the hyper graph
edges()230       EdgeSet& edges() {return _edges;}
231 
232       /**
233        * adds a vertex to the graph. The id of the vertex should be set before
234        * invoking this function. the function fails if another vertex
235        * with the same id is already in the graph.
236        * returns true, on success, or false on failure.
237        */
238       virtual bool addVertex(Vertex* v);
239 
240       /**
241        * Adds an edge to the graph. If the edge is already in the graph, it
242        * does nothing and returns false. Otherwise it returns true.
243        */
244       virtual bool addEdge(Edge* e);
245 
246 
247       /**
248        * Sets the vertex in position "pos" within the edge and keeps the bookkeeping consistent.
249        * If v ==0, the vertex is set to "invalid"
250        */
251       virtual bool setEdgeVertex(Edge* e, int pos, Vertex* v);
252 
253       /**
254        * merges two (valid) vertices, adjusts the bookkeeping and relabels all edges.
255        * the observations of vSmall are retargeted to vBig. If erase = true, vSmall is deleted from the graph
256        * repeatedly calls setEdgeVertex(...)
257        */
258       virtual bool mergeVertices(Vertex* vBig, Vertex* vSmall, bool erase);
259 
260       /**
261        * detaches a vertex from all connected edges
262        */
263       virtual bool detachVertex(Vertex* v);
264 
265       /**
266        * changes the id of a vertex already in the graph, and updates the bookkeeping
267        @ returns false if the vertex is not in the graph;
268        */
269       virtual bool changeId(Vertex* v, int newId);
270 
271     protected:
272       VertexIDMap _vertices;
273       EdgeSet _edges;
274 
275     private:
276       // Disable the copy constructor and assignment operator
HyperGraph(const HyperGraph &)277       HyperGraph(const HyperGraph&) { }
278       HyperGraph& operator= (const HyperGraph&) { return *this; }
279   };
280 
281 } // end namespace
282 
283 //@}
284 
285 #endif
286