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