Pointcloud.cpp
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 /* According to c++ standard including this header has no practical effect
35  * but it can be used to determine the c++ standard library implementation.
36  */
37 #include <ciso646>
38 
39 #if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
40  #include <algorithm>
41 #else
42  #include <ext/algorithm>
43 #endif
44 #include <fstream>
45 #include <math.h>
46 
47 #include <octomap/Pointcloud.h>
48 
49 namespace octomap {
50 
51 
53 
54  }
55 
57  this->clear();
58  }
59 
61 
62  // delete the points
63  if (points.size()) {
64  points.clear();
65  }
66  }
67 
68 
70  for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) {
71  points.push_back(point3d(*it));
72  }
73  }
74 
76  for (Pointcloud::const_iterator it = other->begin(); it != other->end(); it++) {
77  points.push_back(point3d(*it));
78  }
79  }
80 
81 
82  void Pointcloud::push_back(const Pointcloud& other) {
83  for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) {
84  points.push_back(point3d(*it));
85  }
86  }
87 
88  point3d Pointcloud::getPoint(unsigned int i) const{
89  if (i < points.size())
90  return points[i];
91  else {
92  OCTOMAP_WARNING("Pointcloud::getPoint index out of range!\n");
93  return points.back();
94  }
95  }
96 
98 
99  for (unsigned int i=0; i<points.size(); i++) {
100  points[i] = transform.transform(points[i]);
101  }
102 
103  // FIXME: not correct for multiple transforms
104  current_inv_transform = transform.inv();
105  }
106 
107 
109 
110  // undo previous transform, then apply current transform
112 
113  for (unsigned int i=0; i<points.size(); i++) {
114  points[i] = transf.transform(points[i]);
115  }
116 
117  current_inv_transform = transform.inv();
118  }
119 
120 
121  void Pointcloud::rotate(double roll, double pitch, double yaw) {
122 
123  for (unsigned int i=0; i<points.size(); i++) {
124  points[i].rotate_IP(roll, pitch, yaw);
125  }
126  }
127 
128 
129  void Pointcloud::calcBBX(point3d& lowerBound, point3d& upperBound) const {
130  float min_x, min_y, min_z;
131  float max_x, max_y, max_z;
132  min_x = min_y = min_z = 1e6;
133  max_x = max_y = max_z = -1e6;
134 
135  float x,y,z;
136 
137  for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
138 
139  x = (*it)(0);
140  y = (*it)(1);
141  z = (*it)(2);
142 
143  if (x < min_x) min_x = x;
144  if (y < min_y) min_y = y;
145  if (z < min_z) min_z = z;
146 
147  if (x > max_x) max_x = x;
148  if (y > max_y) max_y = y;
149  if (z > max_z) max_z = z;
150  }
151 
152  lowerBound(0) = min_x; lowerBound(1) = min_y; lowerBound(2) = min_z;
153  upperBound(0) = max_x; upperBound(1) = max_y; upperBound(2) = max_z;
154  }
155 
156 
157  void Pointcloud::crop(point3d lowerBound, point3d upperBound) {
158 
159  Pointcloud result;
160 
161  float min_x, min_y, min_z;
162  float max_x, max_y, max_z;
163  float x,y,z;
164 
165  min_x = lowerBound(0); min_y = lowerBound(1); min_z = lowerBound(2);
166  max_x = upperBound(0); max_y = upperBound(1); max_z = upperBound(2);
167 
168  for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
169  x = (*it)(0);
170  y = (*it)(1);
171  z = (*it)(2);
172 
173  if ( (x >= min_x) &&
174  (y >= min_y) &&
175  (z >= min_z) &&
176  (x <= max_x) &&
177  (y <= max_y) &&
178  (z <= max_z) ) {
179  result.push_back (x,y,z);
180  }
181  } // end for points
182 
183  this->clear();
184  this->push_back(result);
185 
186  }
187 
188 
189  void Pointcloud::minDist(double thres) {
190  Pointcloud result;
191 
192  float x,y,z;
193  for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
194  x = (*it)(0);
195  y = (*it)(1);
196  z = (*it)(2);
197  double dist = sqrt(x*x+y*y+z*z);
198  if ( dist > thres ) result.push_back (x,y,z);
199  } // end for points
200  this->clear();
201  this->push_back(result);
202  }
203 
204 
205  void Pointcloud::subSampleRandom(unsigned int num_samples, Pointcloud& sample_cloud) {
206  point3d_collection samples;
207  // visual studio does not support random_sample_n and neither does libc++
208  #if defined(_MSC_VER) || defined(_LIBCPP_VERSION)
209  samples.reserve(this->size());
210  samples.insert(samples.end(), this->begin(), this->end());
211  std::random_shuffle(samples.begin(), samples.end());
212  samples.resize(num_samples);
213  #else
214  random_sample_n(begin(), end(), std::back_insert_iterator<point3d_collection>(samples), num_samples);
215  for (unsigned int i=0; i<samples.size(); i++) {
216  sample_cloud.push_back(samples[i]);
217  }
218  #endif
219  }
220 
221 
222  void Pointcloud::writeVrml(std::string filename){
223 
224  std::ofstream outfile (filename.c_str());
225 
226  outfile << "#VRML V2.0 utf8" << std::endl;
227  outfile << "Transform {" << std::endl;
228  outfile << "translation 0 0 0" << std::endl;
229  outfile << "rotation 0 0 0 0" << std::endl;
230  outfile << " children [" << std::endl;
231  outfile << " Shape{" << std::endl;
232  outfile << " geometry PointSet {" << std::endl;
233  outfile << " coord Coordinate {" << std::endl;
234  outfile << " point [" << std::endl;
235 
236  OCTOMAP_DEBUG_STR("PointCloud::writeVrml writing "
237  << points.size() << " points to "
238  << filename.c_str() << ".");
239 
240  for (unsigned int i = 0; i < (points.size()); i++){
241  outfile << "\t\t" << (points[i])(0)
242  << " " << (points[i])(1)
243  << " " << (points[i])(2)
244  << "\n";
245  }
246 
247  outfile << " ]" << std::endl;
248  outfile << " }" << std::endl;
249  outfile << " color Color{" << std::endl;
250  outfile << " color [" << std::endl;
251 
252  for (unsigned int i = 0; i < points.size(); i++){
253  outfile << "\t\t 1.0 1.0 1.0 \n";
254  }
255 
256  outfile << " ]" << std::endl;
257  outfile << " }" << std::endl;
258 
259  outfile << " }" << std::endl;
260  outfile << " }" << std::endl;
261 
262 
263  outfile << " ]" << std::endl;
264  outfile << "}" << std::endl;
265 
266 
267  }
268 
269  std::istream& Pointcloud::read(std::istream &s){
270  while (!s.eof()){
271  point3d p;
272  for (unsigned int i=0; i<3; i++){
273  s >> p(i);
274  }
275  if (!s.fail()){
276  this->push_back(p);
277  } else {
278  break;
279  }
280  }
281 
282  return s;
283  }
284 
285  std::istream& Pointcloud::readBinary(std::istream &s) {
286 
287  unsigned int pc_size = 0;
288  s.read((char*)&pc_size, sizeof(pc_size));
289  OCTOMAP_DEBUG("Reading %d points from binary file...", pc_size);
290 
291  if (pc_size > 0) {
292  this->points.reserve(pc_size);
293  point3d p;
294  for (unsigned int i=0; i<pc_size; i++) {
295  p.readBinary(s);
296  if (!s.fail()) {
297  this->push_back(p);
298  }
299  else {
300  OCTOMAP_ERROR("Pointcloud::readBinary: ERROR.\n" );
301  break;
302  }
303  }
304  }
305  OCTOMAP_DEBUG("done.\n");
306 
307  return s;
308  }
309 
310 
311  std::ostream& Pointcloud::writeBinary(std::ostream &s) const {
312 
313  size_t pc_size = this->size();
314  OCTOMAP_DEBUG("Writing %lu points to binary file...", (unsigned long)pc_size);
315  s.write((char*)&pc_size, sizeof(pc_size));
316 
317  for (Pointcloud::const_iterator it = this->begin(); it != this->end(); it++) {
318  it->writeBinary(s);
319  }
320  OCTOMAP_DEBUG("done.\n");
321 
322  return s;
323  }
324 
325 } // end namespace
Vector3 transform(const Vector3 &v) const
Transformation of a vector.
Definition: Pose6D.cpp:80
#define OCTOMAP_DEBUG(...)
Definition: octomap_types.h:83
void minDist(double thres)
Definition: Pointcloud.cpp:189
void writeVrml(std::string filename)
Export the Pointcloud to a VRML file.
Definition: Pointcloud.cpp:222
pose6d current_inv_transform
Definition: Pointcloud.h:119
point3d getPoint(unsigned int i) const
Definition: Pointcloud.cpp:88
void transform(pose6d transform)
Apply transform to each point.
Definition: Pointcloud.cpp:97
void push_back(float x, float y, float z)
Definition: Pointcloud.h:61
Pose6D inv() const
Inversion.
Definition: Pose6D.cpp:65
void subSampleRandom(unsigned int num_samples, Pointcloud &sample_cloud)
Definition: Pointcloud.cpp:205
iterator begin()
Definition: Pointcloud.h:100
std::ostream & writeBinary(std::ostream &s) const
Definition: Pointcloud.cpp:311
std::vector< octomath::Vector3 > point3d_collection
Definition: octomap_types.h:52
std::istream & readBinary(std::istream &s)
Definition: Vector3.cpp:87
point3d_collection points
Definition: Pointcloud.h:120
std::istream & readBinary(std::istream &s)
Definition: Pointcloud.cpp:285
This class represents a tree-dimensional pose of an object.
Definition: Pose6D.h:49
#define OCTOMAP_WARNING(...)
Definition: octomap_types.h:87
std::istream & read(std::istream &s)
Definition: Pointcloud.cpp:269
This class represents a three-dimensional vector.
Definition: Vector3.h:50
#define OCTOMAP_DEBUG_STR(args)
Definition: octomap_types.h:84
size_t size() const
Definition: Pointcloud.h:57
void transformAbsolute(pose6d transform)
Apply transform to each point, undo previous transforms.
Definition: Pointcloud.cpp:108
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:48
void rotate(double roll, double pitch, double yaw)
Rotate each point in pointcloud.
Definition: Pointcloud.cpp:121
void calcBBX(point3d &lowerBound, point3d &upperBound) const
Calculate bounding box of Pointcloud.
Definition: Pointcloud.cpp:129
void crop(point3d lowerBound, point3d upperBound)
Crop Pointcloud to given bounding box.
Definition: Pointcloud.cpp:157
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:89
point3d_collection::const_iterator const_iterator
Definition: Pointcloud.h:99


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 10 2019 14:00:13