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