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