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


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Tue Dec 12 2023 03:39:40