Point Cloud Library (PCL)  1.11.1-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 #pragma once
40 
41 #include <pcl/common/common.h>
42 #include <pcl/common/point_tests.h> // for pcl::isFinite
43 #include <pcl/octree/impl/octree_base.hpp>
44 
45 #include <cassert>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT,
49  typename LeafContainerT,
50  typename BranchContainerT,
51  typename OctreeT>
53  OctreePointCloud(const double resolution)
54 : OctreeT()
55 , input_(PointCloudConstPtr())
56 , indices_(IndicesConstPtr())
57 , epsilon_(0)
58 , resolution_(resolution)
59 , min_x_(0.0f)
60 , max_x_(resolution)
61 , min_y_(0.0f)
62 , max_y_(resolution)
63 , min_z_(0.0f)
64 , max_z_(resolution)
65 , bounding_box_defined_(false)
66 , max_objs_per_leaf_(0)
67 {
68  assert(resolution > 0.0f);
69 }
70 
71 //////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT,
73  typename LeafContainerT,
74  typename BranchContainerT,
75  typename OctreeT>
76 void
79 {
80  if (indices_) {
81  for (const int& index : *indices_) {
82  assert((index >= 0) && (index < static_cast<int>(input_->size())));
83 
84  if (isFinite((*input_)[index])) {
85  // add points to octree
86  this->addPointIdx(index);
87  }
88  }
89  }
90  else {
91  for (std::size_t i = 0; i < input_->size(); i++) {
92  if (isFinite((*input_)[i])) {
93  // add points to octree
94  this->addPointIdx(static_cast<unsigned int>(i));
95  }
96  }
97  }
98 }
99 
100 //////////////////////////////////////////////////////////////////////////////////////////////
101 template <typename PointT,
102  typename LeafContainerT,
103  typename BranchContainerT,
104  typename OctreeT>
105 void
107  addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
108 {
109  this->addPointIdx(point_idx_arg);
110  if (indices_arg)
111  indices_arg->push_back(point_idx_arg);
112 }
113 
114 //////////////////////////////////////////////////////////////////////////////////////////////
115 template <typename PointT,
116  typename LeafContainerT,
117  typename BranchContainerT,
118  typename OctreeT>
119 void
121  addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg)
122 {
123  assert(cloud_arg == input_);
124 
125  cloud_arg->push_back(point_arg);
126 
127  this->addPointIdx(static_cast<const int>(cloud_arg->size()) - 1);
128 }
129 
130 //////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointT,
132  typename LeafContainerT,
133  typename BranchContainerT,
134  typename OctreeT>
135 void
137  addPointToCloud(const PointT& point_arg,
138  PointCloudPtr cloud_arg,
139  IndicesPtr indices_arg)
140 {
141  assert(cloud_arg == input_);
142  assert(indices_arg == indices_);
143 
144  cloud_arg->push_back(point_arg);
145 
146  this->addPointFromCloud(static_cast<const int>(cloud_arg->size()) - 1, indices_arg);
147 }
148 
149 //////////////////////////////////////////////////////////////////////////////////////////////
150 template <typename PointT,
151  typename LeafContainerT,
152  typename BranchContainerT,
153  typename OctreeT>
154 bool
156  isVoxelOccupiedAtPoint(const PointT& point_arg) const
157 {
158  if (!isPointWithinBoundingBox(point_arg)) {
159  return false;
160  }
161 
162  OctreeKey key;
163 
164  // generate key for point
165  this->genOctreeKeyforPoint(point_arg, key);
166 
167  // search for key in octree
168  return (this->existLeaf(key));
169 }
170 
171 //////////////////////////////////////////////////////////////////////////////////////////////
172 template <typename PointT,
173  typename LeafContainerT,
174  typename BranchContainerT,
175  typename OctreeT>
176 bool
178  isVoxelOccupiedAtPoint(const int& point_idx_arg) const
179 {
180  // retrieve point from input cloud
181  const PointT& point = (*this->input_)[point_idx_arg];
182 
183  // search for voxel at point in octree
184  return (this->isVoxelOccupiedAtPoint(point));
185 }
186 
187 //////////////////////////////////////////////////////////////////////////////////////////////
188 template <typename PointT,
189  typename LeafContainerT,
190  typename BranchContainerT,
191  typename OctreeT>
192 bool
194  isVoxelOccupiedAtPoint(const double point_x_arg,
195  const double point_y_arg,
196  const double point_z_arg) const
197 {
198  // create a new point with the argument coordinates
199  PointT point;
200  point.x = point_x_arg;
201  point.y = point_y_arg;
202  point.z = point_z_arg;
203 
204  // search for voxel at point in octree
205  return (this->isVoxelOccupiedAtPoint(point));
206 }
207 
208 //////////////////////////////////////////////////////////////////////////////////////////////
209 template <typename PointT,
210  typename LeafContainerT,
211  typename BranchContainerT,
212  typename OctreeT>
213 void
215  deleteVoxelAtPoint(const PointT& point_arg)
216 {
217  if (!isPointWithinBoundingBox(point_arg)) {
218  return;
219  }
220 
221  OctreeKey key;
222 
223  // generate key for point
224  this->genOctreeKeyforPoint(point_arg, key);
225 
226  this->removeLeaf(key);
227 }
228 
229 //////////////////////////////////////////////////////////////////////////////////////////////
230 template <typename PointT,
231  typename LeafContainerT,
232  typename BranchContainerT,
233  typename OctreeT>
234 void
236  deleteVoxelAtPoint(const int& point_idx_arg)
237 {
238  // retrieve point from input cloud
239  const PointT& point = (*this->input_)[point_idx_arg];
240 
241  // delete leaf at point
242  this->deleteVoxelAtPoint(point);
243 }
244 
245 //////////////////////////////////////////////////////////////////////////////////////////////
246 template <typename PointT,
247  typename LeafContainerT,
248  typename BranchContainerT,
249  typename OctreeT>
250 int
252  getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const
253 {
254  OctreeKey key;
255  key.x = key.y = key.z = 0;
256 
257  voxel_center_list_arg.clear();
258 
259  return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg);
260 }
261 
262 //////////////////////////////////////////////////////////////////////////////////////////////
263 template <typename PointT,
264  typename LeafContainerT,
265  typename BranchContainerT,
266  typename OctreeT>
267 int
269  getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
270  const Eigen::Vector3f& end,
271  AlignedPointTVector& voxel_center_list,
272  float precision)
273 {
274  Eigen::Vector3f direction = end - origin;
275  float norm = direction.norm();
276  direction.normalize();
277 
278  const float step_size = static_cast<float>(resolution_) * precision;
279  // Ensure we get at least one step for the first voxel.
280  const int nsteps = std::max(1, static_cast<int>(norm / step_size));
281 
282  OctreeKey prev_key;
283 
284  bool bkeyDefined = false;
285 
286  // Walk along the line segment with small steps.
287  for (int i = 0; i < nsteps; ++i) {
288  Eigen::Vector3f p = origin + (direction * step_size * static_cast<float>(i));
289 
290  PointT octree_p;
291  octree_p.x = p.x();
292  octree_p.y = p.y();
293  octree_p.z = p.z();
294 
295  OctreeKey key;
296  this->genOctreeKeyforPoint(octree_p, key);
297 
298  // Not a new key, still the same voxel.
299  if ((key == prev_key) && (bkeyDefined))
300  continue;
301 
302  prev_key = key;
303  bkeyDefined = true;
304 
305  PointT center;
306  genLeafNodeCenterFromOctreeKey(key, center);
307  voxel_center_list.push_back(center);
308  }
309 
310  OctreeKey end_key;
311  PointT end_p;
312  end_p.x = end.x();
313  end_p.y = end.y();
314  end_p.z = end.z();
315  this->genOctreeKeyforPoint(end_p, end_key);
316  if (!(end_key == prev_key)) {
317  PointT center;
318  genLeafNodeCenterFromOctreeKey(end_key, center);
319  voxel_center_list.push_back(center);
320  }
321 
322  return (static_cast<int>(voxel_center_list.size()));
323 }
324 
325 //////////////////////////////////////////////////////////////////////////////////////////////
326 template <typename PointT,
327  typename LeafContainerT,
328  typename BranchContainerT,
329  typename OctreeT>
330 void
333 {
334 
335  double minX, minY, minZ, maxX, maxY, maxZ;
336 
337  PointT min_pt;
338  PointT max_pt;
339 
340  // bounding box cannot be changed once the octree contains elements
341  assert(this->leaf_count_ == 0);
342 
343  pcl::getMinMax3D(*input_, min_pt, max_pt);
344 
345  float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
346 
347  minX = min_pt.x;
348  minY = min_pt.y;
349  minZ = min_pt.z;
350 
351  maxX = max_pt.x + minValue;
352  maxY = max_pt.y + minValue;
353  maxZ = max_pt.z + minValue;
354 
355  // generate bit masks for octree
356  defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
357 }
358 
359 //////////////////////////////////////////////////////////////////////////////////////////////
360 template <typename PointT,
361  typename LeafContainerT,
362  typename BranchContainerT,
363  typename OctreeT>
364 void
366  defineBoundingBox(const double min_x_arg,
367  const double min_y_arg,
368  const double min_z_arg,
369  const double max_x_arg,
370  const double max_y_arg,
371  const double max_z_arg)
372 {
373  // bounding box cannot be changed once the octree contains elements
374  assert(this->leaf_count_ == 0);
375 
376  assert(max_x_arg >= min_x_arg);
377  assert(max_y_arg >= min_y_arg);
378  assert(max_z_arg >= min_z_arg);
379 
380  min_x_ = min_x_arg;
381  max_x_ = max_x_arg;
382 
383  min_y_ = min_y_arg;
384  max_y_ = max_y_arg;
385 
386  min_z_ = min_z_arg;
387  max_z_ = max_z_arg;
388 
389  min_x_ = std::min(min_x_, max_x_);
390  min_y_ = std::min(min_y_, max_y_);
391  min_z_ = std::min(min_z_, max_z_);
392 
393  max_x_ = std::max(min_x_, max_x_);
394  max_y_ = std::max(min_y_, max_y_);
395  max_z_ = std::max(min_z_, max_z_);
396 
397  // generate bit masks for octree
398  getKeyBitSize();
399 
400  bounding_box_defined_ = true;
401 }
402 
403 //////////////////////////////////////////////////////////////////////////////////////////////
404 template <typename PointT,
405  typename LeafContainerT,
406  typename BranchContainerT,
407  typename OctreeT>
408 void
410  defineBoundingBox(const double max_x_arg,
411  const double max_y_arg,
412  const double max_z_arg)
413 {
414  // bounding box cannot be changed once the octree contains elements
415  assert(this->leaf_count_ == 0);
416 
417  assert(max_x_arg >= 0.0f);
418  assert(max_y_arg >= 0.0f);
419  assert(max_z_arg >= 0.0f);
420 
421  min_x_ = 0.0f;
422  max_x_ = max_x_arg;
423 
424  min_y_ = 0.0f;
425  max_y_ = max_y_arg;
426 
427  min_z_ = 0.0f;
428  max_z_ = max_z_arg;
429 
430  min_x_ = std::min(min_x_, max_x_);
431  min_y_ = std::min(min_y_, max_y_);
432  min_z_ = std::min(min_z_, max_z_);
433 
434  max_x_ = std::max(min_x_, max_x_);
435  max_y_ = std::max(min_y_, max_y_);
436  max_z_ = std::max(min_z_, max_z_);
437 
438  // generate bit masks for octree
439  getKeyBitSize();
440 
441  bounding_box_defined_ = true;
442 }
443 
444 //////////////////////////////////////////////////////////////////////////////////////////////
445 template <typename PointT,
446  typename LeafContainerT,
447  typename BranchContainerT,
448  typename OctreeT>
449 void
451  defineBoundingBox(const double cubeLen_arg)
452 {
453  // bounding box cannot be changed once the octree contains elements
454  assert(this->leaf_count_ == 0);
455 
456  assert(cubeLen_arg >= 0.0f);
457 
458  min_x_ = 0.0f;
459  max_x_ = cubeLen_arg;
460 
461  min_y_ = 0.0f;
462  max_y_ = cubeLen_arg;
463 
464  min_z_ = 0.0f;
465  max_z_ = cubeLen_arg;
466 
467  min_x_ = std::min(min_x_, max_x_);
468  min_y_ = std::min(min_y_, max_y_);
469  min_z_ = std::min(min_z_, max_z_);
470 
471  max_x_ = std::max(min_x_, max_x_);
472  max_y_ = std::max(min_y_, max_y_);
473  max_z_ = std::max(min_z_, max_z_);
474 
475  // generate bit masks for octree
476  getKeyBitSize();
477 
478  bounding_box_defined_ = true;
479 }
480 
481 //////////////////////////////////////////////////////////////////////////////////////////////
482 template <typename PointT,
483  typename LeafContainerT,
484  typename BranchContainerT,
485  typename OctreeT>
486 void
488  getBoundingBox(double& min_x_arg,
489  double& min_y_arg,
490  double& min_z_arg,
491  double& max_x_arg,
492  double& max_y_arg,
493  double& max_z_arg) const
494 {
495  min_x_arg = min_x_;
496  min_y_arg = min_y_;
497  min_z_arg = min_z_;
498 
499  max_x_arg = max_x_;
500  max_y_arg = max_y_;
501  max_z_arg = max_z_;
502 }
503 
504 //////////////////////////////////////////////////////////////////////////////////////////////
505 template <typename PointT,
506  typename LeafContainerT,
507  typename BranchContainerT,
508  typename OctreeT>
509 void
511  adoptBoundingBoxToPoint(const PointT& point_idx_arg)
512 {
513 
514  const float minValue = std::numeric_limits<float>::epsilon();
515 
516  // increase octree size until point fits into bounding box
517  while (true) {
518  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
519  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
520  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
521 
522  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
523  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
524  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
525 
526  // do we violate any bounds?
527  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
528  bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
529  (!bounding_box_defined_)) {
530 
531  if (bounding_box_defined_) {
532 
533  double octreeSideLen;
534  unsigned char child_idx;
535 
536  // octree not empty - we add another tree level and thus increase its size by a
537  // factor of 2*2*2
538  child_idx = static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
539  ((!bUpperBoundViolationY) << 1) |
540  ((!bUpperBoundViolationZ)));
541 
542  BranchNode* newRootBranch;
543 
544  newRootBranch = new BranchNode();
545  this->branch_count_++;
546 
547  this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
548 
549  this->root_node_ = newRootBranch;
550 
551  octreeSideLen = static_cast<double>(1 << this->octree_depth_) * resolution_;
552 
553  if (!bUpperBoundViolationX)
554  min_x_ -= octreeSideLen;
555 
556  if (!bUpperBoundViolationY)
557  min_y_ -= octreeSideLen;
558 
559  if (!bUpperBoundViolationZ)
560  min_z_ -= octreeSideLen;
561 
562  // configure tree depth of octree
563  this->octree_depth_++;
564  this->setTreeDepth(this->octree_depth_);
565 
566  // recalculate bounding box width
567  octreeSideLen =
568  static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
569 
570  // increase octree bounding box
571  max_x_ = min_x_ + octreeSideLen;
572  max_y_ = min_y_ + octreeSideLen;
573  max_z_ = min_z_ + octreeSideLen;
574  }
575  // bounding box is not defined - set it to point position
576  else {
577  // octree is empty - we set the center of the bounding box to our first pixel
578  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
579  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
580  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
581 
582  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
583  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
584  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
585 
586  getKeyBitSize();
587 
588  bounding_box_defined_ = true;
589  }
590  }
591  else
592  // no bound violations anymore - leave while loop
593  break;
594  }
595 }
596 
597 //////////////////////////////////////////////////////////////////////////////////////////////
598 template <typename PointT,
599  typename LeafContainerT,
600  typename BranchContainerT,
601  typename OctreeT>
602 void
605  BranchNode* parent_branch,
606  unsigned char child_idx,
607  unsigned int depth_mask)
608 {
609 
610  if (depth_mask) {
611  // get amount of objects in leaf container
612  std::size_t leaf_obj_count = (*leaf_node)->getSize();
613 
614  // copy leaf data
615  std::vector<int> leafIndices;
616  leafIndices.reserve(leaf_obj_count);
617 
618  (*leaf_node)->getPointIndices(leafIndices);
619 
620  // delete current leaf node
621  this->deleteBranchChild(*parent_branch, child_idx);
622  this->leaf_count_--;
623 
624  // create new branch node
625  BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
626  this->branch_count_++;
627 
628  // add data to new branch
629  OctreeKey new_index_key;
630 
631  for (const int& leafIndex : leafIndices) {
632 
633  const PointT& point_from_index = (*input_)[leafIndex];
634  // generate key
635  genOctreeKeyforPoint(point_from_index, new_index_key);
636 
637  LeafNode* newLeaf;
638  BranchNode* newBranchParent;
639  this->createLeafRecursive(
640  new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
641 
642  (*newLeaf)->addPointIndex(leafIndex);
643  }
644  }
645 }
646 
647 //////////////////////////////////////////////////////////////////////////////////////////////
648 template <typename PointT,
649  typename LeafContainerT,
650  typename BranchContainerT,
651  typename OctreeT>
652 void
654  addPointIdx(const int point_idx_arg)
655 {
656  OctreeKey key;
657 
658  assert(point_idx_arg < static_cast<int>(input_->size()));
659 
660  const PointT& point = (*input_)[point_idx_arg];
661 
662  // make sure bounding box is big enough
663  adoptBoundingBoxToPoint(point);
664 
665  // generate key
666  genOctreeKeyforPoint(point, key);
667 
668  LeafNode* leaf_node;
669  BranchNode* parent_branch_of_leaf_node;
670  unsigned int depth_mask = this->createLeafRecursive(
671  key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
672 
673  if (this->dynamic_depth_enabled_ && depth_mask) {
674  // get amount of objects in leaf container
675  std::size_t leaf_obj_count = (*leaf_node)->getSize();
676 
677  while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
678  // index to branch child
679  unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2);
680 
681  expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
682 
683  depth_mask = this->createLeafRecursive(key,
684  this->depth_mask_,
685  this->root_node_,
686  leaf_node,
687  parent_branch_of_leaf_node);
688  leaf_obj_count = (*leaf_node)->getSize();
689  }
690  }
691 
692  (*leaf_node)->addPointIndex(point_idx_arg);
693 }
694 
695 //////////////////////////////////////////////////////////////////////////////////////////////
696 template <typename PointT,
697  typename LeafContainerT,
698  typename BranchContainerT,
699  typename OctreeT>
700 const PointT&
702  getPointByIndex(const unsigned int index_arg) const
703 {
704  // retrieve point from input cloud
705  assert(index_arg < static_cast<unsigned int>(input_->size()));
706  return ((*this->input_)[index_arg]);
707 }
708 
709 //////////////////////////////////////////////////////////////////////////////////////////////
710 template <typename PointT,
711  typename LeafContainerT,
712  typename BranchContainerT,
713  typename OctreeT>
714 void
717 {
718  unsigned int max_voxels;
719 
720  unsigned int max_key_x;
721  unsigned int max_key_y;
722  unsigned int max_key_z;
723 
724  double octree_side_len;
725 
726  const float minValue = std::numeric_limits<float>::epsilon();
727 
728  // find maximum key values for x, y, z
729  max_key_x =
730  static_cast<unsigned int>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
731  max_key_y =
732  static_cast<unsigned int>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
733  max_key_z =
734  static_cast<unsigned int>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
735 
736  // find maximum amount of keys
737  max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z),
738  static_cast<unsigned int>(2));
739 
740  // tree depth == amount of bits of max_voxels
741  this->octree_depth_ =
742  std::max((std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
743  static_cast<unsigned int>(
744  std::ceil(std::log2(max_voxels) - minValue)))),
745  static_cast<unsigned int>(0));
746 
747  octree_side_len = static_cast<double>(1 << this->octree_depth_) * resolution_;
748 
749  if (this->leaf_count_ == 0) {
750  double octree_oversize_x;
751  double octree_oversize_y;
752  double octree_oversize_z;
753 
754  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
755  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
756  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
757 
758  assert(octree_oversize_x > -minValue);
759  assert(octree_oversize_y > -minValue);
760  assert(octree_oversize_z > -minValue);
761 
762  if (octree_oversize_x > minValue) {
763  min_x_ -= octree_oversize_x;
764  max_x_ += octree_oversize_x;
765  }
766  if (octree_oversize_y > minValue) {
767  min_y_ -= octree_oversize_y;
768  max_y_ += octree_oversize_y;
769  }
770  if (octree_oversize_z > minValue) {
771  min_z_ -= octree_oversize_z;
772  max_z_ += octree_oversize_z;
773  }
774  }
775  else {
776  max_x_ = min_x_ + octree_side_len;
777  max_y_ = min_y_ + octree_side_len;
778  max_z_ = min_z_ + octree_side_len;
779  }
780 
781  // configure tree depth of octree
782  this->setTreeDepth(this->octree_depth_);
783 }
784 
785 //////////////////////////////////////////////////////////////////////////////////////////////
786 template <typename PointT,
787  typename LeafContainerT,
788  typename BranchContainerT,
789  typename OctreeT>
790 void
792  genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
793 {
794  // calculate integer key for point coordinates
795  key_arg.x =
796  static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
797  key_arg.y =
798  static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
799  key_arg.z =
800  static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
801 
802  assert(key_arg.x <= this->max_key_.x);
803  assert(key_arg.y <= this->max_key_.y);
804  assert(key_arg.z <= this->max_key_.z);
805 }
806 
807 //////////////////////////////////////////////////////////////////////////////////////////////
808 template <typename PointT,
809  typename LeafContainerT,
810  typename BranchContainerT,
811  typename OctreeT>
812 void
814  genOctreeKeyforPoint(const double point_x_arg,
815  const double point_y_arg,
816  const double point_z_arg,
817  OctreeKey& key_arg) const
818 {
819  PointT temp_point;
820 
821  temp_point.x = static_cast<float>(point_x_arg);
822  temp_point.y = static_cast<float>(point_y_arg);
823  temp_point.z = static_cast<float>(point_z_arg);
824 
825  // generate key for point
826  genOctreeKeyforPoint(temp_point, key_arg);
827 }
828 
829 //////////////////////////////////////////////////////////////////////////////////////////////
830 template <typename PointT,
831  typename LeafContainerT,
832  typename BranchContainerT,
833  typename OctreeT>
834 bool
836  genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const
837 {
838  const PointT temp_point = getPointByIndex(data_arg);
839 
840  // generate key for point
841  genOctreeKeyforPoint(temp_point, key_arg);
842 
843  return (true);
844 }
845 
846 //////////////////////////////////////////////////////////////////////////////////////////////
847 template <typename PointT,
848  typename LeafContainerT,
849  typename BranchContainerT,
850  typename OctreeT>
851 void
854 {
855  // define point to leaf node voxel center
856  point.x = static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
857  this->min_x_);
858  point.y = static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
859  this->min_y_);
860  point.z = static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
861  this->min_z_);
862 }
863 
864 //////////////////////////////////////////////////////////////////////////////////////////////
865 template <typename PointT,
866  typename LeafContainerT,
867  typename BranchContainerT,
868  typename OctreeT>
869 void
872  unsigned int tree_depth_arg,
873  PointT& point_arg) const
874 {
875  // generate point for voxel center defined by treedepth (bitLen) and key
876  point_arg.x = static_cast<float>(
877  (static_cast<double>(key_arg.x) + 0.5f) *
878  (this->resolution_ *
879  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
880  this->min_x_);
881  point_arg.y = static_cast<float>(
882  (static_cast<double>(key_arg.y) + 0.5f) *
883  (this->resolution_ *
884  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
885  this->min_y_);
886  point_arg.z = static_cast<float>(
887  (static_cast<double>(key_arg.z) + 0.5f) *
888  (this->resolution_ *
889  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
890  this->min_z_);
891 }
892 
893 //////////////////////////////////////////////////////////////////////////////////////////////
894 template <typename PointT,
895  typename LeafContainerT,
896  typename BranchContainerT,
897  typename OctreeT>
898 void
901  unsigned int tree_depth_arg,
902  Eigen::Vector3f& min_pt,
903  Eigen::Vector3f& max_pt) const
904 {
905  // calculate voxel size of current tree depth
906  double voxel_side_len =
907  this->resolution_ *
908  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
909 
910  // calculate voxel bounds
911  min_pt(0) = static_cast<float>(static_cast<double>(key_arg.x) * voxel_side_len +
912  this->min_x_);
913  min_pt(1) = static_cast<float>(static_cast<double>(key_arg.y) * voxel_side_len +
914  this->min_y_);
915  min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
916  this->min_z_);
917 
918  max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
919  this->min_x_);
920  max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
921  this->min_y_);
922  max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
923  this->min_z_);
924 }
925 
926 //////////////////////////////////////////////////////////////////////////////////////////////
927 template <typename PointT,
928  typename LeafContainerT,
929  typename BranchContainerT,
930  typename OctreeT>
931 double
933  getVoxelSquaredSideLen(unsigned int tree_depth_arg) const
934 {
935  double side_len;
936 
937  // side length of the voxel cube increases exponentially with the octree depth
938  side_len = this->resolution_ *
939  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
940 
941  // squared voxel side length
942  side_len *= side_len;
943 
944  return (side_len);
945 }
946 
947 //////////////////////////////////////////////////////////////////////////////////////////////
948 template <typename PointT,
949  typename LeafContainerT,
950  typename BranchContainerT,
951  typename OctreeT>
952 double
954  getVoxelSquaredDiameter(unsigned int tree_depth_arg) const
955 {
956  // return the squared side length of the voxel cube as a function of the octree depth
957  return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
958 }
959 
960 //////////////////////////////////////////////////////////////////////////////////////////////
961 template <typename PointT,
962  typename LeafContainerT,
963  typename BranchContainerT,
964  typename OctreeT>
965 int
968  const OctreeKey& key_arg,
969  AlignedPointTVector& voxel_center_list_arg) const
970 {
971  int voxel_count = 0;
972 
973  // iterate over all children
974  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
975  if (!this->branchHasChild(*node_arg, child_idx))
976  continue;
977 
978  const OctreeNode* child_node;
979  child_node = this->getBranchChildPtr(*node_arg, child_idx);
980 
981  // generate new key for current branch voxel
982  OctreeKey new_key;
983  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
984  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
985  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
986 
987  switch (child_node->getNodeType()) {
988  case BRANCH_NODE: {
989  // recursively proceed with indexed child branch
990  voxel_count += getOccupiedVoxelCentersRecursive(
991  static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
992  break;
993  }
994  case LEAF_NODE: {
995  PointT new_point;
996 
997  genLeafNodeCenterFromOctreeKey(new_key, new_point);
998  voxel_center_list_arg.push_back(new_point);
999 
1000  voxel_count++;
1001  break;
1002  }
1003  default:
1004  break;
1005  }
1006  }
1007  return (voxel_count);
1008 }
1009 
1010 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
1011  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1012  T, \
1013  pcl::octree::OctreeContainerPointIndices, \
1014  pcl::octree::OctreeContainerEmpty, \
1015  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
1016  pcl::octree::OctreeContainerEmpty>>;
1017 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
1018  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1019  T, \
1020  pcl::octree::OctreeContainerPointIndices, \
1021  pcl::octree::OctreeContainerEmpty, \
1022  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1023  pcl::octree::OctreeContainerEmpty>>;
1024 
1025 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1026  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1027  T, \
1028  pcl::octree::OctreeContainerPointIndex, \
1029  pcl::octree::OctreeContainerEmpty, \
1030  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1031  pcl::octree::OctreeContainerEmpty>>;
1032 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1033  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1034  T, \
1035  pcl::octree::OctreeContainerPointIndex, \
1036  pcl::octree::OctreeContainerEmpty, \
1037  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1038  pcl::octree::OctreeContainerEmpty>>;
1039 
1040 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1041  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1042  T, \
1043  pcl::octree::OctreeContainerEmpty, \
1044  pcl::octree::OctreeContainerEmpty, \
1045  pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1046  pcl::octree::OctreeContainerEmpty>>;
1047 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1048  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1049  T, \
1050  pcl::octree::OctreeContainerEmpty, \
1051  pcl::octree::OctreeContainerEmpty, \
1052  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1053  pcl::octree::OctreeContainerEmpty>>;
pcl::octree::OctreePointCloud::getVoxelSquaredDiameter
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
Definition: octree_pointcloud.h:374
pcl::octree::OctreePointCloud::deleteVoxelAtPoint
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Definition: octree_pointcloud.hpp:215
common.h
pcl::octree::OctreePointCloud::genOctreeKeyforPoint
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
Definition: octree_pointcloud.hpp:792
pcl::isFinite
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
pcl::octree::OctreePointCloud::getOccupiedVoxelCenters
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
Definition: octree_pointcloud.hpp:252
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud::getKeyBitSize
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Definition: octree_pointcloud.hpp:716
pcl::octree::OctreePointCloud::genOctreeKeyForDataT
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
Definition: octree_pointcloud.hpp:836
pcl::octree::OctreePointCloud::getOccupiedVoxelCentersRecursive
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.
Definition: octree_pointcloud.hpp:967
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:78
pcl::octree::OctreePointCloud::getApproxIntersectedVoxelCentersBySegment
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.
Definition: octree_pointcloud.hpp:269
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
Definition: octree_pointcloud.hpp:511
pcl::octree::OctreePointCloud::addPointToCloud
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
Definition: octree_pointcloud.hpp:121
pcl::octree::OctreePointCloud::defineBoundingBox
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
Definition: octree_pointcloud.hpp:332
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::octree::OctreePointCloud::addPointIdx
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:654
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:61
pcl::octree::OctreePointCloud::getPointByIndex
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
Definition: octree_pointcloud.hpp:702
pcl::octree::LEAF_NODE
@ LEAF_NODE
Definition: octree_nodes.h:54
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:83
pcl::octree::BRANCH_NODE
@ BRANCH_NODE
Definition: octree_nodes.h:54
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_pointcloud.h:86
pcl::octree::OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty >
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:182
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:89
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:151
pcl::octree::OctreePointCloud::getVoxelSquaredSideLen
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
Definition: octree_pointcloud.h:390
pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
Definition: octree_pointcloud.hpp:156
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition: octree_pointcloud.hpp:107
pcl::octree::OctreeKey::getChildIdxWithDepthMask
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
pcl::getMinMax3D
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:243
pcl::octree::OctreePointCloud::genVoxelBoundsFromOctreeKey
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.
Definition: octree_pointcloud.hpp:900
pcl::octree::OctreePointCloud::genLeafNodeCenterFromOctreeKey
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
Definition: octree_pointcloud.hpp:853
pcl::octree::OctreePointCloud::getBoundingBox
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.
Definition: octree_pointcloud.hpp:488
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesPtr
shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_pointcloud.h:85
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:107
pcl::octree::OctreePointCloud::expandLeafNode
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.
Definition: octree_pointcloud.hpp:604
pcl::octree::OctreePointCloud::OctreePointCloud
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
Definition: octree_pointcloud.hpp:53
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:149
pcl::octree::OctreePointCloud::genVoxelCenterFromOctreeKey
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.
Definition: octree_pointcloud.hpp:871
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:150