Point Cloud Library (PCL)  1.11.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 #pragma once
40 
41 #include <pcl/common/common.h>
42 #include <pcl/common/point_tests.h> // for pcl::isFinite
43 #include <pcl/octree/impl/octree_base.hpp>
44 #include <pcl/types.h>
45 
46 #include <cassert>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointT,
50  typename LeafContainerT,
51  typename BranchContainerT,
52  typename OctreeT>
53 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
54  OctreePointCloud(const double resolution)
55 : OctreeT()
56 , input_(PointCloudConstPtr())
57 , indices_(IndicesConstPtr())
58 , epsilon_(0)
59 , resolution_(resolution)
60 , min_x_(0.0f)
61 , max_x_(resolution)
62 , min_y_(0.0f)
63 , max_y_(resolution)
64 , min_z_(0.0f)
65 , max_z_(resolution)
66 , bounding_box_defined_(false)
67 , max_objs_per_leaf_(0)
68 {
69  assert(resolution > 0.0f);
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointT,
74  typename LeafContainerT,
75  typename BranchContainerT,
76  typename OctreeT>
77 void
78 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
79  addPointsFromInputCloud()
80 {
81  if (indices_) {
82  for (const auto& index : *indices_) {
83  assert((index >= 0) && (index < input_->size()));
84 
85  if (isFinite((*input_)[index])) {
86  // add points to octree
87  this->addPointIdx(index);
88  }
89  }
90  }
91  else {
92  for (index_t i = 0; i < static_cast<index_t>(input_->size()); i++) {
93  if (isFinite((*input_)[i])) {
94  // add points to octree
95  this->addPointIdx(i);
96  }
97  }
98  }
99 }
100 
101 //////////////////////////////////////////////////////////////////////////////////////////////
102 template <typename PointT,
103  typename LeafContainerT,
104  typename BranchContainerT,
105  typename OctreeT>
106 void
107 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
108  addPointFromCloud(const uindex_t point_idx_arg, IndicesPtr indices_arg)
109 {
110  this->addPointIdx(point_idx_arg);
111  if (indices_arg)
112  indices_arg->push_back(point_idx_arg);
113 }
114 
115 //////////////////////////////////////////////////////////////////////////////////////////////
116 template <typename PointT,
117  typename LeafContainerT,
118  typename BranchContainerT,
119  typename OctreeT>
120 void
121 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
122  addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg)
123 {
124  assert(cloud_arg == input_);
125 
126  cloud_arg->push_back(point_arg);
127 
128  this->addPointIdx(cloud_arg->size() - 1);
129 }
130 
131 //////////////////////////////////////////////////////////////////////////////////////////////
132 template <typename PointT,
133  typename LeafContainerT,
134  typename BranchContainerT,
135  typename OctreeT>
136 void
137 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
138  addPointToCloud(const PointT& point_arg,
139  PointCloudPtr cloud_arg,
140  IndicesPtr indices_arg)
141 {
142  assert(cloud_arg == input_);
143  assert(indices_arg == indices_);
144 
145  cloud_arg->push_back(point_arg);
146 
147  this->addPointFromCloud(cloud_arg->size() - 1, indices_arg);
148 }
149 
150 //////////////////////////////////////////////////////////////////////////////////////////////
151 template <typename PointT,
152  typename LeafContainerT,
153  typename BranchContainerT,
154  typename OctreeT>
155 bool
156 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
157  isVoxelOccupiedAtPoint(const PointT& point_arg) const
158 {
159  if (!isPointWithinBoundingBox(point_arg)) {
160  return false;
161  }
162 
163  OctreeKey key;
164 
165  // generate key for point
166  this->genOctreeKeyforPoint(point_arg, key);
167 
168  // search for key in octree
169  return (this->existLeaf(key));
170 }
171 
172 //////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointT,
174  typename LeafContainerT,
175  typename BranchContainerT,
176  typename OctreeT>
177 bool
178 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
179  isVoxelOccupiedAtPoint(const index_t& point_idx_arg) const
180 {
181  // retrieve point from input cloud
182  const PointT& point = (*this->input_)[point_idx_arg];
183 
184  // search for voxel at point in octree
185  return (this->isVoxelOccupiedAtPoint(point));
186 }
187 
188 //////////////////////////////////////////////////////////////////////////////////////////////
189 template <typename PointT,
190  typename LeafContainerT,
191  typename BranchContainerT,
192  typename OctreeT>
193 bool
194 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
195  isVoxelOccupiedAtPoint(const double point_x_arg,
196  const double point_y_arg,
197  const double point_z_arg) const
198 {
199  // create a new point with the argument coordinates
200  PointT point;
201  point.x = point_x_arg;
202  point.y = point_y_arg;
203  point.z = point_z_arg;
204 
205  // search for voxel at point in octree
206  return (this->isVoxelOccupiedAtPoint(point));
207 }
208 
209 //////////////////////////////////////////////////////////////////////////////////////////////
210 template <typename PointT,
211  typename LeafContainerT,
212  typename BranchContainerT,
213  typename OctreeT>
214 void
215 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
216  deleteVoxelAtPoint(const PointT& point_arg)
217 {
218  if (!isPointWithinBoundingBox(point_arg)) {
219  return;
220  }
221 
222  OctreeKey key;
223 
224  // generate key for point
225  this->genOctreeKeyforPoint(point_arg, key);
226 
227  this->removeLeaf(key);
228 }
229 
230 //////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointT,
232  typename LeafContainerT,
233  typename BranchContainerT,
234  typename OctreeT>
235 void
236 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
237  deleteVoxelAtPoint(const index_t& point_idx_arg)
238 {
239  // retrieve point from input cloud
240  const PointT& point = (*this->input_)[point_idx_arg];
241 
242  // delete leaf at point
243  this->deleteVoxelAtPoint(point);
244 }
245 
246 //////////////////////////////////////////////////////////////////////////////////////////////
247 template <typename PointT,
248  typename LeafContainerT,
249  typename BranchContainerT,
250  typename OctreeT>
251 pcl::uindex_t
252 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
253  getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const
254 {
255  OctreeKey key;
256  key.x = key.y = key.z = 0;
257 
258  voxel_center_list_arg.clear();
259 
260  return getOccupiedVoxelCentersRecursive(this->root_node_, key, voxel_center_list_arg);
261 }
262 
263 //////////////////////////////////////////////////////////////////////////////////////////////
264 template <typename PointT,
265  typename LeafContainerT,
266  typename BranchContainerT,
267  typename OctreeT>
268 pcl::uindex_t
269 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
270  getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
271  const Eigen::Vector3f& end,
272  AlignedPointTVector& voxel_center_list,
273  float precision)
274 {
275  Eigen::Vector3f direction = end - origin;
276  float norm = direction.norm();
277  direction.normalize();
278 
279  const float step_size = static_cast<float>(resolution_) * precision;
280  // Ensure we get at least one step for the first voxel.
281  const auto nsteps = std::max<std::size_t>(1, norm / step_size);
282 
283  OctreeKey prev_key;
284 
285  bool bkeyDefined = false;
286 
287  // Walk along the line segment with small steps.
288  for (std::size_t i = 0; i < nsteps; ++i) {
289  Eigen::Vector3f p = origin + (direction * step_size * static_cast<float>(i));
290 
291  PointT octree_p;
292  octree_p.x = p.x();
293  octree_p.y = p.y();
294  octree_p.z = p.z();
295 
296  OctreeKey key;
297  this->genOctreeKeyforPoint(octree_p, key);
298 
299  // Not a new key, still the same voxel.
300  if ((key == prev_key) && (bkeyDefined))
301  continue;
302 
303  prev_key = key;
304  bkeyDefined = true;
305 
306  PointT center;
307  genLeafNodeCenterFromOctreeKey(key, center);
308  voxel_center_list.push_back(center);
309  }
310 
311  OctreeKey end_key;
312  PointT end_p;
313  end_p.x = end.x();
314  end_p.y = end.y();
315  end_p.z = end.z();
316  this->genOctreeKeyforPoint(end_p, end_key);
317  if (!(end_key == prev_key)) {
318  PointT center;
319  genLeafNodeCenterFromOctreeKey(end_key, center);
320  voxel_center_list.push_back(center);
321  }
322 
323  return (static_cast<uindex_t>(voxel_center_list.size()));
324 }
325 
326 //////////////////////////////////////////////////////////////////////////////////////////////
327 template <typename PointT,
328  typename LeafContainerT,
329  typename BranchContainerT,
330  typename OctreeT>
331 void
332 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
333  defineBoundingBox()
334 {
335 
336  double minX, minY, minZ, maxX, maxY, maxZ;
337 
338  PointT min_pt;
339  PointT max_pt;
340 
341  // bounding box cannot be changed once the octree contains elements
342  assert(this->leaf_count_ == 0);
343 
344  pcl::getMinMax3D(*input_, min_pt, max_pt);
345 
346  float minValue = std::numeric_limits<float>::epsilon() * 512.0f;
347 
348  minX = min_pt.x;
349  minY = min_pt.y;
350  minZ = min_pt.z;
351 
352  maxX = max_pt.x + minValue;
353  maxY = max_pt.y + minValue;
354  maxZ = max_pt.z + minValue;
355 
356  // generate bit masks for octree
357  defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
358 }
359 
360 //////////////////////////////////////////////////////////////////////////////////////////////
361 template <typename PointT,
362  typename LeafContainerT,
363  typename BranchContainerT,
364  typename OctreeT>
365 void
366 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
367  defineBoundingBox(const double min_x_arg,
368  const double min_y_arg,
369  const double min_z_arg,
370  const double max_x_arg,
371  const double max_y_arg,
372  const double max_z_arg)
373 {
374  // bounding box cannot be changed once the octree contains elements
375  assert(this->leaf_count_ == 0);
376 
377  assert(max_x_arg >= min_x_arg);
378  assert(max_y_arg >= min_y_arg);
379  assert(max_z_arg >= min_z_arg);
380 
381  min_x_ = min_x_arg;
382  max_x_ = max_x_arg;
383 
384  min_y_ = min_y_arg;
385  max_y_ = max_y_arg;
386 
387  min_z_ = min_z_arg;
388  max_z_ = max_z_arg;
389 
390  min_x_ = std::min(min_x_, max_x_);
391  min_y_ = std::min(min_y_, max_y_);
392  min_z_ = std::min(min_z_, max_z_);
393 
394  max_x_ = std::max(min_x_, max_x_);
395  max_y_ = std::max(min_y_, max_y_);
396  max_z_ = std::max(min_z_, max_z_);
397 
398  // generate bit masks for octree
399  getKeyBitSize();
400 
401  bounding_box_defined_ = true;
402 }
403 
404 //////////////////////////////////////////////////////////////////////////////////////////////
405 template <typename PointT,
406  typename LeafContainerT,
407  typename BranchContainerT,
408  typename OctreeT>
409 void
410 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
411  defineBoundingBox(const double max_x_arg,
412  const double max_y_arg,
413  const double max_z_arg)
414 {
415  // bounding box cannot be changed once the octree contains elements
416  assert(this->leaf_count_ == 0);
417 
418  assert(max_x_arg >= 0.0f);
419  assert(max_y_arg >= 0.0f);
420  assert(max_z_arg >= 0.0f);
421 
422  min_x_ = 0.0f;
423  max_x_ = max_x_arg;
424 
425  min_y_ = 0.0f;
426  max_y_ = max_y_arg;
427 
428  min_z_ = 0.0f;
429  max_z_ = max_z_arg;
430 
431  min_x_ = std::min(min_x_, max_x_);
432  min_y_ = std::min(min_y_, max_y_);
433  min_z_ = std::min(min_z_, max_z_);
434 
435  max_x_ = std::max(min_x_, max_x_);
436  max_y_ = std::max(min_y_, max_y_);
437  max_z_ = std::max(min_z_, max_z_);
438 
439  // generate bit masks for octree
440  getKeyBitSize();
441 
442  bounding_box_defined_ = true;
443 }
444 
445 //////////////////////////////////////////////////////////////////////////////////////////////
446 template <typename PointT,
447  typename LeafContainerT,
448  typename BranchContainerT,
449  typename OctreeT>
450 void
451 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
452  defineBoundingBox(const double cubeLen_arg)
453 {
454  // bounding box cannot be changed once the octree contains elements
455  assert(this->leaf_count_ == 0);
456 
457  assert(cubeLen_arg >= 0.0f);
458 
459  min_x_ = 0.0f;
460  max_x_ = cubeLen_arg;
461 
462  min_y_ = 0.0f;
463  max_y_ = cubeLen_arg;
464 
465  min_z_ = 0.0f;
466  max_z_ = cubeLen_arg;
467 
468  min_x_ = std::min(min_x_, max_x_);
469  min_y_ = std::min(min_y_, max_y_);
470  min_z_ = std::min(min_z_, max_z_);
471 
472  max_x_ = std::max(min_x_, max_x_);
473  max_y_ = std::max(min_y_, max_y_);
474  max_z_ = std::max(min_z_, max_z_);
475 
476  // generate bit masks for octree
477  getKeyBitSize();
478 
479  bounding_box_defined_ = true;
480 }
481 
482 //////////////////////////////////////////////////////////////////////////////////////////////
483 template <typename PointT,
484  typename LeafContainerT,
485  typename BranchContainerT,
486  typename OctreeT>
487 void
488 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
489  getBoundingBox(double& min_x_arg,
490  double& min_y_arg,
491  double& min_z_arg,
492  double& max_x_arg,
493  double& max_y_arg,
494  double& max_z_arg) const
495 {
496  min_x_arg = min_x_;
497  min_y_arg = min_y_;
498  min_z_arg = min_z_;
499 
500  max_x_arg = max_x_;
501  max_y_arg = max_y_;
502  max_z_arg = max_z_;
503 }
504 
505 //////////////////////////////////////////////////////////////////////////////////////////////
506 template <typename PointT,
507  typename LeafContainerT,
508  typename BranchContainerT,
509  typename OctreeT>
510 void
511 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
512  adoptBoundingBoxToPoint(const PointT& point_idx_arg)
513 {
514 
515  const float minValue = std::numeric_limits<float>::epsilon();
516 
517  // increase octree size until point fits into bounding box
518  while (true) {
519  bool bLowerBoundViolationX = (point_idx_arg.x < min_x_);
520  bool bLowerBoundViolationY = (point_idx_arg.y < min_y_);
521  bool bLowerBoundViolationZ = (point_idx_arg.z < min_z_);
522 
523  bool bUpperBoundViolationX = (point_idx_arg.x >= max_x_);
524  bool bUpperBoundViolationY = (point_idx_arg.y >= max_y_);
525  bool bUpperBoundViolationZ = (point_idx_arg.z >= max_z_);
526 
527  // do we violate any bounds?
528  if (bLowerBoundViolationX || bLowerBoundViolationY || bLowerBoundViolationZ ||
529  bUpperBoundViolationX || bUpperBoundViolationY || bUpperBoundViolationZ ||
530  (!bounding_box_defined_)) {
531 
532  if (bounding_box_defined_) {
533 
534  double octreeSideLen;
535  unsigned char child_idx;
536 
537  // octree not empty - we add another tree level and thus increase its size by a
538  // factor of 2*2*2
539  child_idx = static_cast<unsigned char>(((!bUpperBoundViolationX) << 2) |
540  ((!bUpperBoundViolationY) << 1) |
541  ((!bUpperBoundViolationZ)));
542 
543  BranchNode* newRootBranch;
544 
545  newRootBranch = new BranchNode();
546  this->branch_count_++;
547 
548  this->setBranchChildPtr(*newRootBranch, child_idx, this->root_node_);
549 
550  this->root_node_ = newRootBranch;
551 
552  octreeSideLen = static_cast<double>(1 << this->octree_depth_) * resolution_;
553 
554  if (!bUpperBoundViolationX)
555  min_x_ -= octreeSideLen;
556 
557  if (!bUpperBoundViolationY)
558  min_y_ -= octreeSideLen;
559 
560  if (!bUpperBoundViolationZ)
561  min_z_ -= octreeSideLen;
562 
563  // configure tree depth of octree
564  this->octree_depth_++;
565  this->setTreeDepth(this->octree_depth_);
566 
567  // recalculate bounding box width
568  octreeSideLen =
569  static_cast<double>(1 << this->octree_depth_) * resolution_ - minValue;
570 
571  // increase octree bounding box
572  max_x_ = min_x_ + octreeSideLen;
573  max_y_ = min_y_ + octreeSideLen;
574  max_z_ = min_z_ + octreeSideLen;
575  }
576  // bounding box is not defined - set it to point position
577  else {
578  // octree is empty - we set the center of the bounding box to our first pixel
579  this->min_x_ = point_idx_arg.x - this->resolution_ / 2;
580  this->min_y_ = point_idx_arg.y - this->resolution_ / 2;
581  this->min_z_ = point_idx_arg.z - this->resolution_ / 2;
582 
583  this->max_x_ = point_idx_arg.x + this->resolution_ / 2;
584  this->max_y_ = point_idx_arg.y + this->resolution_ / 2;
585  this->max_z_ = point_idx_arg.z + this->resolution_ / 2;
586 
587  getKeyBitSize();
588 
589  bounding_box_defined_ = true;
590  }
591  }
592  else
593  // no bound violations anymore - leave while loop
594  break;
595  }
596 }
597 
598 //////////////////////////////////////////////////////////////////////////////////////////////
599 template <typename PointT,
600  typename LeafContainerT,
601  typename BranchContainerT,
602  typename OctreeT>
603 void
604 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
605  expandLeafNode(LeafNode* leaf_node,
606  BranchNode* parent_branch,
607  unsigned char child_idx,
608  uindex_t depth_mask)
609 {
610 
611  if (depth_mask) {
612  // get amount of objects in leaf container
613  std::size_t leaf_obj_count = (*leaf_node)->getSize();
614 
615  // copy leaf data
616  Indices leafIndices;
617  leafIndices.reserve(leaf_obj_count);
618 
619  (*leaf_node)->getPointIndices(leafIndices);
620 
621  // delete current leaf node
622  this->deleteBranchChild(*parent_branch, child_idx);
623  this->leaf_count_--;
624 
625  // create new branch node
626  BranchNode* childBranch = this->createBranchChild(*parent_branch, child_idx);
627  this->branch_count_++;
628 
629  // add data to new branch
630  OctreeKey new_index_key;
631 
632  for (const auto& leafIndex : leafIndices) {
633 
634  const PointT& point_from_index = (*input_)[leafIndex];
635  // generate key
636  genOctreeKeyforPoint(point_from_index, new_index_key);
637 
638  LeafNode* newLeaf;
639  BranchNode* newBranchParent;
640  this->createLeafRecursive(
641  new_index_key, depth_mask, childBranch, newLeaf, newBranchParent);
642 
643  (*newLeaf)->addPointIndex(leafIndex);
644  }
645  }
646 }
647 
648 //////////////////////////////////////////////////////////////////////////////////////////////
649 template <typename PointT,
650  typename LeafContainerT,
651  typename BranchContainerT,
652  typename OctreeT>
653 void
654 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
655  addPointIdx(const uindex_t point_idx_arg)
656 {
657  OctreeKey key;
658 
659  assert(point_idx_arg < input_->size());
660 
661  const PointT& point = (*input_)[point_idx_arg];
662 
663  // make sure bounding box is big enough
664  adoptBoundingBoxToPoint(point);
665 
666  // generate key
667  genOctreeKeyforPoint(point, key);
668 
669  LeafNode* leaf_node;
670  BranchNode* parent_branch_of_leaf_node;
671  auto depth_mask = this->createLeafRecursive(
672  key, this->depth_mask_, this->root_node_, leaf_node, parent_branch_of_leaf_node);
673 
674  if (this->dynamic_depth_enabled_ && depth_mask) {
675  // get amount of objects in leaf container
676  std::size_t leaf_obj_count = (*leaf_node)->getSize();
677 
678  while (leaf_obj_count >= max_objs_per_leaf_ && depth_mask) {
679  // index to branch child
680  unsigned char child_idx = key.getChildIdxWithDepthMask(depth_mask * 2);
681 
682  expandLeafNode(leaf_node, parent_branch_of_leaf_node, child_idx, depth_mask);
683 
684  depth_mask = this->createLeafRecursive(key,
685  this->depth_mask_,
686  this->root_node_,
687  leaf_node,
688  parent_branch_of_leaf_node);
689  leaf_obj_count = (*leaf_node)->getSize();
690  }
691  }
692 
693  (*leaf_node)->addPointIndex(point_idx_arg);
694 }
695 
696 //////////////////////////////////////////////////////////////////////////////////////////////
697 template <typename PointT,
698  typename LeafContainerT,
699  typename BranchContainerT,
700  typename OctreeT>
701 const PointT&
702 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
703  getPointByIndex(const uindex_t index_arg) const
704 {
705  // retrieve point from input cloud
706  assert(index_arg < input_->size());
707  return ((*this->input_)[index_arg]);
708 }
709 
710 //////////////////////////////////////////////////////////////////////////////////////////////
711 template <typename PointT,
712  typename LeafContainerT,
713  typename BranchContainerT,
714  typename OctreeT>
715 void
716 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
717  getKeyBitSize()
718 {
719  const float minValue = std::numeric_limits<float>::epsilon();
720 
721  // find maximum key values for x, y, z
722  const auto max_key_x =
723  static_cast<uindex_t>(std::ceil((max_x_ - min_x_ - minValue) / resolution_));
724  const auto max_key_y =
725  static_cast<uindex_t>(std::ceil((max_y_ - min_y_ - minValue) / resolution_));
726  const auto max_key_z =
727  static_cast<uindex_t>(std::ceil((max_z_ - min_z_ - minValue) / resolution_));
728 
729  // find maximum amount of keys
730  const auto max_voxels =
731  std::max<uindex_t>(std::max(std::max(max_key_x, max_key_y), max_key_z), 2);
732 
733  // tree depth == amount of bits of max_voxels
734  this->octree_depth_ = std::max<uindex_t>(
735  std::min<uindex_t>(OctreeKey::maxDepth,
736  std::ceil(std::log2(max_voxels) - minValue)),
737  0);
738 
739  const auto octree_side_len =
740  static_cast<double>(1 << this->octree_depth_) * resolution_;
741 
742  if (this->leaf_count_ == 0) {
743  double octree_oversize_x;
744  double octree_oversize_y;
745  double octree_oversize_z;
746 
747  octree_oversize_x = (octree_side_len - (max_x_ - min_x_)) / 2.0;
748  octree_oversize_y = (octree_side_len - (max_y_ - min_y_)) / 2.0;
749  octree_oversize_z = (octree_side_len - (max_z_ - min_z_)) / 2.0;
750 
751  assert(octree_oversize_x > -minValue);
752  assert(octree_oversize_y > -minValue);
753  assert(octree_oversize_z > -minValue);
754 
755  if (octree_oversize_x > minValue) {
756  min_x_ -= octree_oversize_x;
757  max_x_ += octree_oversize_x;
758  }
759  if (octree_oversize_y > minValue) {
760  min_y_ -= octree_oversize_y;
761  max_y_ += octree_oversize_y;
762  }
763  if (octree_oversize_z > minValue) {
764  min_z_ -= octree_oversize_z;
765  max_z_ += octree_oversize_z;
766  }
767  }
768  else {
769  max_x_ = min_x_ + octree_side_len;
770  max_y_ = min_y_ + octree_side_len;
771  max_z_ = min_z_ + octree_side_len;
772  }
773 
774  // configure tree depth of octree
775  this->setTreeDepth(this->octree_depth_);
776 }
777 
778 //////////////////////////////////////////////////////////////////////////////////////////////
779 template <typename PointT,
780  typename LeafContainerT,
781  typename BranchContainerT,
782  typename OctreeT>
783 void
784 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
785  genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
786 {
787  // calculate integer key for point coordinates
788  key_arg.x = static_cast<uindex_t>((point_arg.x - this->min_x_) / this->resolution_);
789  key_arg.y = static_cast<uindex_t>((point_arg.y - this->min_y_) / this->resolution_);
790  key_arg.z = static_cast<uindex_t>((point_arg.z - this->min_z_) / this->resolution_);
791 
792  assert(key_arg.x <= this->max_key_.x);
793  assert(key_arg.y <= this->max_key_.y);
794  assert(key_arg.z <= this->max_key_.z);
795 }
796 
797 //////////////////////////////////////////////////////////////////////////////////////////////
798 template <typename PointT,
799  typename LeafContainerT,
800  typename BranchContainerT,
801  typename OctreeT>
802 void
803 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
804  genOctreeKeyforPoint(const double point_x_arg,
805  const double point_y_arg,
806  const double point_z_arg,
807  OctreeKey& key_arg) const
808 {
809  PointT temp_point;
810 
811  temp_point.x = static_cast<float>(point_x_arg);
812  temp_point.y = static_cast<float>(point_y_arg);
813  temp_point.z = static_cast<float>(point_z_arg);
814 
815  // generate key for point
816  genOctreeKeyforPoint(temp_point, key_arg);
817 }
818 
819 //////////////////////////////////////////////////////////////////////////////////////////////
820 template <typename PointT,
821  typename LeafContainerT,
822  typename BranchContainerT,
823  typename OctreeT>
824 bool
825 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
826  genOctreeKeyForDataT(const index_t& data_arg, OctreeKey& key_arg) const
827 {
828  const PointT temp_point = getPointByIndex(data_arg);
829 
830  // generate key for point
831  genOctreeKeyforPoint(temp_point, key_arg);
832 
833  return (true);
834 }
835 
836 //////////////////////////////////////////////////////////////////////////////////////////////
837 template <typename PointT,
838  typename LeafContainerT,
839  typename BranchContainerT,
840  typename OctreeT>
841 void
842 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
843  genLeafNodeCenterFromOctreeKey(const OctreeKey& key, PointT& point) const
844 {
845  // define point to leaf node voxel center
846  point.x = static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
847  this->min_x_);
848  point.y = static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
849  this->min_y_);
850  point.z = static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
851  this->min_z_);
852 }
853 
854 //////////////////////////////////////////////////////////////////////////////////////////////
855 template <typename PointT,
856  typename LeafContainerT,
857  typename BranchContainerT,
858  typename OctreeT>
859 void
860 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
861  genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
862  uindex_t tree_depth_arg,
863  PointT& point_arg) const
864 {
865  // generate point for voxel center defined by treedepth (bitLen) and key
866  point_arg.x = static_cast<float>(
867  (static_cast<double>(key_arg.x) + 0.5f) *
868  (this->resolution_ *
869  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
870  this->min_x_);
871  point_arg.y = static_cast<float>(
872  (static_cast<double>(key_arg.y) + 0.5f) *
873  (this->resolution_ *
874  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
875  this->min_y_);
876  point_arg.z = static_cast<float>(
877  (static_cast<double>(key_arg.z) + 0.5f) *
878  (this->resolution_ *
879  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg))) +
880  this->min_z_);
881 }
882 
883 //////////////////////////////////////////////////////////////////////////////////////////////
884 template <typename PointT,
885  typename LeafContainerT,
886  typename BranchContainerT,
887  typename OctreeT>
888 void
889 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
890  genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
891  uindex_t tree_depth_arg,
892  Eigen::Vector3f& min_pt,
893  Eigen::Vector3f& max_pt) const
894 {
895  // calculate voxel size of current tree depth
896  double voxel_side_len =
897  this->resolution_ *
898  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
899 
900  // calculate voxel bounds
901  min_pt(0) = static_cast<float>(static_cast<double>(key_arg.x) * voxel_side_len +
902  this->min_x_);
903  min_pt(1) = static_cast<float>(static_cast<double>(key_arg.y) * voxel_side_len +
904  this->min_y_);
905  min_pt(2) = static_cast<float>(static_cast<double>(key_arg.z) * voxel_side_len +
906  this->min_z_);
907 
908  max_pt(0) = static_cast<float>(static_cast<double>(key_arg.x + 1) * voxel_side_len +
909  this->min_x_);
910  max_pt(1) = static_cast<float>(static_cast<double>(key_arg.y + 1) * voxel_side_len +
911  this->min_y_);
912  max_pt(2) = static_cast<float>(static_cast<double>(key_arg.z + 1) * voxel_side_len +
913  this->min_z_);
914 }
915 
916 //////////////////////////////////////////////////////////////////////////////////////////////
917 template <typename PointT,
918  typename LeafContainerT,
919  typename BranchContainerT,
920  typename OctreeT>
921 double
922 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
923  getVoxelSquaredSideLen(uindex_t tree_depth_arg) const
924 {
925  double side_len;
926 
927  // side length of the voxel cube increases exponentially with the octree depth
928  side_len = this->resolution_ *
929  static_cast<double>(1 << (this->octree_depth_ - tree_depth_arg));
930 
931  // squared voxel side length
932  side_len *= side_len;
933 
934  return (side_len);
935 }
936 
937 //////////////////////////////////////////////////////////////////////////////////////////////
938 template <typename PointT,
939  typename LeafContainerT,
940  typename BranchContainerT,
941  typename OctreeT>
942 double
943 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
944  getVoxelSquaredDiameter(uindex_t tree_depth_arg) const
945 {
946  // return the squared side length of the voxel cube as a function of the octree depth
947  return (getVoxelSquaredSideLen(tree_depth_arg) * 3);
948 }
949 
950 //////////////////////////////////////////////////////////////////////////////////////////////
951 template <typename PointT,
952  typename LeafContainerT,
953  typename BranchContainerT,
954  typename OctreeT>
955 pcl::uindex_t
956 pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::
957  getOccupiedVoxelCentersRecursive(const BranchNode* node_arg,
958  const OctreeKey& key_arg,
959  AlignedPointTVector& voxel_center_list_arg) const
960 {
961  uindex_t voxel_count = 0;
962 
963  // iterate over all children
964  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
965  if (!this->branchHasChild(*node_arg, child_idx))
966  continue;
967 
968  const OctreeNode* child_node;
969  child_node = this->getBranchChildPtr(*node_arg, child_idx);
970 
971  // generate new key for current branch voxel
972  OctreeKey new_key;
973  new_key.x = (key_arg.x << 1) | (!!(child_idx & (1 << 2)));
974  new_key.y = (key_arg.y << 1) | (!!(child_idx & (1 << 1)));
975  new_key.z = (key_arg.z << 1) | (!!(child_idx & (1 << 0)));
976 
977  switch (child_node->getNodeType()) {
978  case BRANCH_NODE: {
979  // recursively proceed with indexed child branch
980  voxel_count += getOccupiedVoxelCentersRecursive(
981  static_cast<const BranchNode*>(child_node), new_key, voxel_center_list_arg);
982  break;
983  }
984  case LEAF_NODE: {
985  PointT new_point;
986 
987  genLeafNodeCenterFromOctreeKey(new_key, new_point);
988  voxel_center_list_arg.push_back(new_point);
989 
990  voxel_count++;
991  break;
992  }
993  default:
994  break;
995  }
996  }
997  return (voxel_count);
998 }
999 
1000 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataTVector(T) \
1001  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1002  T, \
1003  pcl::octree::OctreeContainerPointIndices, \
1004  pcl::octree::OctreeContainerEmpty, \
1005  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndices, \
1006  pcl::octree::OctreeContainerEmpty>>;
1007 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataTVector(T) \
1008  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1009  T, \
1010  pcl::octree::OctreeContainerPointIndices, \
1011  pcl::octree::OctreeContainerEmpty, \
1012  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndices, \
1013  pcl::octree::OctreeContainerEmpty>>;
1014 
1015 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithLeafDataT(T) \
1016  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1017  T, \
1018  pcl::octree::OctreeContainerPointIndex, \
1019  pcl::octree::OctreeContainerEmpty, \
1020  pcl::octree::OctreeBase<pcl::octree::OctreeContainerPointIndex, \
1021  pcl::octree::OctreeContainerEmpty>>;
1022 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithLeafDataT(T) \
1023  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1024  T, \
1025  pcl::octree::OctreeContainerPointIndex, \
1026  pcl::octree::OctreeContainerEmpty, \
1027  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerPointIndex, \
1028  pcl::octree::OctreeContainerEmpty>>;
1029 
1030 #define PCL_INSTANTIATE_OctreePointCloudSingleBufferWithEmptyLeaf(T) \
1031  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1032  T, \
1033  pcl::octree::OctreeContainerEmpty, \
1034  pcl::octree::OctreeContainerEmpty, \
1035  pcl::octree::OctreeBase<pcl::octree::OctreeContainerEmpty, \
1036  pcl::octree::OctreeContainerEmpty>>;
1037 #define PCL_INSTANTIATE_OctreePointCloudDoubleBufferWithEmptyLeaf(T) \
1038  template class PCL_EXPORTS pcl::octree::OctreePointCloud< \
1039  T, \
1040  pcl::octree::OctreeContainerEmpty, \
1041  pcl::octree::OctreeContainerEmpty, \
1042  pcl::octree::Octree2BufBase<pcl::octree::OctreeContainerEmpty, \
1043  pcl::octree::OctreeContainerEmpty>>;
pcl::octree::OctreePointCloud::getVoxelSquaredDiameter
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
Definition: octree_pointcloud.h:374
pcl::octree::OctreePointCloud::deleteVoxelAtPoint
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Definition: octree_pointcloud.hpp:216
common.h
pcl::octree::OctreePointCloud::genOctreeKeyforPoint
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generate octree key for voxel at a given point.
Definition: octree_pointcloud.hpp:785
pcl::isFinite
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:55
pcl::octree::OctreeKey::z
uindex_t z
Definition: octree_key.h:149
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
types.h
Defines basic non-point types used by PCL.
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud.h:90
pcl::octree::OctreePointCloud::getKeyBitSize
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
Definition: octree_pointcloud.hpp:717
pcl::octree::OctreeKey::y
uindex_t y
Definition: octree_key.h:148
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:79
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:110
pcl::octree::OctreePointCloud::adoptBoundingBoxToPoint
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
Definition: octree_pointcloud.hpp:512
pcl::octree::OctreePointCloud::addPointToCloud
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
Definition: octree_pointcloud.hpp:122
pcl::octree::OctreePointCloud::defineBoundingBox
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
Definition: octree_pointcloud.hpp:333
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition: octree_pointcloud.hpp:108
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::octree::OctreeNode
Abstract octree node class
Definition: octree_nodes.h:58
pcl::octree::OctreePointCloud::genVoxelBoundsFromOctreeKey
void genVoxelBoundsFromOctreeKey(const OctreeKey &key_arg, uindex_t 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.
Definition: octree_pointcloud.hpp:890
pcl::octree::LEAF_NODE
@ LEAF_NODE
Definition: octree_nodes.h:51
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:80
pcl::octree::BRANCH_NODE
@ BRANCH_NODE
Definition: octree_nodes.h:51
pcl::octree::OctreePointCloud::getPointByIndex
const PointT & getPointByIndex(uindex_t index_arg) const
Get point at index from input pointcloud dataset.
Definition: octree_pointcloud.hpp:703
pcl::octree::OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty >
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:179
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud.h:89
pcl::octree::OctreePointCloud::addPointIdx
virtual void addPointIdx(uindex_t point_idx_arg)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:655
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: octree_pointcloud.h:86
pcl::octree::OctreePointCloud::genVoxelCenterFromOctreeKey
void genVoxelCenterFromOctreeKey(const OctreeKey &key_arg, uindex_t tree_depth_arg, PointT &point_arg) const
Generate a point at center of octree voxel at given tree level.
Definition: octree_pointcloud.hpp:861
pcl::octree::OctreePointCloud::getVoxelSquaredSideLen
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
Definition: octree_pointcloud.h:390
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::octree::OctreePointCloud::isVoxelOccupiedAtPoint
bool isVoxelOccupiedAtPoint(const PointT &point_arg) const
Check if voxel at given point exist.
Definition: octree_pointcloud.hpp:157
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: octree_pointcloud.h:85
pcl::octree::OctreeKey::getChildIdxWithDepthMask
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Definition: octree_key.h:132
pcl::octree::OctreePointCloud::getOccupiedVoxelCenters
uindex_t getOccupiedVoxelCenters(AlignedPointTVector &voxel_center_list_arg) const
Get a PointT vector of centers of all occupied voxels.
Definition: octree_pointcloud.hpp:253
pcl::getMinMax3D
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:295
pcl::octree::OctreePointCloud::genLeafNodeCenterFromOctreeKey
void genLeafNodeCenterFromOctreeKey(const OctreeKey &key_arg, PointT &point_arg) const
Generate a point at center of leaf node voxel.
Definition: octree_pointcloud.hpp:843
pcl::octree::OctreeKey::x
uindex_t x
Definition: octree_key.h:147
pcl::octree::OctreePointCloud::getBoundingBox
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.
Definition: octree_pointcloud.hpp:489
pcl::octree::OctreePointCloud::getOccupiedVoxelCentersRecursive
uindex_t 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.
Definition: octree_pointcloud.hpp:957
pcl::octree::OctreePointCloud::expandLeafNode
void expandLeafNode(LeafNode *leaf_node, BranchNode *parent_branch, unsigned char child_idx, uindex_t depth_mask)
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud.hpp:605
pcl::octree::OctreePointCloud< PointT, OctreeContainerPointIndex, OctreeContainerEmpty, OctreeBase< OctreeContainerPointIndex, OctreeContainerEmpty > >::AlignedPointTVector
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_pointcloud.h:107
pcl::octree::OctreePointCloud::genOctreeKeyForDataT
virtual bool genOctreeKeyForDataT(const index_t &data_arg, OctreeKey &key_arg) const
Virtual method for generating octree key for a given point index.
Definition: octree_pointcloud.hpp:826
pcl::octree::OctreePointCloud::OctreePointCloud
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
Definition: octree_pointcloud.hpp:54
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:118
pcl::octree::OctreePointCloud::getApproxIntersectedVoxelCentersBySegment
uindex_t 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.
Definition: octree_pointcloud.hpp:270