Main MRPT website > C++ reference for MRPT 1.4.0
Pointcloud.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: Pointcloud.h 205 2011-06-10 11:58:40Z ahornung $
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_POINTCLOUD_H
49 #define OCTOMAP_POINTCLOUD_H
50 
51 #include <vector>
52 #include <list>
54 
55 #include <mrpt/maps/link_pragmas.h> // For DLL export within mrpt-maps via the MAPS_IMPEXP macro
56 
57 namespace octomap {
58 
59  /**
60  * A collection of 3D coordinates (point3d), which are regarded as endpoints of a
61  * 3D laser scan.
62  */
63  class Pointcloud {
64 
65  public:
66 
67  Pointcloud();
68  ~Pointcloud();
69 
70  Pointcloud(const Pointcloud& other);
71  Pointcloud(Pointcloud* other);
72 
73  size_t size() const { return points.size(); }
74  void clear();
75  inline void reserve(size_t size) {points.reserve(size); }
76 
77  inline void push_back(float x, float y, float z) {
78  points.push_back(point3d(x,y,z));
79  }
80  inline void push_back(const point3d& p) {
81  points.push_back(p);
82  }
83  inline void push_back(point3d* p) {
84  points.push_back(*p);
85  }
86 
87  /// Add points from other Pointcloud
88  void push_back(const Pointcloud& other);
89 
90  /// Export the Pointcloud to a VRML file
91  void writeVrml(std::string filename);
92 
93  /// Apply transform to each point
95 
96  /// Rotate each point in pointcloud
97  void rotate(double roll, double pitch, double yaw);
98 
99  /// Apply transform to each point, undo previous transforms
100  void transformAbsolute(pose6d transform);
101 
102  /// Calculate bounding box of Pointcloud
103  void calcBBX(point3d& lowerBound, point3d& upperBound) const;
104  /// Crop Pointcloud to given bounding box
105  void crop(point3d lowerBound, point3d upperBound);
106 
107  // removes any points closer than [thres] to (0,0,0)
108  void minDist(double thres);
109 
110  void subSampleRandom(unsigned int num_samples, Pointcloud& sample_cloud);
111 
112  // iterators ------------------
113 
116  iterator begin() { return points.begin(); }
117  iterator end() { return points.end(); }
118  const_iterator begin() const { return points.begin(); }
119  const_iterator end() const { return points.end(); }
120  point3d back() { return points.back(); }
121  point3d getPoint(unsigned int i); // may return NULL
122 
123  // I/O methods
124 
125  std::istream& readBinary(std::istream &s);
126  std::istream& read(std::istream &s);
127  std::ostream& writeBinary(std::ostream &s) const;
128 
129  protected:
132  };
133 
134 }
135 
136 
137 #endif
point3d getPoint(unsigned int i)
point3d_collection::iterator iterator
Definition: Pointcloud.h:114
const_iterator begin() const
Definition: Pointcloud.h:118
void subSampleRandom(unsigned int num_samples, Pointcloud &sample_cloud)
void minDist(double thres)
point3d_collection points
Definition: Pointcloud.h:131
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.
void push_back(const point3d &p)
Definition: Pointcloud.h:80
std::ostream & writeBinary(std::ostream &s) const
Scalar * iterator
Definition: eigen_plugins.h:23
void push_back(point3d *p)
Definition: Pointcloud.h:83
void writeVrml(std::string filename)
Export the Pointcloud to a VRML file.
iterator begin()
Definition: Pointcloud.h:116
const Scalar * const_iterator
Definition: eigen_plugins.h:24
std::istream & read(std::istream &s)
void transformAbsolute(pose6d transform)
Apply transform to each point, undo previous transforms.
void push_back(float x, float y, float z)
Definition: Pointcloud.h:77
size_t size() const
Definition: Pointcloud.h:73
std::vector< octomath::Vector3 > point3d_collection
Definition: octomap_types.h:67
void transform(pose6d transform)
Apply transform to each point.
pose6d current_inv_transform
Definition: Pointcloud.h:130
A collection of 3D coordinates (point3d), which are regarded as endpoints of a 3D laser scan...
Definition: Pointcloud.h:63
void crop(point3d lowerBound, point3d upperBound)
Crop Pointcloud to given bounding box.
void calcBBX(point3d &lowerBound, point3d &upperBound) const
Calculate bounding box of Pointcloud.
const_iterator end() const
Definition: Pointcloud.h:119
void reserve(size_t size)
Definition: Pointcloud.h:75
point3d_collection::const_iterator const_iterator
Definition: Pointcloud.h:115
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:63
void rotate(double roll, double pitch, double yaw)
Rotate each point in pointcloud.
std::istream & readBinary(std::istream &s)
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 Sun Jul 10 11:38:36 UTC 2016