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