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