Point Cloud Library (PCL)  1.9.1-dev
octree_pointcloud.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_OCTREE_POINTCLOUD_HPP_
40 #define PCL_OCTREE_POINTCLOUD_HPP_
41 
42 #include <cassert>
43 
44 #include <pcl/common/common.h>
45 #include <pcl/octree/impl/octree_base.hpp>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
50  OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()),
51  epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
52  max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
53 {
54  assert (resolution > 0.0f);
55 }
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////
58 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
60 {
61 }
62 
63 //////////////////////////////////////////////////////////////////////////////////////////////
64 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
66 {
67  if (indices_)
68  {
69  for (const int &index : *indices_)
70  {
71  assert( (index >= 0) && (index < static_cast<int> (input_->points.size ())));
72 
73  if (isFinite (input_->points[index]))
74  {
75  // add points to octree
76  this->addPointIdx (index);
77  }
78  }
79  }
80  else
81  {
82  for (size_t i = 0; i < input_->points.size (); i++)
83  {
84  if (isFinite (input_->points[i]))
85  {
86  // add points to octree
87  this->addPointIdx (static_cast<unsigned int> (i));
88  }
89  }
90  }
91 }
92 
93 //////////////////////////////////////////////////////////////////////////////////////////////
94 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
96 {
97  this->addPointIdx (point_idx_arg);
98  if (indices_arg)
99  indices_arg->push_back (point_idx_arg);
100 }
101 
102 //////////////////////////////////////////////////////////////////////////////////////////////
103 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
105 {
106  assert (cloud_arg==input_);
107 
108  cloud_arg->push_back (point_arg);
109 
110  this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
111 }
112 
113 //////////////////////////////////////////////////////////////////////////////////////////////
114 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
116  IndicesPtr indices_arg)
117 {
118  assert (cloud_arg==input_);
119  assert (indices_arg==indices_);
120 
121  cloud_arg->push_back (point_arg);
122 
123  this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////
127 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
129 {
130  if (!isPointWithinBoundingBox (point_arg))
131  {
132  return false;
133  }
134 
135  OctreeKey key;
136 
137  // generate key for point
138  this->genOctreeKeyforPoint (point_arg, key);
139 
140  // search for key in octree
141  return (this->existLeaf (key));
142 }
143 
144 //////////////////////////////////////////////////////////////////////////////////////////////
145 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
147 {
148  // retrieve point from input cloud
149  const PointT& point = this->input_->points[point_idx_arg];
150 
151  // search for voxel at point in octree
152  return (this->isVoxelOccupiedAtPoint (point));
153 }
154 
155 //////////////////////////////////////////////////////////////////////////////////////////////
156 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
158  const double point_x_arg, const double point_y_arg, const double point_z_arg) const
159 {
160  // create a new point with the argument coordinates
161  PointT point;
162  point.x = point_x_arg;
163  point.y = point_y_arg;
164  point.z = point_z_arg;
165 
166  // search for voxel at point in octree
167  return (this->isVoxelOccupiedAtPoint (point));
168 }
169 
170 //////////////////////////////////////////////////////////////////////////////////////////////
171 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
173 {
174  if (!isPointWithinBoundingBox (point_arg))
175  {
176  return;
177  }
178 
179  OctreeKey key;
180 
181  // generate key for point
182  this->genOctreeKeyforPoint (point_arg, key);
183 
184  this->removeLeaf (key);
185 }
186 
187 //////////////////////////////////////////////////////////////////////////////////////////////
188 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
190 {
191  // retrieve point from input cloud
192  const PointT& point = this->input_->points[point_idx_arg];
193 
194  // delete leaf at point
195  this->deleteVoxelAtPoint (point);
196 }
197 
198 //////////////////////////////////////////////////////////////////////////////////////////////
199 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
201  AlignedPointTVector &voxel_center_list_arg) const
202 {
203  OctreeKey key;
204  key.x = key.y = key.z = 0;
205 
206  voxel_center_list_arg.clear ();
207 
208  return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
209 
210 }
211 
212 //////////////////////////////////////////////////////////////////////////////////////////////
213 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
215  const Eigen::Vector3f& origin,
216  const Eigen::Vector3f& end,
217  AlignedPointTVector &voxel_center_list,
218  float precision)
219 {
220  Eigen::Vector3f direction = end - origin;
221  float norm = direction.norm ();
222  direction.normalize ();
223 
224  const float step_size = static_cast<const float> (resolution_) * precision;
225  // Ensure we get at least one step for the first voxel.
226  const int nsteps = std::max (1, static_cast<int> (norm / step_size));
227 
228  OctreeKey prev_key;
229 
230  bool bkeyDefined = false;
231 
232  // Walk along the line segment with small steps.
233  for (int i = 0; i < nsteps; ++i)
234  {
235  Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
236 
237  PointT octree_p;
238  octree_p.x = p.x ();
239  octree_p.y = p.y ();
240  octree_p.z = p.z ();
241 
242  OctreeKey key;
243  this->genOctreeKeyforPoint (octree_p, key);
244 
245  // Not a new key, still the same voxel.
246  if ((key == prev_key) && (bkeyDefined) )
247  continue;
248 
249  prev_key = key;
250  bkeyDefined = true;
251 
252  PointT center;
253  genLeafNodeCenterFromOctreeKey (key, center);
254  voxel_center_list.push_back (center);
255  }
256 
257  OctreeKey end_key;
258  PointT end_p;
259  end_p.x = end.x ();
260  end_p.y = end.y ();
261  end_p.z = end.z ();
262  this->genOctreeKeyforPoint (end_p, end_key);
263  if (!(end_key == prev_key))
264  {
265  PointT center;
266  genLeafNodeCenterFromOctreeKey (end_key, center);
267  voxel_center_list.push_back (center);
268  }
269 
270  return (static_cast<int> (voxel_center_list.size ()));
271 }
272 
273 //////////////////////////////////////////////////////////////////////////////////////////////
274 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
276 {
277 
278  double minX, minY, minZ, maxX, maxY, maxZ;
279 
280  PointT min_pt;
281  PointT max_pt;
282 
283  // bounding box cannot be changed once the octree contains elements
284  assert (this->leaf_count_ == 0);
285 
286  pcl::getMinMax3D (*input_, min_pt, max_pt);
287 
288  float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
289 
290  minX = min_pt.x;
291  minY = min_pt.y;
292  minZ = min_pt.z;
293 
294  maxX = max_pt.x + minValue;
295  maxY = max_pt.y + minValue;
296  maxZ = max_pt.z + minValue;
297 
298  // generate bit masks for octree
299  defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
300 }
301 
302 //////////////////////////////////////////////////////////////////////////////////////////////
303 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
305  const double min_y_arg,
306  const double min_z_arg,
307  const double max_x_arg,
308  const double max_y_arg,
309  const double max_z_arg)
310 {
311  // bounding box cannot be changed once the octree contains elements
312  assert (this->leaf_count_ == 0);
313 
314  assert (max_x_arg >= min_x_arg);
315  assert (max_y_arg >= min_y_arg);
316  assert (max_z_arg >= min_z_arg);
317 
318  min_x_ = min_x_arg;
319  max_x_ = max_x_arg;
320 
321  min_y_ = min_y_arg;
322  max_y_ = max_y_arg;
323 
324  min_z_ = min_z_arg;
325  max_z_ = max_z_arg;
326 
327  min_x_ = std::min (min_x_, max_x_);
328  min_y_ = std::min (min_y_, max_y_);
329  min_z_ = std::min (min_z_, max_z_);
330 
331  max_x_ = std::max (min_x_, max_x_);
332  max_y_ = std::max (min_y_, max_y_);
333  max_z_ = std::max (min_z_, max_z_);
334 
335  // generate bit masks for octree
336  getKeyBitSize ();
337 
338  bounding_box_defined_ = true;
339 }
340 
341 //////////////////////////////////////////////////////////////////////////////////////////////
342 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
344  const double max_x_arg, const double max_y_arg, const double max_z_arg)
345 {
346  // bounding box cannot be changed once the octree contains elements
347  assert (this->leaf_count_ == 0);
348 
349  assert (max_x_arg >= 0.0f);
350  assert (max_y_arg >= 0.0f);
351  assert (max_z_arg >= 0.0f);
352 
353  min_x_ = 0.0f;
354  max_x_ = max_x_arg;
355 
356  min_y_ = 0.0f;
357  max_y_ = max_y_arg;
358 
359  min_z_ = 0.0f;
360  max_z_ = max_z_arg;
361 
362  min_x_ = std::min (min_x_, max_x_);
363  min_y_ = std::min (min_y_, max_y_);
364  min_z_ = std::min (min_z_, max_z_);
365 
366  max_x_ = std::max (min_x_, max_x_);
367  max_y_ = std::max (min_y_, max_y_);
368  max_z_ = std::max (min_z_, max_z_);
369 
370  // generate bit masks for octree
371  getKeyBitSize ();
372 
373  bounding_box_defined_ = true;
374 }
375 
376 //////////////////////////////////////////////////////////////////////////////////////////////
377 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
379 {
380  // bounding box cannot be changed once the octree contains elements
381  assert (this->leaf_count_ == 0);
382 
383  assert (cubeLen_arg >= 0.0f);
384 
385  min_x_ = 0.0f;
386  max_x_ = cubeLen_arg;
387 
388  min_y_ = 0.0f;
389  max_y_ = cubeLen_arg;
390 
391  min_z_ = 0.0f;
392  max_z_ = cubeLen_arg;
393 
394  min_x_ = std::min (min_x_, max_x_);
395  min_y_ = std::min (min_y_, max_y_);
396  min_z_ = std::min (min_z_, max_z_);
397 
398  max_x_ = std::max (min_x_, max_x_);
399  max_y_ = std::max (min_y_, max_y_);
400  max_z_ = std::max (min_z_, max_z_);
401 
402  // generate bit masks for octree
403  getKeyBitSize ();
404 
405  bounding_box_defined_ = true;
406 }
407 
408 //////////////////////////////////////////////////////////////////////////////////////////////
409 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
411  double& min_x_arg, double& min_y_arg, double& min_z_arg,
412  double& max_x_arg, double& max_y_arg, double& max_z_arg) const
413 {
414  min_x_arg = min_x_;
415  min_y_arg = min_y_;
416  min_z_arg = min_z_;
417 
418  max_x_arg = max_x_;
419  max_y_arg = max_y_;
420  max_z_arg = max_z_;
421 }
422 
423 
424 //////////////////////////////////////////////////////////////////////////////////////////////
425 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
426 void
428 {
429 
430  const float minValue = std::numeric_limits<float>::epsilon ();
431 
432  // increase octree size until point fits into bounding box
433  while (true)
434  {
435  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
436  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
437  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
438 
439  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
440  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
441  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
442 
443  // do we violate any bounds?
444  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
445  || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
446  {
447 
449  {
450 
451  double octreeSideLen;
452  unsigned char child_idx;
453 
454  // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2
455  child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
456  | ((!bUpperBoundViolationZ)));
457 
458  BranchNode* newRootBranch;
459 
460  newRootBranch = new BranchNode();
461  this->branch_count_++;
462 
463  this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
464 
465  this->root_node_ = newRootBranch;
466 
467  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
468 
469  if (!bUpperBoundViolationX)
470  min_x_ -= octreeSideLen;
471 
472  if (!bUpperBoundViolationY)
473  min_y_ -= octreeSideLen;
474 
475  if (!bUpperBoundViolationZ)
476  min_z_ -= octreeSideLen;
477 
478  // configure tree depth of octree
479  this->octree_depth_++;
480  this->setTreeDepth (this->octree_depth_);
481 
482  // recalculate bounding box width
483  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
484 
485  // increase octree bounding box
486  max_x_ = min_x_ + octreeSideLen;
487  max_y_ = min_y_ + octreeSideLen;
488  max_z_ = min_z_ + octreeSideLen;
489 
490  }
491  // bounding box is not defined - set it to point position
492  else
493  {
494  // octree is empty - we set the center of the bounding box to our first pixel
495  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
496  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
497  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
498 
499  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
500  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
501  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
502 
503  getKeyBitSize ();
504 
505  bounding_box_defined_ = true;
506  }
507 
508  }
509  else
510  // no bound violations anymore - leave while loop
511  break;
512  }
513 }
514 
515 //////////////////////////////////////////////////////////////////////////////////////////////
516 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
517 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask)
518 {
519 
520  if (depth_mask)
521  {
522  // get amount of objects in leaf container
523  size_t leaf_obj_count = (*leaf_node)->getSize ();
524 
525  // copy leaf data
526  std::vector<int> leafIndices;
527  leafIndices.reserve(leaf_obj_count);
528 
529  (*leaf_node)->getPointIndices(leafIndices);
530 
531  // delete current leaf node
532  this->deleteBranchChild(*parent_branch, child_idx);
533  this->leaf_count_ --;
534 
535  // create new branch node
536  BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
537  this->branch_count_ ++;
538 
539  // add data to new branch
540  OctreeKey new_index_key;
541 
542  for (const int &leafIndex : leafIndices)
543  {
544 
545  const PointT& point_from_index = input_->points[leafIndex];
546  // generate key
547  genOctreeKeyforPoint (point_from_index, new_index_key);
548 
549  LeafNode* newLeaf;
550  BranchNode* newBranchParent;
551  this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
552 
553  (*newLeaf)->addPointIndex(leafIndex);
554  }
555  }
556 
557 
558 }
559 
560 
561 //////////////////////////////////////////////////////////////////////////////////////////////
562 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
564 {
565  OctreeKey key;
566 
567  assert (point_idx_arg < static_cast<int> (input_->points.size ()));
568 
569  const PointT& point = input_->points[point_idx_arg];
570 
571  // make sure bounding box is big enough
572  adoptBoundingBoxToPoint (point);
573 
574  // generate key
575  genOctreeKeyforPoint (point, key);
576 
577  LeafNode* leaf_node;
578  BranchNode* parent_branch_of_leaf_node;
579  unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
580 
581  if (this->dynamic_depth_enabled_ && depth_mask)
582  {
583  // get amount of objects in leaf container
584  size_t leaf_obj_count = (*leaf_node)->getSize ();
585 
586  while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
587  {
588  // index to branch child
589  unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2);
590 
591  expandLeafNode (leaf_node,
592  parent_branch_of_leaf_node,
593  child_idx,
594  depth_mask);
595 
596  depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
597  leaf_obj_count = (*leaf_node)->getSize ();
598  }
599 
600  }
601 
602  (*leaf_node)->addPointIndex (point_idx_arg);
603 }
604 
605 //////////////////////////////////////////////////////////////////////////////////////////////
606 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> const PointT&
608 {
609  // retrieve point from input cloud
610  assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
611  return (this->input_->points[index_arg]);
612 }
613 
614 //////////////////////////////////////////////////////////////////////////////////////////////
615 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
617 {
618  unsigned int max_voxels;
619 
620  unsigned int max_key_x;
621  unsigned int max_key_y;
622  unsigned int max_key_z;
623 
624  double octree_side_len;
625 
626  const float minValue = std::numeric_limits<float>::epsilon();
627 
628  // find maximum key values for x, y, z
629  max_key_x = static_cast<unsigned int> (ceil ((max_x_ - min_x_ - minValue) / resolution_));
630  max_key_y = static_cast<unsigned int> (ceil ((max_y_ - min_y_ - minValue) / resolution_));
631  max_key_z = static_cast<unsigned int> (ceil ((max_z_ - min_z_ - minValue) / resolution_));
632 
633  // find maximum amount of keys
634  max_voxels = std::max (std::max (std::max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
635 
636 
637  // tree depth == amount of bits of max_voxels
638  this->octree_depth_ = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (std::log2 (max_voxels) - minValue)))),
639  static_cast<unsigned int> (0));
640 
641  octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_;
642 
643  if (this->leaf_count_ == 0)
644  {
645  double octree_oversize_x;
646  double octree_oversize_y;
647  double octree_oversize_z;
648 
649  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
650  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
651  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
652 
653  assert (octree_oversize_x > -minValue);
654  assert (octree_oversize_y > -minValue);
655  assert (octree_oversize_z > -minValue);
656 
657  if (octree_oversize_x > minValue)
658  {
659  min_x_ -= octree_oversize_x;
660  max_x_ += octree_oversize_x;
661  }
662  if (octree_oversize_y > minValue)
663  {
664  min_y_ -= octree_oversize_y;
665  max_y_ += octree_oversize_y;
666  }
667  if (octree_oversize_z > minValue)
668  {
669  min_z_ -= octree_oversize_z;
670  max_z_ += octree_oversize_z;
671  }
672  }
673  else
674  {
675  max_x_ = min_x_ + octree_side_len;
676  max_y_ = min_y_ + octree_side_len;
677  max_z_ = min_z_ + octree_side_len;
678  }
679 
680  // configure tree depth of octree
681  this->setTreeDepth (this->octree_depth_);
682 
683 }
684 
685 //////////////////////////////////////////////////////////////////////////////////////////////
686 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
688  OctreeKey & key_arg) const
689  {
690  // calculate integer key for point coordinates
691  key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
692  key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
693  key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
694 
695  assert (key_arg.x <= this->max_key_.x);
696  assert (key_arg.y <= this->max_key_.y);
697  assert (key_arg.z <= this->max_key_.z);
698  }
699 
700 //////////////////////////////////////////////////////////////////////////////////////////////
701 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
703  const double point_x_arg, const double point_y_arg,
704  const double point_z_arg, OctreeKey & key_arg) const
705 {
706  PointT temp_point;
707 
708  temp_point.x = static_cast<float> (point_x_arg);
709  temp_point.y = static_cast<float> (point_y_arg);
710  temp_point.z = static_cast<float> (point_z_arg);
711 
712  // generate key for point
713  genOctreeKeyforPoint (temp_point, key_arg);
714 }
715 
716 //////////////////////////////////////////////////////////////////////////////////////////////
717 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
719 {
720  const PointT temp_point = getPointByIndex (data_arg);
721 
722  // generate key for point
723  genOctreeKeyforPoint (temp_point, key_arg);
724 
725  return (true);
726 }
727 
728 //////////////////////////////////////////////////////////////////////////////////////////////
729 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
731 {
732  // define point to leaf node voxel center
733  point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_);
734  point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_);
735  point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_);
736 }
737 
738 //////////////////////////////////////////////////////////////////////////////////////////////
739 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
741  const OctreeKey & key_arg,
742  unsigned int tree_depth_arg,
743  PointT& point_arg) const
744 {
745  // generate point for voxel center defined by treedepth (bitLen) and key
746  point_arg.x = static_cast<float> ((static_cast <double> (key_arg.x) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_x_);
747  point_arg.y = static_cast<float> ((static_cast <double> (key_arg.y) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_y_);
748  point_arg.z = static_cast<float> ((static_cast <double> (key_arg.z) + 0.5f) * (this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg))) + this->min_z_);
749 }
750 
751 //////////////////////////////////////////////////////////////////////////////////////////////
752 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
754  const OctreeKey & key_arg,
755  unsigned int tree_depth_arg,
756  Eigen::Vector3f &min_pt,
757  Eigen::Vector3f &max_pt) const
758 {
759  // calculate voxel size of current tree depth
760  double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
761 
762  // calculate voxel bounds
763  min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->min_x_);
764  min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->min_y_);
765  min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->min_z_);
766 
767  max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->min_x_);
768  max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->min_y_);
769  max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->min_z_);
770 }
771 
772 //////////////////////////////////////////////////////////////////////////////////////////////
773 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
775 {
776  double side_len;
777 
778  // side length of the voxel cube increases exponentially with the octree depth
779  side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
780 
781  // squared voxel side length
782  side_len *= side_len;
783 
784  return (side_len);
785 }
786 
787 //////////////////////////////////////////////////////////////////////////////////////////////
788 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
790 {
791  // return the squared side length of the voxel cube as a function of the octree depth
792  return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
793 }
794 
795 //////////////////////////////////////////////////////////////////////////////////////////////
796 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
798  const BranchNode* node_arg,
799  const OctreeKey& key_arg,
800  AlignedPointTVector &voxel_center_list_arg) const
801 {
802  int voxel_count = 0;
803 
804  // iterate over all children
805  for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
806  {
807  if (!this->branchHasChild (*node_arg, child_idx))
808  continue;
809 
810  const OctreeNode * child_node;
811  child_node = this->getBranchChildPtr (*node_arg, child_idx);
812 
813  // generate new key for current branch voxel
814  OctreeKey new_key;
815  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
816  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
817  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
818 
819  switch (child_node->getNodeType ())
820  {
821  case BRANCH_NODE:
822  {
823  // recursively proceed with indexed child branch
824  voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
825  break;
826  }
827  case LEAF_NODE:
828  {
829  PointT new_point;
830 
831  genLeafNodeCenterFromOctreeKey (new_key, new_point);
832  voxel_center_list_arg.push_back (new_point);
833 
834  voxel_count++;
835  break;
836  }
837  default:
838  break;
839  }
840  }
841  return (voxel_count);
842 }
843 
844 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
845 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > >;
846 
847 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
848 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, pcl::octree::OctreeContainerEmpty > >;
849 
850 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
851 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeContainerEmpty > >;
852 
853 #endif /* OCTREE_POINTCLOUD_HPP_ */
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:53
void addPointsFromInputCloud()
Add points from input point cloud to octree.
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.
~OctreePointCloud()
Empty deconstructor.
static const unsigned char maxDepth
Definition: octree_key.h:142
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
boost::shared_ptr< PointCloud > PointCloudPtr
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr input_
Pointer to input point cloud dataset.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
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.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
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.
bool isPointWithinBoundingBox(const PointT &point_idx_arg) const
Checks if given point is within the bounding box of the octree.
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
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
std::size_t max_objs_per_leaf_
Amount of DataT objects per leafNode before expanding branch.
Octree key class
Definition: octree_key.h:50
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
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.
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.
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.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
boost::shared_ptr< std::vector< int > > IndicesPtr
OctreeT::BranchNode BranchNode
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
bool bounding_box_defined_
Flag indicating if octree has defined bounding box.
Abstract octree node class
Definition: octree_nodes.h:67
double resolution_
Octree resolution.