Point Cloud Library (PCL)  1.15.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 #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>
53 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, 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
76 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
77  addPointsFromInputCloud()
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
105 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
119 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
135 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
154 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
176 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
192 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
213 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
234 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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>
249 pcl::uindex_t
250 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, 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>
266 pcl::uindex_t
267 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, 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
330 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
331  defineBoundingBox()
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
369 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
405 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
438 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
469 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
492 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
493  adoptBoundingBoxToPoint(const PointT& point_arg)
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 ? 4 : 0) |
521  (bLowerBoundViolationY ? 2 : 0) |
522  (bLowerBoundViolationZ ? 1 : 0));
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
585 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
586  expandLeafNode(LeafNode* leaf_node,
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
635 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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&
683 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
697 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
698  getKeyBitSize()
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
765 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
784 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
806 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
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
823 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
824  genLeafNodeCenterFromOctreeKey(const OctreeKey& key, PointT& point) const
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
841 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
842  genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
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
870 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
871  genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
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) = min_pt(0) + voxel_side_len;
890  max_pt(1) = min_pt(1) + voxel_side_len;
891  max_pt(2) = min_pt(2) + voxel_side_len;
892 }
893 
894 //////////////////////////////////////////////////////////////////////////////////////////////
895 template <typename PointT,
896  typename LeafContainerT,
897  typename BranchContainerT,
898  typename OctreeT>
899 double
900 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
901  getVoxelSquaredSideLen(uindex_t tree_depth_arg) const
902 {
903  double side_len;
904 
905  // side length of the voxel cube increases exponentially with the octree depth
906  side_len = this->resolution_ *
907  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
908 
909  // squared voxel side length
910  side_len *= side_len;
911 
912  return (side_len);
913 }
914 
915 //////////////////////////////////////////////////////////////////////////////////////////////
916 template <typename PointT,
917  typename LeafContainerT,
918  typename BranchContainerT,
919  typename OctreeT>
920 double
921 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
922  getVoxelSquaredDiameter(uindex_t tree_depth_arg) const
923 {
924  // return the squared side length of the voxel cube as a function of the octree depth
925  return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
926 }
927 
928 //////////////////////////////////////////////////////////////////////////////////////////////
929 template <typename PointT,
930  typename LeafContainerT,
931  typename BranchContainerT,
932  typename OctreeT>
933 pcl::uindex_t
934 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
935  getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
936  const OctreeKey& key_arg,
937  AlignedPointTVector& voxel_center_list_arg) const
938 {
939  uindex_t voxel_count = 0;
940 
941  // iterate over all children
942  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
943  if (!this->branchHasChild(*node_arg, child_idx))
944  continue;
945 
946  const OctreeNode* child_node;
947  child_node = this->getBranchChildPtr(*node_arg, child_idx);
948 
949  // generate new key for current branch voxel
950  OctreeKey new_key;
951  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
952  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
953  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
954 
955  switch (child_node->getNodeType()) {
956  case BRANCH_NODE: {
957  // recursively proceed with indexed child branch
958  voxel_count += getOccupiedVoxelCentersRecursive(
959  static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
960  break;
961  }
962  case LEAF_NODE: {
963  PointT new_point;
964 
965  genLeafNodeCenterFromOctreeKey(new_key, new_point);
966  voxel_center_list_arg.push_back(new_point);
967 
968  voxel_count++;
969  break;
970  }
971  default:
972  break;
973  }
974  }
975  return (voxel_count);
976 }
977 
978 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
979  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
980  T, \
981  pcl::octree::OctreeContainerPointIndices, \
982  pcl::octree::OctreeContainerEmpty, \
983  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
984  pcl::octree::OctreeContainerEmpty>>;
985 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
986  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
987  T, \
988  pcl::octree::OctreeContainerPointIndices, \
989  pcl::octree::OctreeContainerEmpty, \
990  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
991  pcl::octree::OctreeContainerEmpty>>;
992 
993 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
994  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
995  T, \
996  pcl::octree::OctreeContainerPointIndex, \
997  pcl::octree::OctreeContainerEmpty, \
998  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
999  pcl::octree::OctreeContainerEmpty>>;
1000 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1001  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1002  T, \
1003  pcl::octree::OctreeContainerPointIndex, \
1004  pcl::octree::OctreeContainerEmpty, \
1005  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1006  pcl::octree::OctreeContainerEmpty>>;
1007 
1008 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1009  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1010  T, \
1011  pcl::octree::OctreeContainerEmpty, \
1012  pcl::octree::OctreeContainerEmpty, \
1013  pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1014  pcl::octree::OctreeContainerEmpty>>;
1015 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1016  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1017  T, \
1018  pcl::octree::OctreeContainerEmpty, \
1019  pcl::octree::OctreeContainerEmpty, \
1020  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1021  pcl::octree::OctreeContainerEmpty>>;
pcl::InitFailedException
An exception thrown when init can not be performed should be used in all the PCLBase class inheritant...
Definition: exceptions.h:197
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:179
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:54
pcl::octree::OctreeKey::maxDepth
static const unsigned char maxDepth
Definition: octree_key.h:142
pcl::octree::OctreeKey::getChildIdxWithDepthMask
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:59
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving the type of octree node (branch or leaf)
pcl::octree::OctreePointCloud::getVoxelSquaredDiameter
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
Definition: octree_pointcloud.h:378
pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
Definition: octree_pointcloud.hpp:493
pcl::octree::OctreePointCloud::getOccupiedVoxelCenters
uindex_t getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
Definition: octree_pointcloud.hpp:251
pcl::octree::OctreePointCloud::getPointByIndex
const PointT & getPointByIndex(uindex_t index_arg) const
Get point at index from input pointcloud dataset.
Definition: octree_pointcloud.hpp:684
pcl::octree::OctreePointCloud::expandLeafNode
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.
Definition: octree_pointcloud.hpp:586
pcl::octree::OctreePointCloud::deleteVoxelAtPoint
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Definition: octree_pointcloud.hpp:214
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:824
pcl::octree::OctreePointCloud::genVoxelCenterFromOctreeKey
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.
Definition: octree_pointcloud.hpp:842
pcl::octree::OctreePointCloud::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:89
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition: octree_pointcloud.hpp:106
pcl::octree::OctreePointCloud::OctreePointCloud
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
Definition: octree_pointcloud.hpp:54
pcl::octree::OctreePointCloud::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: octree_pointcloud.h:85
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:120
pcl::octree::OctreePointCloud::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud::defineBoundingBox
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
Definition: octree_pointcloud.hpp:331
pcl::octree::OctreePointCloud::genVoxelBoundsFromOctreeKey
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.
Definition: octree_pointcloud.hpp:871
pcl::octree::OctreePointCloud::LeafNode
typename OctreeT::LeafNode LeafNode
Definition: octree_pointcloud.h:76
pcl::octree::OctreePointCloud::addPointIdx
virtual void addPointIdx(uindex_t point_idx_arg)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:636
pcl::octree::OctreePointCloud::getVoxelSquaredSideLen
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
Definition: octree_pointcloud.h:394
pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
Definition: octree_pointcloud.hpp:155
pcl::octree::OctreePointCloud::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:107
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:77
pcl::octree::OctreePointCloud::getOccupiedVoxelCentersRecursive
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.
Definition: octree_pointcloud.hpp:935
pcl::octree::OctreePointCloud::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_pointcloud.h:77
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:766
pcl::octree::OctreePointCloud::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: octree_pointcloud.h:86
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:470
pcl::octree::OctreePointCloud::getApproxIntersectedVoxelCentersBySegment
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.
Definition: octree_pointcloud.hpp:268
pcl::octree::OctreePointCloud::genOctreeKeyForDataT
virtual bool genOctreeKeyForDataT(const index_t &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
Definition: octree_pointcloud.hpp:807
pcl::octree::OctreePointCloud::getKeyBitSize
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Definition: octree_pointcloud.hpp:698
common.h
Define standard C methods and C++ classes that are common to all methods.
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:295
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
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::index_t
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
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:594
types.h
Defines basic non-point types used by PCL.