Point Cloud Library (PCL)  1.11.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 
45 #include <cassert>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT,
49  typename LeafContainerT,
50  typename BranchContainerT,
51  typename OctreeT>
53  OctreePointCloud(const double resolution)
54 : OctreeT()
55 , input_(PointCloudConstPtr())
56 , indices_(IndicesConstPtr())
57 , epsilon_(0)
58 , resolution_(resolution)
59 , min_x_(0.0f)
60 , max_x_(resolution)
61 , min_y_(0.0f)
62 , max_y_(resolution)
63 , min_z_(0.0f)
64 , max_z_(resolution)
65 , bounding_box_defined_(false)
66 , max_objs_per_leaf_(0)
67 {
68  assert(resolution > 0.0f);
69 }
70 
71 //////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT,
73  typename LeafContainerT,
74  typename BranchContainerT,
75  typename OctreeT>
76 void
79 {
80  if (indices_) {
81  for (const int& index : *indices_) {
82  assert((index >= 0) && (index < static_cast<int>(input_->points.size())));
83 
84  if (isFinite(input_->points[index])) {
85  // add points to octree
86  this->addPointIdx(index);
87  }
88  }
89  }
90  else {
91  for (std::size_t i = 0; i < input_->points.size(); i++) {
92  if (isFinite(input_->points[i])) {
93  // add points to octree
94  this->addPointIdx(static_cast<unsigned int>(i));
95  }
96  }
97  }
98 }
99 
100 //////////////////////////////////////////////////////////////////////////////////////////////
101 template <typename PointT,
102  typename LeafContainerT,
103  typename BranchContainerT,
104  typename OctreeT>
105 void
107  addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
108 {
109  this->addPointIdx(point_idx_arg);
110  if (indices_arg)
111  indices_arg->push_back(point_idx_arg);
112 }
113 
114 //////////////////////////////////////////////////////////////////////////////////////////////
115 template <typename PointT,
116  typename LeafContainerT,
117  typename BranchContainerT,
118  typename OctreeT>
119 void
121  addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg)
122 {
123  assert(cloud_arg == input_);
124 
125  cloud_arg->push_back(point_arg);
126 
127  this->addPointIdx(static_cast<const int>(cloud_arg->points.size()) - 1);
128 }
129 
130 //////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointT,
132  typename LeafContainerT,
133  typename BranchContainerT,
134  typename OctreeT>
135 void
137  addPointToCloud(const PointT& point_arg,
138  PointCloudPtr cloud_arg,
139  IndicesPtr indices_arg)
140 {
141  assert(cloud_arg == input_);
142  assert(indices_arg == indices_);
143 
144  cloud_arg->push_back(point_arg);
145 
146  this->addPointFromCloud(static_cast<const int>(cloud_arg->points.size()) - 1,
147  indices_arg);
148 }
149 
150 //////////////////////////////////////////////////////////////////////////////////////////////
151 template <typename PointT,
152  typename LeafContainerT,
153  typename BranchContainerT,
154  typename OctreeT>
155 bool
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
179  isVoxelOccupiedAtPoint(const int& point_idx_arg) const
180 {
181  // retrieve point from input cloud
182  const PointT& point = this->input_->points[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
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
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
237  deleteVoxelAtPoint(const int& point_idx_arg)
238 {
239  // retrieve point from input cloud
240  const PointT& point = this->input_->points[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 int
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 int
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 int nsteps = std::max(1, static_cast<int>(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 (int 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<int>(voxel_center_list.size()));
324 }
325 
326 //////////////////////////////////////////////////////////////////////////////////////////////
327 template <typename PointT,
328  typename LeafContainerT,
329  typename BranchContainerT,
330  typename OctreeT>
331 void
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
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  assert(max_x_arg >= min_x_arg);
378  assert(max_y_arg >= min_y_arg);
379  assert(max_z_arg >= min_z_arg);
380 
381  min_x_ = min_x_arg;
382  max_x_ = max_x_arg;
383 
384  min_y_ = min_y_arg;
385  max_y_ = max_y_arg;
386 
387  min_z_ = min_z_arg;
388  max_z_ = max_z_arg;
389 
390  min_x_ = std::min(min_x_, max_x_);
391  min_y_ = std::min(min_y_, max_y_);
392  min_z_ = std::min(min_z_, max_z_);
393 
394  max_x_ = std::max(min_x_, max_x_);
395  max_y_ = std::max(min_y_, max_y_);
396  max_z_ = std::max(min_z_, max_z_);
397 
398  // generate bit masks for octree
399  getKeyBitSize();
400 
401  bounding_box_defined_ = true;
402 }
403 
404 //////////////////////////////////////////////////////////////////////////////////////////////
405 template <typename PointT,
406  typename LeafContainerT,
407  typename BranchContainerT,
408  typename OctreeT>
409 void
411  defineBoundingBox(const double max_x_arg,
412  const double max_y_arg,
413  const double max_z_arg)
414 {
415  // bounding box cannot be changed once the octree contains elements
416  assert(this->leaf_count_ == 0);
417 
418  assert(max_x_arg >= 0.0f);
419  assert(max_y_arg >= 0.0f);
420  assert(max_z_arg >= 0.0f);
421 
422  min_x_ = 0.0f;
423  max_x_ = max_x_arg;
424 
425  min_y_ = 0.0f;
426  max_y_ = max_y_arg;
427 
428  min_z_ = 0.0f;
429  max_z_ = max_z_arg;
430 
431  min_x_ = std::min(min_x_, max_x_);
432  min_y_ = std::min(min_y_, max_y_);
433  min_z_ = std::min(min_z_, max_z_);
434 
435  max_x_ = std::max(min_x_, max_x_);
436  max_y_ = std::max(min_y_, max_y_);
437  max_z_ = std::max(min_z_, max_z_);
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
452  defineBoundingBox(const double cubeLen_arg)
453 {
454  // bounding box cannot be changed once the octree contains elements
455  assert(this->leaf_count_ == 0);
456 
457  assert(cubeLen_arg >= 0.0f);
458 
459  min_x_ = 0.0f;
460  max_x_ = cubeLen_arg;
461 
462  min_y_ = 0.0f;
463  max_y_ = cubeLen_arg;
464 
465  min_z_ = 0.0f;
466  max_z_ = cubeLen_arg;
467 
468  min_x_ = std::min(min_x_, max_x_);
469  min_y_ = std::min(min_y_, max_y_);
470  min_z_ = std::min(min_z_, max_z_);
471 
472  max_x_ = std::max(min_x_, max_x_);
473  max_y_ = std::max(min_y_, max_y_);
474  max_z_ = std::max(min_z_, max_z_);
475 
476  // generate bit masks for octree
477  getKeyBitSize();
478 
479  bounding_box_defined_ = true;
480 }
481 
482 //////////////////////////////////////////////////////////////////////////////////////////////
483 template <typename PointT,
484  typename LeafContainerT,
485  typename BranchContainerT,
486  typename OctreeT>
487 void
489  getBoundingBox(double& min_x_arg,
490  double& min_y_arg,
491  double& min_z_arg,
492  double& max_x_arg,
493  double& max_y_arg,
494  double& max_z_arg) const
495 {
496  min_x_arg = min_x_;
497  min_y_arg = min_y_;
498  min_z_arg = min_z_;
499 
500  max_x_arg = max_x_;
501  max_y_arg = max_y_;
502  max_z_arg = max_z_;
503 }
504 
505 //////////////////////////////////////////////////////////////////////////////////////////////
506 template <typename PointT,
507  typename LeafContainerT,
508  typename BranchContainerT,
509  typename OctreeT>
510 void
512  adoptBoundingBoxToPoint(const PointT& point_idx_arg)
513 {
514 
515  const float minValue = std::numeric_limits<float>::epsilon();
516 
517  // increase octree size until point fits into bounding box
518  while (true) {
519  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
520  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
521  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
522 
523  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
524  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
525  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
526 
527  // do we violate any bounds?
528  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
529  bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
530  (!bounding_box_defined_)) {
531 
532  if (bounding_box_defined_) {
533 
534  double octreeSideLen;
535  unsigned char child_idx;
536 
537  // octree not empty - we add another tree level and thus increase its size by a
538  // factor of 2*2*2
539  child_idx = static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
540  ((!bUpperBoundViolationY) << 1) |
541  ((!bUpperBoundViolationZ)));
542 
543  BranchNode* newRootBranch;
544 
545  newRootBranch = new BranchNode();
546  this->branch_count_++;
547 
548  this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
549 
550  this->root_node_ = newRootBranch;
551 
552  octreeSideLen = static_cast<double>(1 << this->octree_depth_) * resolution_;
553 
554  if (!bUpperBoundViolationX)
555  min_x_ -= octreeSideLen;
556 
557  if (!bUpperBoundViolationY)
558  min_y_ -= octreeSideLen;
559 
560  if (!bUpperBoundViolationZ)
561  min_z_ -= octreeSideLen;
562 
563  // configure tree depth of octree
564  this->octree_depth_++;
565  this->setTreeDepth(this->octree_depth_);
566 
567  // recalculate bounding box width
568  octreeSideLen =
569  static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
570 
571  // increase octree bounding box
572  max_x_ = min_x_ + octreeSideLen;
573  max_y_ = min_y_ + octreeSideLen;
574  max_z_ = min_z_ + octreeSideLen;
575  }
576  // bounding box is not defined - set it to point position
577  else {
578  // octree is empty - we set the center of the bounding box to our first pixel
579  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
580  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
581  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
582 
583  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
584  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
585  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
586 
587  getKeyBitSize();
588 
589  bounding_box_defined_ = true;
590  }
591  }
592  else
593  // no bound violations anymore - leave while loop
594  break;
595  }
596 }
597 
598 //////////////////////////////////////////////////////////////////////////////////////////////
599 template <typename PointT,
600  typename LeafContainerT,
601  typename BranchContainerT,
602  typename OctreeT>
603 void
606  BranchNode* parent_branch,
607  unsigned char child_idx,
608  unsigned int depth_mask)
609 {
610 
611  if (depth_mask) {
612  // get amount of objects in leaf container
613  std::size_t leaf_obj_count = (*leaf_node)->getSize();
614 
615  // copy leaf data
616  std::vector<int> leafIndices;
617  leafIndices.reserve(leaf_obj_count);
618 
619  (*leaf_node)->getPointIndices(leafIndices);
620 
621  // delete current leaf node
622  this->deleteBranchChild(*parent_branch, child_idx);
623  this->leaf_count_--;
624 
625  // create new branch node
626  BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
627  this->branch_count_++;
628 
629  // add data to new branch
630  OctreeKey new_index_key;
631 
632  for (const int& leafIndex : leafIndices) {
633 
634  const PointT& point_from_index = input_->points[leafIndex];
635  // generate key
636  genOctreeKeyforPoint(point_from_index, new_index_key);
637 
638  LeafNode* newLeaf;
639  BranchNode* newBranchParent;
640  this->createLeafRecursive(
641  new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
642 
643  (*newLeaf)->addPointIndex(leafIndex);
644  }
645  }
646 }
647 
648 //////////////////////////////////////////////////////////////////////////////////////////////
649 template <typename PointT,
650  typename LeafContainerT,
651  typename BranchContainerT,
652  typename OctreeT>
653 void
655  addPointIdx(const int point_idx_arg)
656 {
657  OctreeKey key;
658 
659  assert(point_idx_arg < static_cast<int>(input_->points.size()));
660 
661  const PointT& point = input_->points[point_idx_arg];
662 
663  // make sure bounding box is big enough
664  adoptBoundingBoxToPoint(point);
665 
666  // generate key
667  genOctreeKeyforPoint(point, key);
668 
669  LeafNode* leaf_node;
670  BranchNode* parent_branch_of_leaf_node;
671  unsigned int depth_mask = this->createLeafRecursive(
672  key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
673 
674  if (this->dynamic_depth_enabled_ && depth_mask) {
675  // get amount of objects in leaf container
676  std::size_t leaf_obj_count = (*leaf_node)->getSize();
677 
678  while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
679  // index to branch child
680  unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2);
681 
682  expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
683 
684  depth_mask = this->createLeafRecursive(key,
685  this->depth_mask_,
686  this->root_node_,
687  leaf_node,
688  parent_branch_of_leaf_node);
689  leaf_obj_count = (*leaf_node)->getSize();
690  }
691  }
692 
693  (*leaf_node)->addPointIndex(point_idx_arg);
694 }
695 
696 //////////////////////////////////////////////////////////////////////////////////////////////
697 template <typename PointT,
698  typename LeafContainerT,
699  typename BranchContainerT,
700  typename OctreeT>
701 const PointT&
703  getPointByIndex(const unsigned int index_arg) const
704 {
705  // retrieve point from input cloud
706  assert(index_arg < static_cast<unsigned int>(input_->points.size()));
707  return (this->input_->points[index_arg]);
708 }
709 
710 //////////////////////////////////////////////////////////////////////////////////////////////
711 template <typename PointT,
712  typename LeafContainerT,
713  typename BranchContainerT,
714  typename OctreeT>
715 void
718 {
719  unsigned int max_voxels;
720 
721  unsigned int max_key_x;
722  unsigned int max_key_y;
723  unsigned int max_key_z;
724 
725  double octree_side_len;
726 
727  const float minValue = std::numeric_limits<float>::epsilon();
728 
729  // find maximum key values for x, y, z
730  max_key_x =
731  static_cast<unsigned int>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
732  max_key_y =
733  static_cast<unsigned int>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
734  max_key_z =
735  static_cast<unsigned int>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
736 
737  // find maximum amount of keys
738  max_voxels = std::max(std::max(std::max(max_key_x, max_key_y), max_key_z),
739  static_cast<unsigned int>(2));
740 
741  // tree depth == amount of bits of max_voxels
742  this->octree_depth_ =
743  std::max((std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
744  static_cast<unsigned int>(
745  std::ceil(std::log2(max_voxels) - minValue)))),
746  static_cast<unsigned int>(0));
747 
748  octree_side_len = static_cast<double>(1 << this->octree_depth_) * resolution_;
749 
750  if (this->leaf_count_ == 0) {
751  double octree_oversize_x;
752  double octree_oversize_y;
753  double octree_oversize_z;
754 
755  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
756  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
757  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
758 
759  assert(octree_oversize_x > -minValue);
760  assert(octree_oversize_y > -minValue);
761  assert(octree_oversize_z > -minValue);
762 
763  if (octree_oversize_x > minValue) {
764  min_x_ -= octree_oversize_x;
765  max_x_ += octree_oversize_x;
766  }
767  if (octree_oversize_y > minValue) {
768  min_y_ -= octree_oversize_y;
769  max_y_ += octree_oversize_y;
770  }
771  if (octree_oversize_z > minValue) {
772  min_z_ -= octree_oversize_z;
773  max_z_ += octree_oversize_z;
774  }
775  }
776  else {
777  max_x_ = min_x_ + octree_side_len;
778  max_y_ = min_y_ + octree_side_len;
779  max_z_ = min_z_ + octree_side_len;
780  }
781 
782  // configure tree depth of octree
783  this->setTreeDepth(this->octree_depth_);
784 }
785 
786 //////////////////////////////////////////////////////////////////////////////////////////////
787 template <typename PointT,
788  typename LeafContainerT,
789  typename BranchContainerT,
790  typename OctreeT>
791 void
793  genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
794 {
795  // calculate integer key for point coordinates
796  key_arg.x =
797  static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
798  key_arg.y =
799  static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
800  key_arg.z =
801  static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
802 
803  assert(key_arg.x <= this->max_key_.x);
804  assert(key_arg.y <= this->max_key_.y);
805  assert(key_arg.z <= this->max_key_.z);
806 }
807 
808 //////////////////////////////////////////////////////////////////////////////////////////////
809 template <typename PointT,
810  typename LeafContainerT,
811  typename BranchContainerT,
812  typename OctreeT>
813 void
815  genOctreeKeyforPoint(const double point_x_arg,
816  const double point_y_arg,
817  const double point_z_arg,
818  OctreeKey& key_arg) const
819 {
820  PointT temp_point;
821 
822  temp_point.x = static_cast<float>(point_x_arg);
823  temp_point.y = static_cast<float>(point_y_arg);
824  temp_point.z = static_cast<float>(point_z_arg);
825 
826  // generate key for point
827  genOctreeKeyforPoint(temp_point, key_arg);
828 }
829 
830 //////////////////////////////////////////////////////////////////////////////////////////////
831 template <typename PointT,
832  typename LeafContainerT,
833  typename BranchContainerT,
834  typename OctreeT>
835 bool
837  genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const
838 {
839  const PointT temp_point = getPointByIndex(data_arg);
840 
841  // generate key for point
842  genOctreeKeyforPoint(temp_point, key_arg);
843 
844  return (true);
845 }
846 
847 //////////////////////////////////////////////////////////////////////////////////////////////
848 template <typename PointT,
849  typename LeafContainerT,
850  typename BranchContainerT,
851  typename OctreeT>
852 void
855 {
856  // define point to leaf node voxel center
857  point.x = static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
858  this->min_x_);
859  point.y = static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
860  this->min_y_);
861  point.z = static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
862  this->min_z_);
863 }
864 
865 //////////////////////////////////////////////////////////////////////////////////////////////
866 template <typename PointT,
867  typename LeafContainerT,
868  typename BranchContainerT,
869  typename OctreeT>
870 void
873  unsigned int tree_depth_arg,
874  PointT& point_arg) const
875 {
876  // generate point for voxel center defined by treedepth (bitLen) and key
877  point_arg.x = static_cast<float>(
878  (static_cast<double>(key_arg.x) + 0.5f) *
879  (this->resolution_ *
880  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
881  this->min_x_);
882  point_arg.y = static_cast<float>(
883  (static_cast<double>(key_arg.y) + 0.5f) *
884  (this->resolution_ *
885  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
886  this->min_y_);
887  point_arg.z = static_cast<float>(
888  (static_cast<double>(key_arg.z) + 0.5f) *
889  (this->resolution_ *
890  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
891  this->min_z_);
892 }
893 
894 //////////////////////////////////////////////////////////////////////////////////////////////
895 template <typename PointT,
896  typename LeafContainerT,
897  typename BranchContainerT,
898  typename OctreeT>
899 void
902  unsigned int tree_depth_arg,
903  Eigen::Vector3f& min_pt,
904  Eigen::Vector3f& max_pt) const
905 {
906  // calculate voxel size of current tree depth
907  double voxel_side_len =
908  this->resolution_ *
909  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
910 
911  // calculate voxel bounds
912  min_pt(0) = static_cast<float>(static_cast<double>(key_arg.x) * voxel_side_len +
913  this->min_x_);
914  min_pt(1) = static_cast<float>(static_cast<double>(key_arg.y) * voxel_side_len +
915  this->min_y_);
916  min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
917  this->min_z_);
918 
919  max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
920  this->min_x_);
921  max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
922  this->min_y_);
923  max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
924  this->min_z_);
925 }
926 
927 //////////////////////////////////////////////////////////////////////////////////////////////
928 template <typename PointT,
929  typename LeafContainerT,
930  typename BranchContainerT,
931  typename OctreeT>
932 double
934  getVoxelSquaredSideLen(unsigned int tree_depth_arg) const
935 {
936  double side_len;
937 
938  // side length of the voxel cube increases exponentially with the octree depth
939  side_len = this->resolution_ *
940  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
941 
942  // squared voxel side length
943  side_len *= side_len;
944 
945  return (side_len);
946 }
947 
948 //////////////////////////////////////////////////////////////////////////////////////////////
949 template <typename PointT,
950  typename LeafContainerT,
951  typename BranchContainerT,
952  typename OctreeT>
953 double
955  getVoxelSquaredDiameter(unsigned int tree_depth_arg) const
956 {
957  // return the squared side length of the voxel cube as a function of the octree depth
958  return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
959 }
960 
961 //////////////////////////////////////////////////////////////////////////////////////////////
962 template <typename PointT,
963  typename LeafContainerT,
964  typename BranchContainerT,
965  typename OctreeT>
966 int
969  const OctreeKey& key_arg,
970  AlignedPointTVector& voxel_center_list_arg) const
971 {
972  int voxel_count = 0;
973 
974  // iterate over all children
975  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
976  if (!this->branchHasChild(*node_arg, child_idx))
977  continue;
978 
979  const OctreeNode* child_node;
980  child_node = this->getBranchChildPtr(*node_arg, child_idx);
981 
982  // generate new key for current branch voxel
983  OctreeKey new_key;
984  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
985  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
986  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
987 
988  switch (child_node->getNodeType()) {
989  case BRANCH_NODE: {
990  // recursively proceed with indexed child branch
991  voxel_count += getOccupiedVoxelCentersRecursive(
992  static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
993  break;
994  }
995  case LEAF_NODE: {
996  PointT new_point;
997 
998  genLeafNodeCenterFromOctreeKey(new_key, new_point);
999  voxel_center_list_arg.push_back(new_point);
1000 
1001  voxel_count++;
1002  break;
1003  }
1004  default:
1005  break;
1006  }
1007  }
1008  return (voxel_count);
1009 }
1010 
1011 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
1012  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1013  T, \
1014  pcl::octree::OctreeContainerPointIndices, \
1015  pcl::octree::OctreeContainerEmpty, \
1016  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
1017  pcl::octree::OctreeContainerEmpty>>;
1018 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
1019  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1020  T, \
1021  pcl::octree::OctreeContainerPointIndices, \
1022  pcl::octree::OctreeContainerEmpty, \
1023  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1024  pcl::octree::OctreeContainerEmpty>>;
1025 
1026 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1027  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1028  T, \
1029  pcl::octree::OctreeContainerPointIndex, \
1030  pcl::octree::OctreeContainerEmpty, \
1031  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1032  pcl::octree::OctreeContainerEmpty>>;
1033 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1034  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1035  T, \
1036  pcl::octree::OctreeContainerPointIndex, \
1037  pcl::octree::OctreeContainerEmpty, \
1038  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1039  pcl::octree::OctreeContainerEmpty>>;
1040 
1041 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1042  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1043  T, \
1044  pcl::octree::OctreeContainerEmpty, \
1045  pcl::octree::OctreeContainerEmpty, \
1046  pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1047  pcl::octree::OctreeContainerEmpty>>;
1048 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1049  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1050  T, \
1051  pcl::octree::OctreeContainerEmpty, \
1052  pcl::octree::OctreeContainerEmpty, \
1053  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1054  pcl::octree::OctreeContainerEmpty>>;
pcl::octree::OctreePointCloud::getVoxelSquaredDiameter
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
Definition: octree_pointcloud.h:374
pcl::octree::OctreePointCloud::deleteVoxelAtPoint
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Definition: octree_pointcloud.hpp:216
common.h
pcl::octree::OctreePointCloud::genOctreeKeyforPoint
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
Definition: octree_pointcloud.hpp:793
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
pcl::octree::OctreePointCloud::getOccupiedVoxelCenters
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
Definition: octree_pointcloud.hpp:253
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud::getKeyBitSize
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Definition: octree_pointcloud.hpp:717
pcl::octree::OctreePointCloud::genOctreeKeyForDataT
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
Definition: octree_pointcloud.hpp:837
pcl::octree::OctreePointCloud::getOccupiedVoxelCentersRecursive
int getOccupiedVoxelCentersRecursive(const BranchNode *node_arg, const OctreeKey &key_arg, AlignedPointTVector &voxel_center_list_arg) const
Recursively search the tree for all leaf nodes and return a vector of voxel centers.
Definition: octree_pointcloud.hpp:968
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:78
pcl::octree::OctreePointCloud::getApproxIntersectedVoxelCentersBySegment
int getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f &origin, const Eigen::Vector3f &end, AlignedPointTVector &voxel_center_list, float precision=0.2)
Get a PointT vector of centers of voxels intersected by a line segment.
Definition: octree_pointcloud.hpp:270
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
Definition: octree_pointcloud.hpp:512
pcl::octree::OctreePointCloud::addPointToCloud
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
Definition: octree_pointcloud.hpp:121
pcl::octree::OctreePointCloud::defineBoundingBox
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
Definition: octree_pointcloud.hpp:333
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::octree::OctreePointCloud::addPointIdx
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:655
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:61
pcl::octree::OctreePointCloud::getPointByIndex
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
Definition: octree_pointcloud.hpp:703
pcl::octree::LEAF_NODE
@ LEAF_NODE
Definition: octree_nodes.h:54
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:83
pcl::octree::BRANCH_NODE
@ BRANCH_NODE
Definition: octree_nodes.h:54
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: octree_pointcloud.h:86
pcl::octree::OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty >
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:182
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:89
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:151
pcl::octree::OctreePointCloud::getVoxelSquaredSideLen
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
Definition: octree_pointcloud.h:390
pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
Definition: octree_pointcloud.hpp:157
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition: octree_pointcloud.hpp:107
pcl::octree::OctreeKey::getChildIdxWithDepthMask
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
pcl::getMinMax3D
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
Definition: common.hpp:242
pcl::octree::OctreePointCloud::genVoxelBoundsFromOctreeKey
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, unsigned int tree_depth_arg, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
Generate bounds of an octree voxel using octree key and tree depth arguments.
Definition: octree_pointcloud.hpp:901
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:854
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:489
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesPtr
shared_ptr< std::vector< int > > IndicesPtr
Definition: octree_pointcloud.h:85
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:107
pcl::octree::OctreePointCloud::expandLeafNode
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, unsigned int depth_mask)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:605
pcl::octree::OctreePointCloud::OctreePointCloud
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
Definition: octree_pointcloud.hpp:53
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:149
pcl::octree::OctreePointCloud::genVoxelCenterFromOctreeKey
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, unsigned int tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
Definition: octree_pointcloud.hpp:872
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:150