Point Cloud Library (PCL)  1.7.1
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_ */
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:212
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.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
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.
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
int getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
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.
boost::shared_ptr< PointCloud > PointCloudPtr
static const unsigned char maxDepth
Definition: octree_key.h:136
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
boost::shared_ptr< std::vector< int > > IndicesPtr
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
Abstract octree node class
Definition: octree_nodes.h:69
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:128
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
Octree key class
Definition: octree_key.h:53
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
OctreeT::BranchNode BranchNode
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.
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
virtual ~OctreePointCloud()
Empty deconstructor.
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
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.
virtual bool genOctreeKeyForDataT(const int &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
void addPointsFromInputCloud()
Add points from input point cloud to octree.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.