Point Cloud Library (PCL)  1.7.0
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 <vector>
43 #include <assert.h>
44 
45 #include <pcl/common/common.h>
46 
47 using namespace std;
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
52  OctreeT (), input_ (PointCloudConstPtr ()), indices_ (IndicesConstPtr ()),
53  epsilon_ (0), resolution_ (resolution), min_x_ (0.0f), max_x_ (resolution), min_y_ (0.0f),
54  max_y_ (resolution), min_z_ (0.0f), max_z_ (resolution), bounding_box_defined_ (false), max_objs_per_leaf_(0)
55 {
56  assert (resolution > 0.0f);
57 }
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
62 {
63 }
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
68 {
69  size_t i;
70 
71  if (indices_)
72  {
73  for (std::vector<int>::const_iterator current = indices_->begin (); current != indices_->end (); ++current)
74  {
75  assert( (*current>=0) && (*current < static_cast<int> (input_->points.size ())));
76 
77  if (isFinite (input_->points[*current]))
78  {
79  // add points to octree
80  this->addPointIdx (*current);
81  }
82  }
83  }
84  else
85  {
86  for (i = 0; i < input_->points.size (); i++)
87  {
88  if (isFinite (input_->points[i]))
89  {
90  // add points to octree
91  this->addPointIdx (static_cast<unsigned int> (i));
92  }
93  }
94  }
95 }
96 
97 //////////////////////////////////////////////////////////////////////////////////////////////
98 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
100 {
101  this->addPointIdx (point_idx_arg);
102  if (indices_arg)
103  indices_arg->push_back (point_idx_arg);
104 }
105 
106 //////////////////////////////////////////////////////////////////////////////////////////////
107 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
109 {
110  assert (cloud_arg==input_);
111 
112  cloud_arg->push_back (point_arg);
113 
114  this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
115 }
116 
117 //////////////////////////////////////////////////////////////////////////////////////////////
118 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
120  IndicesPtr indices_arg)
121 {
122  assert (cloud_arg==input_);
123  assert (indices_arg==indices_);
124 
125  cloud_arg->push_back (point_arg);
126 
127  this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
128 }
129 
130 //////////////////////////////////////////////////////////////////////////////////////////////
131 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
133 {
134  OctreeKey key;
135 
136  // generate key for point
137  this->genOctreeKeyforPoint (point_arg, key);
138 
139  // search for key in octree
140  return (isPointWithinBoundingBox (point_arg) && this->existLeaf (key));
141 }
142 
143 //////////////////////////////////////////////////////////////////////////////////////////////
144 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
146 {
147  // retrieve point from input cloud
148  const PointT& point = this->input_->points[point_idx_arg];
149 
150  // search for voxel at point in octree
151  return (this->isVoxelOccupiedAtPoint (point));
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////
155 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
157  const double point_x_arg, const double point_y_arg, const double point_z_arg) const
158 {
159  OctreeKey key;
160 
161  // generate key for point
162  this->genOctreeKeyforPoint (point_x_arg, point_y_arg, point_z_arg, key);
163 
164  return (this->existLeaf (key));
165 }
166 
167 //////////////////////////////////////////////////////////////////////////////////////////////
168 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
170 {
171  OctreeKey key;
172 
173  // generate key for point
174  this->genOctreeKeyforPoint (point_arg, key);
175 
176  this->removeLeaf (key);
177 }
178 
179 //////////////////////////////////////////////////////////////////////////////////////////////
180 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
182 {
183  // retrieve point from input cloud
184  const PointT& point = this->input_->points[point_idx_arg];
185 
186  // delete leaf at point
187  this->deleteVoxelAtPoint (point);
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////////////////////
191 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
193  AlignedPointTVector &voxel_center_list_arg) const
194 {
195  OctreeKey key;
196  key.x = key.y = key.z = 0;
197 
198  voxel_center_list_arg.clear ();
199 
200  return getOccupiedVoxelCentersRecursive (this->root_node_, key, voxel_center_list_arg);
201 
202 }
203 
204 //////////////////////////////////////////////////////////////////////////////////////////////
205 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
207  const Eigen::Vector3f& origin,
208  const Eigen::Vector3f& end,
209  AlignedPointTVector &voxel_center_list,
210  float precision)
211 {
212  Eigen::Vector3f direction = end - origin;
213  float norm = direction.norm ();
214  direction.normalize ();
215 
216  const float step_size = static_cast<const float> (resolution_) * precision;
217  // Ensure we get at least one step for the first voxel.
218  const int nsteps = std::max (1, static_cast<int> (norm / step_size));
219 
220  OctreeKey prev_key;
221 
222  bool bkeyDefined = false;
223 
224  // Walk along the line segment with small steps.
225  for (int i = 0; i < nsteps; ++i)
226  {
227  Eigen::Vector3f p = origin + (direction * step_size * static_cast<const float> (i));
228 
229  PointT octree_p;
230  octree_p.x = p.x ();
231  octree_p.y = p.y ();
232  octree_p.z = p.z ();
233 
234  OctreeKey key;
235  this->genOctreeKeyforPoint (octree_p, key);
236 
237  // Not a new key, still the same voxel.
238  if ((key == prev_key) && (bkeyDefined) )
239  continue;
240 
241  prev_key = key;
242  bkeyDefined = true;
243 
244  PointT center;
245  genLeafNodeCenterFromOctreeKey (key, center);
246  voxel_center_list.push_back (center);
247  }
248 
249  OctreeKey end_key;
250  PointT end_p;
251  end_p.x = end.x ();
252  end_p.y = end.y ();
253  end_p.z = end.z ();
254  this->genOctreeKeyforPoint (end_p, end_key);
255  if (!(end_key == prev_key))
256  {
257  PointT center;
258  genLeafNodeCenterFromOctreeKey (end_key, center);
259  voxel_center_list.push_back (center);
260  }
261 
262  return (static_cast<int> (voxel_center_list.size ()));
263 }
264 
265 //////////////////////////////////////////////////////////////////////////////////////////////
266 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
268 {
269 
270  double minX, minY, minZ, maxX, maxY, maxZ;
271 
272  PointT min_pt;
273  PointT max_pt;
274 
275  // bounding box cannot be changed once the octree contains elements
276  assert (this->leaf_count_ == 0);
277 
278  pcl::getMinMax3D (*input_, min_pt, max_pt);
279 
280  float minValue = std::numeric_limits<float>::epsilon () * 512.0f;
281 
282  minX = min_pt.x;
283  minY = min_pt.y;
284  minZ = min_pt.z;
285 
286  maxX = max_pt.x + minValue;
287  maxY = max_pt.y + minValue;
288  maxZ = max_pt.z + minValue;
289 
290  // generate bit masks for octree
291  defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);
292 }
293 
294 //////////////////////////////////////////////////////////////////////////////////////////////
295 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
297  const double min_y_arg,
298  const double min_z_arg,
299  const double max_x_arg,
300  const double max_y_arg,
301  const double max_z_arg)
302 {
303  // bounding box cannot be changed once the octree contains elements
304  assert (this->leaf_count_ == 0);
305 
306  assert (max_x_arg >= min_x_arg);
307  assert (max_y_arg >= min_y_arg);
308  assert (max_z_arg >= min_z_arg);
309 
310  min_x_ = min_x_arg;
311  max_x_ = max_x_arg;
312 
313  min_y_ = min_y_arg;
314  max_y_ = max_y_arg;
315 
316  min_z_ = min_z_arg;
317  max_z_ = max_z_arg;
318 
319  min_x_ = min (min_x_, max_x_);
320  min_y_ = min (min_y_, max_y_);
321  min_z_ = min (min_z_, max_z_);
322 
323  max_x_ = max (min_x_, max_x_);
324  max_y_ = max (min_y_, max_y_);
325  max_z_ = max (min_z_, max_z_);
326 
327  // generate bit masks for octree
328  getKeyBitSize ();
329 
330  bounding_box_defined_ = true;
331 }
332 
333 //////////////////////////////////////////////////////////////////////////////////////////////
334 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
336  const double max_x_arg, const double max_y_arg, const double max_z_arg)
337 {
338  // bounding box cannot be changed once the octree contains elements
339  assert (this->leaf_count_ == 0);
340 
341  assert (max_x_arg >= 0.0f);
342  assert (max_y_arg >= 0.0f);
343  assert (max_z_arg >= 0.0f);
344 
345  min_x_ = 0.0f;
346  max_x_ = max_x_arg;
347 
348  min_y_ = 0.0f;
349  max_y_ = max_y_arg;
350 
351  min_z_ = 0.0f;
352  max_z_ = max_z_arg;
353 
354  min_x_ = min (min_x_, max_x_);
355  min_y_ = min (min_y_, max_y_);
356  min_z_ = min (min_z_, max_z_);
357 
358  max_x_ = max (min_x_, max_x_);
359  max_y_ = max (min_y_, max_y_);
360  max_z_ = max (min_z_, max_z_);
361 
362  // generate bit masks for octree
363  getKeyBitSize ();
364 
365  bounding_box_defined_ = true;
366 }
367 
368 //////////////////////////////////////////////////////////////////////////////////////////////
369 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
371 {
372  // bounding box cannot be changed once the octree contains elements
373  assert (this->leaf_count_ == 0);
374 
375  assert (cubeLen_arg >= 0.0f);
376 
377  min_x_ = 0.0f;
378  max_x_ = cubeLen_arg;
379 
380  min_y_ = 0.0f;
381  max_y_ = cubeLen_arg;
382 
383  min_z_ = 0.0f;
384  max_z_ = cubeLen_arg;
385 
386  min_x_ = min (min_x_, max_x_);
387  min_y_ = min (min_y_, max_y_);
388  min_z_ = min (min_z_, max_z_);
389 
390  max_x_ = max (min_x_, max_x_);
391  max_y_ = max (min_y_, max_y_);
392  max_z_ = max (min_z_, max_z_);
393 
394  // generate bit masks for octree
395  getKeyBitSize ();
396 
397  bounding_box_defined_ = true;
398 }
399 
400 //////////////////////////////////////////////////////////////////////////////////////////////
401 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
403  double& min_x_arg, double& min_y_arg, double& min_z_arg,
404  double& max_x_arg, double& max_y_arg, double& max_z_arg) const
405 {
406  min_x_arg = min_x_;
407  min_y_arg = min_y_;
408  min_z_arg = min_z_;
409 
410  max_x_arg = max_x_;
411  max_y_arg = max_y_;
412  max_z_arg = max_z_;
413 }
414 
415 
416 //////////////////////////////////////////////////////////////////////////////////////////////
417 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT>
418 void
420 {
421 
422  const float minValue = std::numeric_limits<float>::epsilon ();
423 
424  // increase octree size until point fits into bounding box
425  while (true)
426  {
427  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
428  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
429  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
430 
431  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
432  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
433  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
434 
435  // do we violate any bounds?
436  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ || bUpperBoundViolationX
437  || bUpperBoundViolationY || bUpperBoundViolationZ || (!bounding_box_defined_) )
438  {
439 
440  if (bounding_box_defined_)
441  {
442 
443  double octreeSideLen;
444  unsigned char child_idx;
445 
446  // octree not empty - we add another tree level and thus increase its size by a factor of 2*2*2
447  child_idx = static_cast<unsigned char> (((!bUpperBoundViolationX) << 2) | ((!bUpperBoundViolationY) << 1)
448  | ((!bUpperBoundViolationZ)));
449 
450  BranchNode* newRootBranch;
451 
452  newRootBranch = new BranchNode();
453  this->branch_count_++;
454 
455  this->setBranchChildPtr (*newRootBranch, child_idx, this->root_node_);
456 
457  this->root_node_ = newRootBranch;
458 
459  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_;
460 
461  if (!bUpperBoundViolationX)
462  min_x_ -= octreeSideLen;
463 
464  if (!bUpperBoundViolationY)
465  min_y_ -= octreeSideLen;
466 
467  if (!bUpperBoundViolationZ)
468  min_z_ -= octreeSideLen;
469 
470  // configure tree depth of octree
471  this->octree_depth_++;
472  this->setTreeDepth (this->octree_depth_);
473 
474  // recalculate bounding box width
475  octreeSideLen = static_cast<double> (1 << this->octree_depth_) * resolution_ - minValue;
476 
477  // increase octree bounding box
478  max_x_ = min_x_ + octreeSideLen;
479  max_y_ = min_y_ + octreeSideLen;
480  max_z_ = min_z_ + octreeSideLen;
481 
482  }
483  // bounding box is not defined - set it to point position
484  else
485  {
486  // octree is empty - we set the center of the bounding box to our first pixel
487  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
488  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
489  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
490 
491  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
492  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
493  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
494 
495  getKeyBitSize ();
496 
497  bounding_box_defined_ = true;
498  }
499 
500  }
501  else
502  // no bound violations anymore - leave while loop
503  break;
504  }
505 }
506 
507 //////////////////////////////////////////////////////////////////////////////////////////////
508 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
509 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::expandLeafNode (LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, unsigned int depth_mask)
510 {
511 
512  if (depth_mask)
513  {
514  // get amount of objects in leaf container
515  size_t leaf_obj_count = (*leaf_node)->getSize ();
516 
517  // copy leaf data
518  std::vector<int> leafIndices;
519  leafIndices.reserve(leaf_obj_count);
520 
521  (*leaf_node)->getPointIndices(leafIndices);
522 
523  // delete current leaf node
524  this->deleteBranchChild(*parent_branch, child_idx);
525  this->leaf_count_ --;
526 
527  // create new branch node
528  BranchNode* childBranch = this->createBranchChild (*parent_branch, child_idx);
529  this->branch_count_ ++;
530 
531  typename std::vector<int>::iterator it = leafIndices.begin();
532  typename std::vector<int>::const_iterator it_end = leafIndices.end();
533 
534  // add data to new branch
535  OctreeKey new_index_key;
536 
537  for (it = leafIndices.begin(); it!=it_end; ++it)
538  {
539 
540  const PointT& point_from_index = input_->points[*it];
541  // generate key
542  genOctreeKeyforPoint (point_from_index, new_index_key);
543 
544  LeafNode* newLeaf;
545  BranchNode* newBranchParent;
546  this->createLeafRecursive (new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
547 
548  (*newLeaf)->addPointIndex(*it);
549  }
550  }
551 
552 
553 }
554 
555 
556 //////////////////////////////////////////////////////////////////////////////////////////////
557 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
559 {
560  OctreeKey key;
561 
562  assert (point_idx_arg < static_cast<int> (input_->points.size ()));
563 
564  const PointT& point = input_->points[point_idx_arg];
565 
566  // make sure bounding box is big enough
567  adoptBoundingBoxToPoint (point);
568 
569  // generate key
570  genOctreeKeyforPoint (point, key);
571 
572  LeafNode* leaf_node;
573  BranchNode* parent_branch_of_leaf_node;
574  unsigned int depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
575 
576  if (this->dynamic_depth_enabled_ && depth_mask)
577  {
578  // get amount of objects in leaf container
579  size_t leaf_obj_count = (*leaf_node)->getSize ();
580 
581  while (leaf_obj_count>=max_objs_per_leaf_ && depth_mask)
582  {
583  // index to branch child
584  unsigned char child_idx = key.getChildIdxWithDepthMask (depth_mask*2);
585 
586  expandLeafNode (leaf_node,
587  parent_branch_of_leaf_node,
588  child_idx,
589  depth_mask);
590 
591  depth_mask = this->createLeafRecursive (key, this->depth_mask_ ,this->root_node_, leaf_node, parent_branch_of_leaf_node);
592  leaf_obj_count = (*leaf_node)->getSize ();
593  }
594 
595  }
596 
597  (*leaf_node)->addPointIndex (point_idx_arg);
598 }
599 
600 //////////////////////////////////////////////////////////////////////////////////////////////
601 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> const PointT&
603 {
604  // retrieve point from input cloud
605  assert (index_arg < static_cast<unsigned int> (input_->points.size ()));
606  return (this->input_->points[index_arg]);
607 }
608 
609 //////////////////////////////////////////////////////////////////////////////////////////////
610 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
612 {
613  unsigned int max_voxels;
614 
615  unsigned int max_key_x;
616  unsigned int max_key_y;
617  unsigned int max_key_z;
618 
619  double octree_side_len;
620 
621  const float minValue = std::numeric_limits<float>::epsilon();
622 
623  // find maximum key values for x, y, z
624  max_key_x = static_cast<unsigned int> ((max_x_ - min_x_) / resolution_);
625  max_key_y = static_cast<unsigned int> ((max_y_ - min_y_) / resolution_);
626  max_key_z = static_cast<unsigned int> ((max_z_ - min_z_) / resolution_);
627 
628  // find maximum amount of keys
629  max_voxels = max (max (max (max_key_x, max_key_y), max_key_z), static_cast<unsigned int> (2));
630 
631 
632  // tree depth == amount of bits of max_voxels
633  this->octree_depth_ = max ((min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (ceil (this->Log2 (max_voxels)-minValue)))),
634  static_cast<unsigned int> (0));
635 
636  octree_side_len = static_cast<double> (1 << this->octree_depth_) * resolution_-minValue;
637 
638  if (this->leaf_count_ == 0)
639  {
640  double octree_oversize_x;
641  double octree_oversize_y;
642  double octree_oversize_z;
643 
644  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
645  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
646  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
647 
648  min_x_ -= octree_oversize_x;
649  min_y_ -= octree_oversize_y;
650  min_z_ -= octree_oversize_z;
651 
652  max_x_ += octree_oversize_x;
653  max_y_ += octree_oversize_y;
654  max_z_ += octree_oversize_z;
655  }
656  else
657  {
658  max_x_ = min_x_ + octree_side_len;
659  max_y_ = min_y_ + octree_side_len;
660  max_z_ = min_z_ + octree_side_len;
661  }
662 
663  // configure tree depth of octree
664  this->setTreeDepth (this->octree_depth_);
665 
666 }
667 
668 //////////////////////////////////////////////////////////////////////////////////////////////
669 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
671  OctreeKey & key_arg) const
672  {
673  // calculate integer key for point coordinates
674  key_arg.x = static_cast<unsigned int> ((point_arg.x - this->min_x_) / this->resolution_);
675  key_arg.y = static_cast<unsigned int> ((point_arg.y - this->min_y_) / this->resolution_);
676  key_arg.z = static_cast<unsigned int> ((point_arg.z - this->min_z_) / this->resolution_);
677  }
678 
679 //////////////////////////////////////////////////////////////////////////////////////////////
680 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
682  const double point_x_arg, const double point_y_arg,
683  const double point_z_arg, OctreeKey & key_arg) const
684 {
685  PointT temp_point;
686 
687  temp_point.x = static_cast<float> (point_x_arg);
688  temp_point.y = static_cast<float> (point_y_arg);
689  temp_point.z = static_cast<float> (point_z_arg);
690 
691  // generate key for point
692  genOctreeKeyforPoint (temp_point, key_arg);
693 }
694 
695 //////////////////////////////////////////////////////////////////////////////////////////////
696 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> bool
698 {
699  const PointT temp_point = getPointByIndex (data_arg);
700 
701  // generate key for point
702  genOctreeKeyforPoint (temp_point, key_arg);
703 
704  return (true);
705 }
706 
707 //////////////////////////////////////////////////////////////////////////////////////////////
708 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
710 {
711  // define point to leaf node voxel center
712  point.x = static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_);
713  point.y = static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_);
714  point.z = static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_);
715 }
716 
717 //////////////////////////////////////////////////////////////////////////////////////////////
718 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
720  const OctreeKey & key_arg,
721  unsigned int tree_depth_arg,
722  PointT& point_arg) const
723 {
724  // generate point for voxel center defined by treedepth (bitLen) and key
725  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_);
726  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_);
727  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_);
728 }
729 
730 //////////////////////////////////////////////////////////////////////////////////////////////
731 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
733  const OctreeKey & key_arg,
734  unsigned int tree_depth_arg,
735  Eigen::Vector3f &min_pt,
736  Eigen::Vector3f &max_pt) const
737 {
738  // calculate voxel size of current tree depth
739  double voxel_side_len = this->resolution_ * static_cast<double> (1 << (this->octree_depth_ - tree_depth_arg));
740 
741  // calculate voxel bounds
742  min_pt (0) = static_cast<float> (static_cast<double> (key_arg.x) * voxel_side_len + this->min_x_);
743  min_pt (1) = static_cast<float> (static_cast<double> (key_arg.y) * voxel_side_len + this->min_y_);
744  min_pt (2) = static_cast<float> (static_cast<double> (key_arg.z) * voxel_side_len + this->min_z_);
745 
746  max_pt (0) = static_cast<float> (static_cast<double> (key_arg.x + 1) * voxel_side_len + this->min_x_);
747  max_pt (1) = static_cast<float> (static_cast<double> (key_arg.y + 1) * voxel_side_len + this->min_y_);
748  max_pt (2) = static_cast<float> (static_cast<double> (key_arg.z + 1) * voxel_side_len + this->min_z_);
749 }
750 
751 //////////////////////////////////////////////////////////////////////////////////////////////
752 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
754 {
755  double side_len;
756 
757  // side length of the voxel cube increases exponentially with the octree depth
758  side_len = this->resolution_ * static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
759 
760  // squared voxel side length
761  side_len *= side_len;
762 
763  return (side_len);
764 }
765 
766 //////////////////////////////////////////////////////////////////////////////////////////////
767 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> double
769 {
770  // return the squared side length of the voxel cube as a function of the octree depth
771  return (getVoxelSquaredSideLen (tree_depth_arg) * 3);
772 }
773 
774 //////////////////////////////////////////////////////////////////////////////////////////////
775 template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> int
777  const BranchNode* node_arg,
778  const OctreeKey& key_arg,
779  AlignedPointTVector &voxel_center_list_arg) const
780 {
781  // child iterator
782  unsigned char child_idx;
783 
784  int voxel_count = 0;
785 
786  // iterate over all children
787  for (child_idx = 0; child_idx < 8; child_idx++)
788  {
789  if (!this->branchHasChild (*node_arg, child_idx))
790  continue;
791 
792  const OctreeNode * child_node;
793  child_node = this->getBranchChildPtr (*node_arg, child_idx);
794 
795  // generate new key for current branch voxel
796  OctreeKey new_key;
797  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
798  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
799  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
800 
801  switch (child_node->getNodeType ())
802  {
803  case BRANCH_NODE:
804  {
805  // recursively proceed with indexed child branch
806  voxel_count += getOccupiedVoxelCentersRecursive (static_cast<const BranchNode*> (child_node), new_key, voxel_center_list_arg);
807  break;
808  }
809  case LEAF_NODE:
810  {
811  PointT new_point;
812 
813  genLeafNodeCenterFromOctreeKey (new_key, new_point);
814  voxel_center_list_arg.push_back (new_point);
815 
816  voxel_count++;
817  break;
818  }
819  default:
820  break;
821  }
822  }
823  return (voxel_count);
824 }
825 
826 #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 > >;
827 #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 > >;
828 
829 #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 > >;
830 #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 > >;
831 
832 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
833 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) template class PCL_EXPORTS pcl::octree::OctreePointCloud<T, pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty, pcl::octree::Octree2BufBase<pcl::octree::OctreeLeafEmpty, pcl::octree::OctreeContainerEmpty > >;
834 
835 #endif /* OCTREE_POINTCLOUD_HPP_ */