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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Fri Jun 7 2019 21:17:32