ScanGraph.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef OCTOMAP_SCANGRAPH_H
35 #define OCTOMAP_SCANGRAPH_H
36 
37 
38 #include <string>
39 #include <math.h>
40 
41 #include "Pointcloud.h"
42 #include "octomap_types.h"
43 
44 namespace octomap {
45 
46  class ScanGraph;
47 
48 
52  class ScanNode {
53 
54  public:
55 
56  ScanNode (Pointcloud* _scan, pose6d _pose, unsigned int _id)
57  : scan(_scan), pose(_pose), id(_id) {}
59  : scan(NULL) {}
60 
61  ~ScanNode();
62 
63  bool operator == (const ScanNode& other) {
64  return (id == other.id);
65  }
66 
67  std::ostream& writeBinary(std::ostream &s) const;
68  std::istream& readBinary(std::istream &s);
69 
70  std::ostream& writePoseASCII(std::ostream &s) const;
71  std::istream& readPoseASCII(std::istream &s);
72 
75  unsigned int id;
76 
77  };
78 
82  class ScanEdge {
83 
84  public:
85 
86  ScanEdge(ScanNode* _first, ScanNode* _second, pose6d _constraint)
87  : first(_first), second(_second), constraint(_constraint), weight(1.0) { }
88  ScanEdge() {}
89 
90  bool operator == (const ScanEdge& other) {
91  return ( (*first == *(other.first) ) && ( *second == *(other.second) ) );
92  }
93 
94  std::ostream& writeBinary(std::ostream &s) const;
95  // a graph has to be given to recover ScanNode pointers
96  std::istream& readBinary(std::istream &s, ScanGraph& graph);
97 
98  std::ostream& writeASCII(std::ostream &s) const;
99  std::istream& readASCII(std::istream &s, ScanGraph& graph);
100 
103 
105  double weight;
106  };
107 
108 
114  class ScanGraph {
115 
116  public:
117 
118  ScanGraph() {};
119  ~ScanGraph();
120 
122  void clear();
123 
132  ScanNode* addNode(Pointcloud* scan, pose6d pose);
133 
143  ScanEdge* addEdge(ScanNode* first, ScanNode* second, pose6d constraint);
144 
145  ScanEdge* addEdge(unsigned int first_id, unsigned int second_id);
146 
148  ScanNode* getNodeByID(unsigned int id);
149 
151  bool edgeExists(unsigned int first_id, unsigned int second_id);
152 
154  void connectPrevious();
155 
156  std::vector<unsigned int> getNeighborIDs(unsigned int id);
157  std::vector<ScanEdge*> getOutEdges(ScanNode* node);
158  // warning: constraints are reversed
159  std::vector<ScanEdge*> getInEdges(ScanNode* node);
160 
161  void exportDot(std::string filename);
162 
164  void transformScans();
165 
167  void crop(point3d lowerBound, point3d upperBound);
168 
170  void cropEachScan(point3d lowerBound, point3d upperBound);
171 
172 
173  typedef std::vector<ScanNode*>::iterator iterator;
174  typedef std::vector<ScanNode*>::const_iterator const_iterator;
175  iterator begin() { return nodes.begin(); }
176  iterator end() { return nodes.end(); }
177  const_iterator begin() const { return nodes.begin(); }
178  const_iterator end() const { return nodes.end(); }
179 
180  size_t size() const { return nodes.size(); }
181  size_t getNumPoints(unsigned int max_id = -1) const;
182 
183  typedef std::vector<ScanEdge*>::iterator edge_iterator;
184  typedef std::vector<ScanEdge*>::const_iterator const_edge_iterator;
185  edge_iterator edges_begin() { return edges.begin(); }
186  edge_iterator edges_end() { return edges.end(); }
187  const_edge_iterator edges_begin() const { return edges.begin(); }
188  const_edge_iterator edges_end() const { return edges.end(); }
189 
190 
191  std::ostream& writeBinary(std::ostream &s) const;
192  std::istream& readBinary(std::ifstream &s);
193  bool writeBinary(const std::string& filename) const;
194  bool readBinary(const std::string& filename);
195 
196 
197  std::ostream& writeEdgesASCII(std::ostream &s) const;
198  std::istream& readEdgesASCII(std::istream &s);
199 
200  std::ostream& writeNodePosesASCII(std::ostream &s) const;
201  std::istream& readNodePosesASCII(std::istream &s);
202 
219  std::istream& readPlainASCII(std::istream& s);
220  void readPlainASCII(const std::string& filename);
221 
222  protected:
223 
224  std::vector<ScanNode*> nodes;
225  std::vector<ScanEdge*> edges;
226  };
227 
228 }
229 
230 
231 #endif
std::istream & readPoseASCII(std::istream &s)
Definition: ScanGraph.cpp:90
iterator begin()
Definition: ScanGraph.h:175
size_t size() const
Definition: ScanGraph.h:180
ScanNode * first
Definition: ScanGraph.h:101
Pointcloud * scan
Definition: ScanGraph.h:73
edge_iterator edges_end()
Definition: ScanGraph.h:186
std::vector< ScanNode * >::const_iterator const_iterator
Definition: ScanGraph.h:174
ScanNode(Pointcloud *_scan, pose6d _pose, unsigned int _id)
Definition: ScanGraph.h:56
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
ScanNode * second
Definition: ScanGraph.h:102
std::istream & readBinary(std::istream &s)
Definition: ScanGraph.cpp:65
ScanEdge(ScanNode *_first, ScanNode *_second, pose6d _constraint)
Definition: ScanGraph.h:86
unsigned int id
Definition: ScanGraph.h:75
std::vector< ScanNode * >::iterator iterator
Definition: ScanGraph.h:173
iterator end()
Definition: ScanGraph.h:176
const_iterator begin() const
Definition: ScanGraph.h:177
const_iterator end() const
Definition: ScanGraph.h:178
bool operator==(const ScanNode &other)
Definition: ScanGraph.h:63
std::vector< ScanEdge * > edges
Definition: ScanGraph.h:225
std::vector< ScanEdge * >::iterator edge_iterator
Definition: ScanGraph.h:183
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
const_edge_iterator edges_begin() const
Definition: ScanGraph.h:187
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:52
This class represents a three-dimensional vector.
Definition: Vector3.h:50
const_edge_iterator edges_end() const
Definition: ScanGraph.h:188
std::vector< ScanEdge * >::const_iterator const_edge_iterator
Definition: ScanGraph.h:184
std::vector< ScanNode * > nodes
Definition: ScanGraph.h:224
std::ostream & writePoseASCII(std::ostream &s) const
Definition: ScanGraph.cpp:80
edge_iterator edges_begin()
Definition: ScanGraph.h:185


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06