octomap  1.9.7
ScanGraph.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * https://octomap.github.io/
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
octomap::ScanGraph::clear
void clear()
Clears all nodes and edges, and will delete the corresponding objects.
Definition: ScanGraph.cpp:167
octomap::ScanNode::pose
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:74
octomap::Pointcloud
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan.
Definition: Pointcloud.h:47
octomap::ScanEdge::constraint
pose6d constraint
Definition: ScanGraph.h:104
octomap::ScanNode::ScanNode
ScanNode(Pointcloud *_scan, pose6d _pose, unsigned int _id)
Definition: ScanGraph.h:56
octomap::ScanNode::readPoseASCII
std::istream & readPoseASCII(std::istream &s)
Definition: ScanGraph.cpp:90
octomap::ScanGraph::edges_begin
edge_iterator edges_begin()
Definition: ScanGraph.h:185
octomap::ScanGraph::readEdgesASCII
std::istream & readEdgesASCII(std::istream &s)
Definition: ScanGraph.cpp:510
octomap::ScanNode::writePoseASCII
std::ostream & writePoseASCII(std::ostream &s) const
Definition: ScanGraph.cpp:80
octomap::ScanNode::writeBinary
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:52
octomap::ScanGraph::edges_begin
const_edge_iterator edges_begin() const
Definition: ScanGraph.h:187
octomap::ScanGraph::getOutEdges
std::vector< ScanEdge * > getOutEdges(ScanNode *node)
Definition: ScanGraph.cpp:287
octomap::ScanGraph::edgeExists
bool edgeExists(unsigned int first_id, unsigned int second_id)
Definition: ScanGraph.cpp:259
octomap::ScanGraph::writeEdgesASCII
std::ostream & writeEdgesASCII(std::ostream &s) const
Definition: ScanGraph.cpp:491
octomap::ScanGraph::const_iterator
std::vector< ScanNode * >::const_iterator const_iterator
Definition: ScanGraph.h:174
octomap::ScanEdge::second
ScanNode * second
Definition: ScanGraph.h:102
octomap::ScanGraph::readPlainASCII
std::istream & readPlainASCII(std::istream &s)
Reads in a ScanGraph from a "plain" ASCII file of the form NODE x y z R P Y x y z x y z x y z NODE x ...
Definition: ScanGraph.cpp:436
octomath::Vector3
This class represents a three-dimensional vector.
Definition: Vector3.h:50
octomap::ScanGraph::nodes
std::vector< ScanNode * > nodes
Definition: ScanGraph.h:224
octomap::ScanGraph
A ScanGraph is a collection of ScanNodes, connected by ScanEdges.
Definition: ScanGraph.h:114
octomap::ScanGraph::~ScanGraph
~ScanGraph()
Definition: ScanGraph.cpp:163
octomap::ScanGraph::end
iterator end()
Definition: ScanGraph.h:176
octomap::ScanNode::~ScanNode
~ScanNode()
Definition: ScanGraph.cpp:45
octomap::ScanGraph::ScanGraph
ScanGraph()
Definition: ScanGraph.h:118
octomap::ScanGraph::getNumPoints
size_t getNumPoints(unsigned int max_id=-1) const
Definition: ScanGraph.cpp:611
octomap::ScanGraph::writeNodePosesASCII
std::ostream & writeNodePosesASCII(std::ostream &s) const
Definition: ScanGraph.cpp:543
octomap::ScanEdge::first
ScanNode * first
Definition: ScanGraph.h:101
octomap::ScanGraph::end
const_iterator end() const
Definition: ScanGraph.h:178
octomap::ScanNode
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:52
octomap::ScanGraph::exportDot
void exportDot(std::string filename)
Definition: ScanGraph.cpp:236
octomap::ScanNode::ScanNode
ScanNode()
Definition: ScanGraph.h:58
octomap::ScanGraph::begin
iterator begin()
Definition: ScanGraph.h:175
octomap::ScanGraph::connectPrevious
void connectPrevious()
Connect previously added ScanNode to the one before that.
Definition: ScanGraph.cpp:226
octomap::ScanEdge::operator==
bool operator==(const ScanEdge &other)
Definition: ScanGraph.h:90
octomap::ScanGraph::size
size_t size() const
Definition: ScanGraph.h:180
octomap::ScanGraph::begin
const_iterator begin() const
Definition: ScanGraph.h:177
octomap::ScanGraph::getNodeByID
ScanNode * getNodeByID(unsigned int id)
will return NULL if node was not found
Definition: ScanGraph.cpp:252
octomap::ScanNode::readBinary
std::istream & readBinary(std::istream &s)
Definition: ScanGraph.cpp:65
octomap::ScanEdge::ScanEdge
ScanEdge(ScanNode *_first, ScanNode *_second, pose6d _constraint)
Definition: ScanGraph.h:86
octomap::ScanNode::id
unsigned int id
Definition: ScanGraph.h:75
octomap::ScanNode::operator==
bool operator==(const ScanNode &other)
Definition: ScanGraph.h:63
octomap::ScanEdge::readBinary
std::istream & readBinary(std::istream &s, ScanGraph &graph)
Definition: ScanGraph.cpp:117
octomap::ScanGraph::edges_end
edge_iterator edges_end()
Definition: ScanGraph.h:186
octomap_types.h
octomap::ScanEdge::readASCII
std::istream & readASCII(std::istream &s, ScanGraph &graph)
Definition: ScanGraph.cpp:146
octomap::ScanGraph::getInEdges
std::vector< ScanEdge * > getInEdges(ScanNode *node)
Definition: ScanGraph.cpp:299
octomap::ScanEdge
A connection between two ScanNodes.
Definition: ScanGraph.h:82
octomap::ScanGraph::edges_end
const_edge_iterator edges_end() const
Definition: ScanGraph.h:188
Pointcloud.h
octomap::ScanGraph::const_edge_iterator
std::vector< ScanEdge * >::const_iterator const_edge_iterator
Definition: ScanGraph.h:184
octomap::ScanGraph::addNode
ScanNode * addNode(Pointcloud *scan, pose6d pose)
Creates a new ScanNode in the graph from a Pointcloud.
Definition: ScanGraph.cpp:179
octomap::ScanGraph::addEdge
ScanEdge * addEdge(ScanNode *first, ScanNode *second, pose6d constraint)
Creates an edge between two ScanNodes.
Definition: ScanGraph.cpp:191
octomap::ScanEdge::writeBinary
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:106
octomap::ScanGraph::writeBinary
std::ostream & writeBinary(std::ostream &s) const
Definition: ScanGraph.cpp:328
octomap::ScanGraph::transformScans
void transformScans()
Transform every scan according to its pose.
Definition: ScanGraph.cpp:311
octomap::ScanGraph::readNodePosesASCII
std::istream & readNodePosesASCII(std::istream &s)
Definition: ScanGraph.cpp:556
octomap::ScanGraph::edges
std::vector< ScanEdge * > edges
Definition: ScanGraph.h:225
octomap::ScanEdge::writeASCII
std::ostream & writeASCII(std::ostream &s) const
Definition: ScanGraph.cpp:134
octomap::ScanEdge::weight
double weight
Definition: ScanGraph.h:105
octomap
Tables used by the Marching Cubes Algorithm The tables are from Paul Bourke's web page http://paulbou...
octomath::Pose6D
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
octomap::ScanNode::scan
Pointcloud * scan
Definition: ScanGraph.h:73
octomap::ScanGraph::cropEachScan
void cropEachScan(point3d lowerBound, point3d upperBound)
Cut Pointclouds to given BBX in local coords.
Definition: ScanGraph.cpp:588
octomap::ScanGraph::iterator
std::vector< ScanNode * >::iterator iterator
Definition: ScanGraph.h:173
octomap::ScanGraph::getNeighborIDs
std::vector< unsigned int > getNeighborIDs(unsigned int id)
Definition: ScanGraph.cpp:272
octomap::ScanGraph::crop
void crop(point3d lowerBound, point3d upperBound)
Cut graph (all containing Pointclouds) to given BBX in global coords.
Definition: ScanGraph.cpp:596
octomap::ScanEdge::ScanEdge
ScanEdge()
Definition: ScanGraph.h:88
octomap::ScanGraph::readBinary
std::istream & readBinary(std::ifstream &s)
Definition: ScanGraph.cpp:369
octomap::ScanGraph::edge_iterator
std::vector< ScanEdge * >::iterator edge_iterator
Definition: ScanGraph.h:183