Point Cloud Library (PCL)  1.10.0-dev
octree_pointcloud.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_OCTREE_POINTCLOUD_HPP_
40 #define PCL_OCTREE_POINTCLOUD_HPP_
41 
42 #include <cassert>
43 
44 #include <pcl/common/common.h>
45 #include <pcl/octree/impl/octree_base.hpp>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
50  OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()),
51  epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
52  max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
53 {
54  assert (resolution > 0.0f);
55 }
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////
58 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
60 {
61  if (indices_)
62  {
63  for (const int &index : *indices_)
64  {
65  assert( (index >= 0) && (index < static_cast<int> (input_->points.size ())));
66 
67  if (isFinite (input_->points[index]))
68  {
69  // add points to octree
70  this->addPointIdx (index);
71  }
72  }
73  }
74  else
75  {
76  for (std::size_t i = 0; i < input_->points.size (); i++)
77  {
78  if (isFinite (input_->points[i]))
79  {
80  // add points to octree
81  this->addPointIdx (static_cast<unsigned int> (i));
82  }
83  }
84  }
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////
88 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
90 {
91  this->addPointIdx (point_idx_arg);
92  if (indices_arg)
93  indices_arg->push_back (point_idx_arg);
94 }
95 
96 //////////////////////////////////////////////////////////////////////////////////////////////
97 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
99 {
100  assert (cloud_arg==input_);
101 
102  cloud_arg->push_back (point_arg);
103 
104  this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
105 }
106 
107 //////////////////////////////////////////////////////////////////////////////////////////////
108 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
110  IndicesPtr indices_arg)
111 {
112  assert (cloud_arg==input_);
113  assert (indices_arg==indices_);
114 
115  cloud_arg->push_back (point_arg);
116 
117  this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
118 }
119 
120 //////////////////////////////////////////////////////////////////////////////////////////////
121 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
123 {
124  if (!isPointWithinBoundingBox (point_arg))
125  {
126  return false;
127  }
128 
129  OctreeKey key;
130 
131  // generate key for point
132  this->genOctreeKeyforPoint (point_arg, key);
133 
134  // search for key in octree
135  return (this->existLeaf (key));
136 }
137 
138 //////////////////////////////////////////////////////////////////////////////////////////////
139 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
141 {
142  // retrieve point from input cloud
143  const PointT& point = this->input_->points[point_idx_arg];
144 
145  // search for voxel at point in octree
146  return (this->isVoxelOccupiedAtPoint (point));
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////////
150 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
152  const double point_x_arg, const double point_y_arg, const double point_z_arg) const
153 {
154  // create a new point with the argument coordinates
155  PointT point;
156  point.x = point_x_arg;
157  point.y = point_y_arg;
158  point.z = point_z_arg;
159 
160  // search for voxel at point in octree
161  return (this->isVoxelOccupiedAtPoint (point));
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////
165 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
167 {
168  if (!isPointWithinBoundingBox (point_arg))
169  {
170  return;
171  }
172 
173  OctreeKey key;
174 
175  // generate key for point
176  this->genOctreeKeyforPoint (point_arg, key);
177 
178  this->removeLeaf (key);
179 }
180 
181 //////////////////////////////////////////////////////////////////////////////////////////////
182 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
184 {
185  // retrieve point from input cloud
186  const PointT& point = this->input_->points[point_idx_arg];
187 
188  // delete leaf at point
189  this->deleteVoxelAtPoint (point);
190 }
191 
192 //////////////////////////////////////////////////////////////////////////////////////////////
193 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
195  AlignedPointTVector &voxel_center_list_arg) const
196 {
197  OctreeKey key;
198  key.x = key.y = key.z = 0;
199 
200  voxel_center_list_arg.clear ();
201 
202  return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
203 
204 }
205 
206 //////////////////////////////////////////////////////////////////////////////////////////////
207 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
209  const Eigen::Vector3f& origin,
210  const Eigen::Vector3f& end,
211  AlignedPointTVector &voxel_center_list,
212  float precision)
213 {
214  Eigen::Vector3f direction = end - origin;
215  float norm = direction.norm ();
216  direction.normalize ();
217 
218  const float step_size = static_cast<float> (resolution_) * precision;
219  // Ensure we get at least one step for the first voxel.
220  const int nsteps = std::max (1, static_cast<int> (norm / step_size));
221 
222  OctreeKey prev_key;
223 
224  bool bkeyDefined = false;
225 
226  // Walk along the line segment with small steps.
227  for (int i = 0; i < nsteps; ++i)
228  {
229  Eigen::Vector3f p = origin + (direction * step_size * static_cast<float> (i));
230 
231  PointT octree_p;
232  octree_p.x = p.x ();
233  octree_p.y = p.y ();
234  octree_p.z = p.z ();
235 
236  OctreeKey key;
237  this->genOctreeKeyforPoint (octree_p, key);
238 
239  // Not a new key, still the same voxel.
240  if ((key == prev_key) && (bkeyDefined) )
241  continue;
242 
243  prev_key = key;
244  bkeyDefined = true;
245 
246  PointT center;
247  genLeafNodeCenterFromOctreeKey (key, center);
248  voxel_center_list.push_back (center);
249  }
250 
251  OctreeKey end_key;
252  PointT end_p;
253  end_p.x = end.x ();
254  end_p.y = end.y ();
255  end_p.z = end.z ();
256  this->genOctreeKeyforPoint (end_p, end_key);
257  if (!(end_key == prev_key))
258  {
259  PointT center;
260  genLeafNodeCenterFromOctreeKey (end_key, center);
261  voxel_center_list.push_back (center);
262  }
263 
264  return (static_cast<int> (voxel_center_list.size ()));
265 }
266 
267 //////////////////////////////////////////////////////////////////////////////////////////////
268 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
270 {
271 
272  double minX, minY, minZ, maxX, maxY, maxZ;
273 
274  PointT min_pt;
275  PointT max_pt;
276 
277  // bounding box cannot be changed once the octree contains elements
278  assert (this->leaf_count_ == 0);
279 
280  pcl::getMinMax3D (*input_, min_pt, max_pt);
281 
282  float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
283 
284  minX = min_pt.x;
285  minY = min_pt.y;
286  minZ = min_pt.z;
287 
288  maxX = max_pt.x + minValue;
289  maxY = max_pt.y + minValue;
290  maxZ = max_pt.z + minValue;
291 
292  // generate bit masks for octree
293  defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
294 }
295 
296 //////////////////////////////////////////////////////////////////////////////////////////////
297 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
299  const double min_y_arg,
300  const double min_z_arg,
301  const double max_x_arg,
302  const double max_y_arg,
303  const double max_z_arg)
304 {
305  // bounding box cannot be changed once the octree contains elements
306  assert (this->leaf_count_ == 0);
307 
308  assert (max_x_arg >= min_x_arg);
309  assert (max_y_arg >= min_y_arg);
310  assert (max_z_arg >= min_z_arg);
311 
312  min_x_ = min_x_arg;
313  max_x_ = max_x_arg;
314 
315  min_y_ = min_y_arg;
316  max_y_ = max_y_arg;
317 
318  min_z_ = min_z_arg;
319  max_z_ = max_z_arg;
320 
321  min_x_ = std::min (min_x_, max_x_);
322  min_y_ = std::min (min_y_, max_y_);
323  min_z_ = std::min (min_z_, max_z_);
324 
325  max_x_ = std::max (min_x_, max_x_);
326  max_y_ = std::max (min_y_, max_y_);
327  max_z_ = std::max (min_z_, max_z_);
328 
329  // generate bit masks for octree
330  getKeyBitSize ();
331 
332  bounding_box_defined_ = true;
333 }
334 
335 //////////////////////////////////////////////////////////////////////////////////////////////
336 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
338  const double max_x_arg, const double max_y_arg, const double max_z_arg)
339 {
340  // bounding box cannot be changed once the octree contains elements
341  assert (this->leaf_count_ == 0);
342 
343  assert (max_x_arg >= 0.0f);
344  assert (max_y_arg >= 0.0f);
345  assert (max_z_arg >= 0.0f);
346 
347  min_x_ = 0.0f;
348  max_x_ = max_x_arg;
349 
350  min_y_ = 0.0f;
351  max_y_ = max_y_arg;
352 
353  min_z_ = 0.0f;
354  max_z_ = max_z_arg;
355 
356  min_x_ = std::min (min_x_, max_x_);
357  min_y_ = std::min (min_y_, max_y_);
358  min_z_ = std::min (min_z_, max_z_);
359 
360  max_x_ = std::max (min_x_, max_x_);
361  max_y_ = std::max (min_y_, max_y_);
362  max_z_ = std::max (min_z_, max_z_);
363 
364  // generate bit masks for octree
365  getKeyBitSize ();
366 
367  bounding_box_defined_ = true;
368 }
369 
370 //////////////////////////////////////////////////////////////////////////////////////////////
371 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
373 {
374  // bounding box cannot be changed once the octree contains elements
375  assert (this->leaf_count_ == 0);
376 
377  assert (cubeLen_arg >= 0.0f);
378 
379  min_x_ = 0.0f;
380  max_x_ = cubeLen_arg;
381 
382  min_y_ = 0.0f;
383  max_y_ = cubeLen_arg;
384 
385  min_z_ = 0.0f;
386  max_z_ = cubeLen_arg;
387 
388  min_x_ = std::min (min_x_, max_x_);
389  min_y_ = std::min (min_y_, max_y_);
390  min_z_ = std::min (min_z_, max_z_);
391 
392  max_x_ = std::max (min_x_, max_x_);
393  max_y_ = std::max (min_y_, max_y_);
394  max_z_ = std::max (min_z_, max_z_);
395 
396  // generate bit masks for octree
397  getKeyBitSize ();
398 
399  bounding_box_defined_ = true;
400 }
401 
402 //////////////////////////////////////////////////////////////////////////////////////////////
403 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
405  double& min_x_arg, double& min_y_arg, double& min_z_arg,
406  double& max_x_arg, double& max_y_arg, double& max_z_arg) const
407 {
408  min_x_arg = min_x_;
409  min_y_arg = min_y_;
410  min_z_arg = min_z_;
411 
412  max_x_arg = max_x_;
413  max_y_arg = max_y_;
414  max_z_arg = max_z_;
415 }
416 
417 
418 //////////////////////////////////////////////////////////////////////////////////////////////
419 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
420 void
422 {
423 
424  const float minValue = std::numeric_limits<float>::epsilon ();
425 
426  // increase octree size until point fits into bounding box
427  while (true)
428  {
429  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
430  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
431  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
432 
433  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
434  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
435  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
436 
437  // do we violate any bounds?
438  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
439  || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
440  {
441 
442  if (bounding_box_defined_)
443  {
444 
445  double octreeSideLen;
446  unsigned char child_idx;
447 
448  // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2
449  child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
450  | ((!bUpperBoundViolationZ)));
451 
452  BranchNode* newRootBranch;
453 
454  newRootBranch = new BranchNode();
455  this->branch_count_++;
456 
457  this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
458 
459  this->root_node_ = newRootBranch;
460 
461  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
462 
463  if (!bUpperBoundViolationX)
464  min_x_ -= octreeSideLen;
465 
466  if (!bUpperBoundViolationY)
467  min_y_ -= octreeSideLen;
468 
469  if (!bUpperBoundViolationZ)
470  min_z_ -= octreeSideLen;
471 
472  // configure tree depth of octree
473  this->octree_depth_++;
474  this->setTreeDepth (this->octree_depth_);
475 
476  // recalculate bounding box width
477  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
478 
479  // increase octree bounding box
480  max_x_ = min_x_ + octreeSideLen;
481  max_y_ = min_y_ + octreeSideLen;
482  max_z_ = min_z_ + octreeSideLen;
483 
484  }
485  // bounding box is not defined - set it to point position
486  else
487  {
488  // octree is empty - we set the center of the bounding box to our first pixel
489  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
490  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
491  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
492 
493  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
494  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
495  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
496 
497  getKeyBitSize ();
498 
499  bounding_box_defined_ = true;
500  }
501 
502  }
503  else
504  // no bound violations anymore - leave while loop
505  break;
506  }
507 }
508 
509 //////////////////////////////////////////////////////////////////////////////////////////////
510 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
511 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask)
512 {
513 
514  if (depth_mask)
515  {
516  // get amount of objects in leaf container
517  std::size_t leaf_obj_count = (*leaf_node)->getSize ();
518 
519  // copy leaf data
520  std::vector<int> leafIndices;
521  leafIndices.reserve(leaf_obj_count);
522 
523  (*leaf_node)->getPointIndices(leafIndices);
524 
525  // delete current leaf node
526  this->deleteBranchChild(*parent_branch, child_idx);
527  this->leaf_count_ --;
528 
529  // create new branch node
530  BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
531  this->branch_count_ ++;
532 
533  // add data to new branch
534  OctreeKey new_index_key;
535 
536  for (const int &leafIndex : leafIndices)
537  {
538 
539  const PointT& point_from_index = input_->points[leafIndex];
540  // generate key
541  genOctreeKeyforPoint (point_from_index, new_index_key);
542 
543  LeafNode* newLeaf;
544  BranchNode* newBranchParent;
545  this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
546 
547  (*newLeaf)->addPointIndex(leafIndex);
548  }
549  }
550 
551 
552 }
553 
554 
555 //////////////////////////////////////////////////////////////////////////////////////////////
556 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
558 {
559  OctreeKey key;
560 
561  assert (point_idx_arg < static_cast<int> (input_->points.size ()));
562 
563  const PointT& point = input_->points[point_idx_arg];
564 
565  // make sure bounding box is big enough
566  adoptBoundingBoxToPoint (point);
567 
568  // generate key
569  genOctreeKeyforPoint (point, key);
570 
571  LeafNode* leaf_node;
572  BranchNode* parent_branch_of_leaf_node;
573  unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
574 
575  if (this->dynamic_depth_enabled_ && depth_mask)
576  {
577  // get amount of objects in leaf container
578  std::size_t leaf_obj_count = (*leaf_node)->getSize ();
579 
580  while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
581  {
582  // index to branch child
583  unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2);
584 
585  expandLeafNode (leaf_node,
586  parent_branch_of_leaf_node,
587  child_idx,
588  depth_mask);
589 
590  depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
591  leaf_obj_count = (*leaf_node)->getSize ();
592  }
593 
594  }
595 
596  (*leaf_node)->addPointIndex (point_idx_arg);
597 }
598 
599 //////////////////////////////////////////////////////////////////////////////////////////////
600 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> const PointT&
602 {
603  // retrieve point from input cloud
604  assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
605  return (this->input_->points[index_arg]);
606 }
607 
608 //////////////////////////////////////////////////////////////////////////////////////////////
609 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
611 {
612  unsigned int max_voxels;
613 
614  unsigned int max_key_x;
615  unsigned int max_key_y;
616  unsigned int max_key_z;
617 
618  double octree_side_len;
619 
620  const float minValue = std::numeric_limits<float>::epsilon();
621 
622  // find maximum key values for x, y, z
623  max_key_x = static_cast<unsigned int> (std::ceil ((max_x_ - min_x_ - minValue) / resolution_));
624  max_key_y = static_cast<unsigned int> (std::ceil ((max_y_ - min_y_ - minValue) / resolution_));
625  max_key_z = static_cast<unsigned int> (std::ceil ((max_z_ - min_z_ - minValue) / resolution_));
626 
627  // find maximum amount of keys
628  max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
629 
630 
631  // tree depth == amount of bits of max_voxels
632  this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (std::log2 (max_voxels) - minValue)))),
633  static_cast<unsigned int> (0));
634 
635  octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
636 
637  if (this->leaf_count_ == 0)
638  {
639  double octree_oversize_x;
640  double octree_oversize_y;
641  double octree_oversize_z;
642 
643  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
644  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
645  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
646 
647  assert (octree_oversize_x > -minValue);
648  assert (octree_oversize_y > -minValue);
649  assert (octree_oversize_z > -minValue);
650 
651  if (octree_oversize_x > minValue)
652  {
653  min_x_ -= octree_oversize_x;
654  max_x_ += octree_oversize_x;
655  }
656  if (octree_oversize_y > minValue)
657  {
658  min_y_ -= octree_oversize_y;
659  max_y_ += octree_oversize_y;
660  }
661  if (octree_oversize_z > minValue)
662  {
663  min_z_ -= octree_oversize_z;
664  max_z_ += octree_oversize_z;
665  }
666  }
667  else
668  {
669  max_x_ = min_x_ + octree_side_len;
670  max_y_ = min_y_ + octree_side_len;
671  max_z_ = min_z_ + octree_side_len;
672  }
673 
674  // configure tree depth of octree
675  this->setTreeDepth (this->octree_depth_);
676 
677 }
678 
679 //////////////////////////////////////////////////////////////////////////////////////////////
680 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
682  OctreeKey & key_arg) const
683  {
684  // calculate integer key for point coordinates
685  key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
686  key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
687  key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
688 
689  assert (key_arg.x <= this->max_key_.x);
690  assert (key_arg.y <= this->max_key_.y);
691  assert (key_arg.z <= this->max_key_.z);
692  }
693 
694 //////////////////////////////////////////////////////////////////////////////////////////////
695 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
697  const double point_x_arg, const double point_y_arg,
698  const double point_z_arg, OctreeKey & key_arg) const
699 {
700  PointT temp_point;
701 
702  temp_point.x = static_cast<float> (point_x_arg);
703  temp_point.y = static_cast<float> (point_y_arg);
704  temp_point.z = static_cast<float> (point_z_arg);
705 
706  // generate key for point
707  genOctreeKeyforPoint (temp_point, key_arg);
708 }
709 
710 //////////////////////////////////////////////////////////////////////////////////////////////
711 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
713 {
714  const PointT temp_point = getPointByIndex (data_arg);
715 
716  // generate key for point
717  genOctreeKeyforPoint (temp_point, key_arg);
718 
719  return (true);
720 }
721 
722 //////////////////////////////////////////////////////////////////////////////////////////////
723 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
725 {
726  // define point to leaf node voxel center
727  point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_);
728  point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_);
729  point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_);
730 }
731 
732 //////////////////////////////////////////////////////////////////////////////////////////////
733 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
735  const OctreeKey & key_arg,
736  unsigned int tree_depth_arg,
737  PointT& point_arg) const
738 {
739  // generate point for voxel center defined by treedepth (bitLen) and key
740  point_arg.x = static_cast<float> ((static_cast <double> (key_arg.x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_);
741  point_arg.y = static_cast<float> ((static_cast <double> (key_arg.y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_);
742  point_arg.z = static_cast<float> ((static_cast <double> (key_arg.z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_);
743 }
744 
745 //////////////////////////////////////////////////////////////////////////////////////////////
746 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
748  const OctreeKey & key_arg,
749  unsigned int tree_depth_arg,
750  Eigen::Vector3f &min_pt,
751  Eigen::Vector3f &max_pt) const
752 {
753  // calculate voxel size of current tree depth
754  double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
755 
756  // calculate voxel bounds
757  min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->min_x_);
758  min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->min_y_);
759  min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->min_z_);
760 
761  max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->min_x_);
762  max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->min_y_);
763  max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->min_z_);
764 }
765 
766 //////////////////////////////////////////////////////////////////////////////////////////////
767 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
769 {
770  double side_len;
771 
772  // side length of the voxel cube increases exponentially with the octree depth
773  side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
774 
775  // squared voxel side length
776  side_len *= side_len;
777 
778  return (side_len);
779 }
780 
781 //////////////////////////////////////////////////////////////////////////////////////////////
782 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
784 {
785  // return the squared side length of the voxel cube as a function of the octree depth
786  return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
787 }
788 
789 //////////////////////////////////////////////////////////////////////////////////////////////
790 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
792  const BranchNode* node_arg,
793  const OctreeKey& key_arg,
794  AlignedPointTVector &voxel_center_list_arg) const
795 {
796  int voxel_count = 0;
797 
798  // iterate over all children
799  for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
800  {
801  if (!this->branchHasChild (*node_arg, child_idx))
802  continue;
803 
804  const OctreeNode * child_node;
805  child_node = this->getBranchChildPtr (*node_arg, child_idx);
806 
807  // generate new key for current branch voxel
808  OctreeKey new_key;
809  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
810  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
811  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
812 
813  switch (child_node->getNodeType ())
814  {
815  case BRANCH_NODE:
816  {
817  // recursively proceed with indexed child branch
818  voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
819  break;
820  }
821  case LEAF_NODE:
822  {
823  PointT new_point;
824 
825  genLeafNodeCenterFromOctreeKey (new_key, new_point);
826  voxel_center_list_arg.push_back (new_point);
827 
828  voxel_count++;
829  break;
830  }
831  default:
832  break;
833  }
834  }
835  return (voxel_count);
836 }
837 
838 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
839 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
840 
841 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
842 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
843 
844 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
845 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
846 
847 #endif /* OCTREE_POINTCLOUD_HPP_ */
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
void addPointsFromInputCloud()
Add points from input point cloud to octree.
int getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f &origin, const Eigen::Vector3f &end, AlignedPointTVector &voxel_center_list, float precision=0.2)
Get a PointT vector of centers of voxels intersected by a line segment.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void getBoundingBox(double &min_x_arg, double &min_y_arg, double &min_z_arg, double &max_x_arg, double &max_y_arg, double &max_z_arg) const
Get bounding box for octree.
Define standard C methods and C++ classes that are common to all methods.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, unsigned int tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
Generate bounds of an octree voxel using octree key and tree depth arguments.
int getOccupiedVoxelCentersRecursive(const BranchNode *node_arg, const OctreeKey &key_arg, AlignedPointTVector &voxel_center_list_arg) const
Recursively search the tree for all leaf nodes and return a vector of voxel centers.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:136
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:242
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Octree key class
Definition: octree_key.h:50
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Abstract octree leaf class
Definition: octree_nodes.h:96
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, unsigned int tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, unsigned int depth_mask)
Add point at index from input pointcloud dataset to octree.
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Abstract octree branch class
Definition: octree_nodes.h:203
Abstract octree node class
Definition: octree_nodes.h:67
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.