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