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