Main MRPT website > C++ reference for MRPT 1.4.0
ScanGraph.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 // $Id: ScanGraph.h 245 2011-08-09 10:00:53Z kai_wurm $
10 
11 /**
12 * OctoMap:
13 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
14 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
15 * @see http://octomap.sourceforge.net/
16 * License: New BSD License
17 */
18 
19 /*
20  * Copyright (c) 2009, K. M. Wurm, A. Hornung, University of Freiburg
21  * All rights reserved.
22  *
23  * Redistribution and use in source and binary forms, with or without
24  * modification, are permitted provided that the following conditions are met:
25  *
26  * * Redistributions of source code must retain the above copyright
27  * notice, this list of conditions and the following disclaimer.
28  * * Redistributions in binary form must reproduce the above copyright
29  * notice, this list of conditions and the following disclaimer in the
30  * documentation and/or other materials provided with the distribution.
31  * * Neither the name of the University of Freiburg nor the names of its
32  * contributors may be used to endorse or promote products derived from
33  * this software without specific prior written permission.
34  *
35  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
36  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
37  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
38  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
39  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
40  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
41  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
42  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
43  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
44  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45  * POSSIBILITY OF SUCH DAMAGE.
46  */
47 
48 #ifndef OCTOMAP_SCANGRAPH_H
49 #define OCTOMAP_SCANGRAPH_H
50 
51 
52 #include <string>
53 #include <math.h>
54 
55 #include "Pointcloud.h"
56 #include "octomap_types.h"
57 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
58 
59 namespace octomap {
60 
61  class ScanGraph;
62 
63 
64  /**
65  * A 3D scan as Pointcloud, performed from a Pose6D.
66  */
67  class /*MAPS_IMPEXP*/ ScanNode {
68 
69  public:
70 
71  ScanNode (Pointcloud* _scan, pose6d _pose, uint64_t _id)
72  : scan(_scan), pose(_pose), id(_id) {}
74  : scan(NULL) {}
75 
76  ~ScanNode();
77 
78  bool operator == (const ScanNode& other) {
79  return (id == other.id);
80  }
81 
82  std::ostream& writeBinary(std::ostream &s) const;
83  std::istream& readBinary(std::istream &s);
84 
85  std::ostream& writePoseASCII(std::ostream &s) const;
86  std::istream& readPoseASCII(std::istream &s);
87 
89  pose6d pose; ///< 6D pose from which the scan was performed
90  uint64_t id; //!< JLBC: Changed from "unsigned int" so binarized versions are platform-independent.
91 
92  };
93 
94  /**
95  * A connection between two \ref ScanNode "ScanNodes"
96  */
97  class /*MAPS_IMPEXP*/ ScanEdge {
98 
99  public:
100 
101  ScanEdge(ScanNode* _first, ScanNode* _second, pose6d _constraint)
102  : first(_first), second(_second), constraint(_constraint), weight(1.0) { }
103  ScanEdge() {}
104 
105  bool operator == (const ScanEdge& other) {
106  return ( (*first == *(other.first) ) && ( *second == *(other.second) ) );
107  }
108 
109  std::ostream& writeBinary(std::ostream &s) const;
110  // a graph has to be given to recover ScanNode pointers
111  std::istream& readBinary(std::istream &s, ScanGraph& graph);
112 
113  std::ostream& writeASCII(std::ostream &s) const;
114  std::istream& readASCII(std::istream &s, ScanGraph& graph);
115 
118 
120  double weight;
121  };
122 
123 
124  /**
125  * A ScanGraph is a collection of ScanNodes, connected by ScanEdges.
126  * Each ScanNode contains a 3D scan performed from a pose.
127  *
128  */
129  class /*MAPS_IMPEXP*/ ScanGraph {
130 
131  public:
132 
133  ScanGraph() {};
134  ~ScanGraph();
135 
136  /// Clears all nodes and edges, and will delete the corresponding objects
137  void clear();
138 
139  /**
140  * Creates a new ScanNode in the graph from a Pointcloud.
141  *
142  * @param scan Pointer to a pointcloud to be added to the ScanGraph.
143  * ScanGraph will delete the object when it's no longer needed, don't delete it yourself.
144  * @param pose 6D pose of the origin of the Pointcloud
145  * @return Pointer to the new node
146  */
147  ScanNode* addNode(Pointcloud* scan, pose6d pose);
148 
149  /**
150  * Creates an edge between two ScanNodes.
151  * ScanGraph will delete the object when it's no longer needed, don't delete it yourself.
152  *
153  * @param first ScanNode
154  * @param second ScanNode
155  * @param constraint 6D transform between the two nodes
156  * @return
157  */
158  ScanEdge* addEdge(ScanNode* first, ScanNode* second, pose6d constraint);
159 
160  ScanEdge* addEdge(uint64_t first_id, uint64_t second_id);
161 
162  /// will return NULL if node was not found
163  ScanNode* getNodeByID(uint64_t id);
164 
165  /// \return true when an edge between first_id and second_id exists
166  bool edgeExists(uint64_t first_id, uint64_t second_id);
167 
168  /// Connect previously added ScanNode to the one before that
169  void connectPrevious();
170 
171  std::vector<uint64_t> getNeighborIDs(uint64_t id);
172  std::vector<ScanEdge*> getOutEdges(ScanNode* node);
173  // warning: constraints are reversed
174  std::vector<ScanEdge*> getInEdges(ScanNode* node);
175 
176  void exportDot(std::string filename);
177 
178  /// Transform every scan according to its pose
179  void transformScans();
180 
181  /// Cut graph (all containing Pointclouds) to given BBX in global coords
182  void crop(point3d lowerBound, point3d upperBound);
183 
184  /// Cut Pointclouds to given BBX in local coords
185  void cropEachScan(point3d lowerBound, point3d upperBound);
186 
187 
190  iterator begin() { return nodes.begin(); }
191  iterator end() { return nodes.end(); }
192  const_iterator begin() const { return nodes.begin(); }
193  const_iterator end() const { return nodes.end(); }
194 
195  size_t size() const { return nodes.size(); }
196  size_t getNumPoints(uint64_t max_id = -1) const;
197 
200  edge_iterator edges_begin() { return edges.begin(); }
201  edge_iterator edges_end() { return edges.end(); }
202  const_edge_iterator edges_begin() const { return edges.begin(); }
203  const_edge_iterator edges_end() const { return edges.end(); }
204 
205 
206  std::ostream& writeBinary(std::ostream &s) const;
207  std::istream& readBinary(std::ifstream &s);
208  bool writeBinary(const std::string& filename) const;
209  bool readBinary(const std::string& filename);
210 
211 
212  std::ostream& writeEdgesASCII(std::ostream &s) const;
213  std::istream& readEdgesASCII(std::istream &s);
214 
215  std::ostream& writeNodePosesASCII(std::ostream &s) const;
216  std::istream& readNodePosesASCII(std::istream &s);
217 
218  /**
219  * Reads in a ScanGraph from a "plain" ASCII file of the form
220  * NODE x y z R P Y
221  * x y z
222  * x y z
223  * x y z
224  * NODE x y z R P Y
225  * x y z
226  *
227  * Lines starting with the NODE keyword contain the 6D pose of a scan node,
228  * all 3D point following until the next NODE keyword (or end of file) are
229  * inserted into that scan node as pointcloud in its local coordinate frame
230  *
231  * @param input stream to read from
232  * @return read stream
233  */
234  std::istream& readPlainASCII(std::istream& s);
235  void readPlainASCII(const std::string& filename);
236 
237  protected:
238 
239  std::vector<ScanNode*> nodes;
240  std::vector<ScanEdge*> edges;
241  };
242 
243 }
244 
245 
246 #endif
edge_iterator edges_begin()
Definition: ScanGraph.h:200
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:64
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
iterator begin()
Definition: ScanGraph.h:190
std::ostream & writeBinary(std::ostream &s) const
Pointcloud * scan
Definition: ScanGraph.h:88
Scalar * iterator
Definition: eigen_plugins.h:23
std::vector< ScanNode * >::const_iterator const_iterator
Definition: ScanGraph.h:189
uint64_t id
JLBC: Changed from "unsigned int" so binarized versions are platform-independent. ...
Definition: ScanGraph.h:90
const Scalar * const_iterator
Definition: eigen_plugins.h:24
std::vector< ScanNode * >::iterator iterator
Definition: ScanGraph.h:188
const_iterator end() const
Definition: ScanGraph.h:193
edge_iterator edges_end()
Definition: ScanGraph.h:201
std::istream & readPoseASCII(std::istream &s)
std::istream & readBinary(std::istream &s)
A ScanGraph is a collection of ScanNodes, connected by ScanEdges.
Definition: ScanGraph.h:129
ScanNode(Pointcloud *_scan, pose6d _pose, uint64_t _id)
Definition: ScanGraph.h:71
std::ostream & writePoseASCII(std::ostream &s) const
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
std::vector< ScanEdge * >::const_iterator const_edge_iterator
Definition: ScanGraph.h:199
const_edge_iterator edges_begin() const
Definition: ScanGraph.h:202
ScanNode * second
Definition: ScanGraph.h:117
ScanEdge(ScanNode *_first, ScanNode *_second, pose6d _constraint)
Definition: ScanGraph.h:101
const_iterator begin() const
Definition: ScanGraph.h:192
iterator end()
Definition: ScanGraph.h:191
size_t size() const
Definition: ScanGraph.h:195
std::vector< ScanNode * > nodes
Definition: ScanGraph.h:239
A 3D scan as Pointcloud, performed from a Pose6D.
Definition: ScanGraph.h:67
bool operator==(const ScanNode &other)
Definition: ScanGraph.h:78
std::vector< ScanEdge * > edges
Definition: ScanGraph.h:240
const_edge_iterator edges_end() const
Definition: ScanGraph.h:203
ScanNode * first
Definition: ScanGraph.h:116
pose6d pose
6D pose from which the scan was performed
Definition: ScanGraph.h:89
A connection between two ScanNodes.
Definition: ScanGraph.h:97
std::vector< ScanEdge * >::iterator edge_iterator
Definition: ScanGraph.h:198
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon Jul 4 10:31:07 UTC 2016