Point Cloud Library (PCL)  1.10.1-dev
octree_pointcloud.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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/octree/octree_base.h>
42 
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_types.h>
45 
46 #include <vector>
47 
48 namespace pcl {
49 namespace octree {
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52 /** \brief @b Octree pointcloud class
53  * \note Octree implementation for pointclouds. Only indices are stored by the octree
54  * leaf nodes (zero-copy).
55  * \note The octree pointcloud class needs to be initialized with its voxel resolution.
56  * Its bounding box is automatically adjusted
57  * \note according to the pointcloud dimension or it can be predefined.
58  * \note Note: The tree depth equates to the resolution and the bounding box dimensions
59  * of the octree.
60  * \tparam PointT: type of point used in pointcloud
61  * \tparam LeafContainerT: leaf node container
62  * \tparam BranchContainerT: branch node container
63  * \tparam OctreeT: octree implementation
64  * \ingroup octree
65  * \author Julius Kammerl * (julius@kammerl.de)
66  */
67 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointT,
69  typename LeafContainerT = OctreeContainerPointIndices,
70  typename BranchContainerT = OctreeContainerEmpty,
71  typename OctreeT = OctreeBase<LeafContainerT, BranchContainerT>>
72 
73 class OctreePointCloud : public OctreeT {
74 public:
75  using Base = OctreeT;
76 
77  using LeafNode = typename OctreeT::LeafNode;
78  using BranchNode = typename OctreeT::BranchNode;
79 
80  /** \brief Octree pointcloud constructor.
81  * \param[in] resolution_arg octree resolution at lowest octree level
82  */
83  OctreePointCloud(const double resolution_arg);
84 
85  // public typedefs
88 
90  using PointCloudPtr = typename PointCloud::Ptr;
92 
93  // public typedefs for single/double buffering
95  LeafContainerT,
96  BranchContainerT,
98  // using DoubleBuffer = OctreePointCloud<PointT, LeafContainerT, BranchContainerT,
99  // Octree2BufBase<LeafContainerT> >;
100 
101  // Boost shared pointers
102  using Ptr =
104  using ConstPtr = shared_ptr<
106 
107  // Eigen aligned allocator
108  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT>>;
109  using AlignedPointXYZVector =
110  std::vector<PointXYZ, Eigen::aligned_allocator<PointXYZ>>;
111 
112  /** \brief Provide a pointer to the input data set.
113  * \param[in] cloud_arg the const boost shared pointer to a PointCloud message
114  * \param[in] indices_arg the point indices subset that is to be used from \a cloud -
115  * if 0 the whole point cloud is used
116  */
117  inline void
119  const IndicesConstPtr& indices_arg = IndicesConstPtr())
120  {
121  input_ = cloud_arg;
122  indices_ = indices_arg;
123  }
124 
125  /** \brief Get a pointer to the vector of indices used.
126  * \return pointer to vector of indices used.
127  */
128  inline IndicesConstPtr const
129  getIndices() const
130  {
131  return (indices_);
132  }
133 
134  /** \brief Get a pointer to the input point cloud dataset.
135  * \return pointer to pointcloud input class.
136  */
137  inline PointCloudConstPtr
139  {
140  return (input_);
141  }
142 
143  /** \brief Set the search epsilon precision (error bound) for nearest neighbors
144  * searches.
145  * \param[in] eps precision (error bound) for nearest neighbors searches
146  */
147  inline void
148  setEpsilon(double eps)
149  {
150  epsilon_ = eps;
151  }
152 
153  /** \brief Get the search epsilon precision (error bound) for nearest neighbors
154  * searches. */
155  inline double
156  getEpsilon() const
157  {
158  return (epsilon_);
159  }
160 
161  /** \brief Set/change the octree voxel resolution
162  * \param[in] resolution_arg side length of voxels at lowest tree level
163  */
164  inline void
165  setResolution(double resolution_arg)
166  {
167  // octree needs to be empty to change its resolution
168  assert(this->leaf_count_ == 0);
169 
170  resolution_ = resolution_arg;
171 
172  getKeyBitSize();
173  }
174 
175  /** \brief Get octree voxel resolution
176  * \return voxel resolution at lowest tree level
177  */
178  inline double
180  {
181  return (resolution_);
182  }
183 
184  /** \brief Get the maximum depth of the octree.
185  * \return depth_arg: maximum depth of octree
186  * */
187  inline unsigned int
188  getTreeDepth() const
189  {
190  return this->octree_depth_;
191  }
192 
193  /** \brief Add points from input point cloud to octree. */
194  void
196 
197  /** \brief Add point at given index from input point cloud to octree. Index will be
198  * also added to indices vector.
199  * \param[in] point_idx_arg index of point to be added
200  * \param[in] indices_arg pointer to indices vector of the dataset (given by \a
201  * setInputCloud)
202  */
203  void
204  addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg);
205 
206  /** \brief Add point simultaneously to octree and input point cloud.
207  * \param[in] point_arg point to be added
208  * \param[in] cloud_arg pointer to input point cloud dataset (given by \a
209  * setInputCloud)
210  */
211  void
212  addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg);
213 
214  /** \brief Add point simultaneously to octree and input point cloud. A corresponding
215  * index will be added to the indices vector.
216  * \param[in] point_arg point to be added
217  * \param[in] cloud_arg pointer to input point cloud dataset (given by \a
218  * setInputCloud)
219  * \param[in] indices_arg pointer to indices vector of the dataset (given by \a
220  * setInputCloud)
221  */
222  void
223  addPointToCloud(const PointT& point_arg,
224  PointCloudPtr cloud_arg,
225  IndicesPtr indices_arg);
226 
227  /** \brief Check if voxel at given point exist.
228  * \param[in] point_arg point to be checked
229  * \return "true" if voxel exist; "false" otherwise
230  */
231  bool
232  isVoxelOccupiedAtPoint(const PointT& point_arg) const;
233 
234  /** \brief Delete the octree structure and its leaf nodes.
235  * */
236  void
238  {
239  // reset bounding box
240  min_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0;
241  this->bounding_box_defined_ = false;
242 
243  OctreeT::deleteTree();
244  }
245 
246  /** \brief Check if voxel at given point coordinates exist.
247  * \param[in] point_x_arg X coordinate of point to be checked
248  * \param[in] point_y_arg Y coordinate of point to be checked
249  * \param[in] point_z_arg Z coordinate of point to be checked
250  * \return "true" if voxel exist; "false" otherwise
251  */
252  bool
253  isVoxelOccupiedAtPoint(const double point_x_arg,
254  const double point_y_arg,
255  const double point_z_arg) const;
256 
257  /** \brief Check if voxel at given point from input cloud exist.
258  * \param[in] point_idx_arg point to be checked
259  * \return "true" if voxel exist; "false" otherwise
260  */
261  bool
262  isVoxelOccupiedAtPoint(const int& point_idx_arg) const;
263 
264  /** \brief Get a PointT vector of centers of all occupied voxels.
265  * \param[out] voxel_center_list_arg results are written to this vector of PointT
266  * elements
267  * \return number of occupied voxels
268  */
269  int
270  getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const;
271 
272  /** \brief Get a PointT vector of centers of voxels intersected by a line segment.
273  * This returns a approximation of the actual intersected voxels by walking
274  * along the line with small steps. Voxels are ordered, from closest to
275  * furthest w.r.t. the origin.
276  * \param[in] origin origin of the line segment
277  * \param[in] end end of the line segment
278  * \param[out] voxel_center_list results are written to this vector of PointT elements
279  * \param[in] precision determines the size of the steps: step_size =
280  * octree_resolution x precision
281  * \return number of intersected voxels
282  */
283  int
284  getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin,
285  const Eigen::Vector3f& end,
286  AlignedPointTVector& voxel_center_list,
287  float precision = 0.2);
288 
289  /** \brief Delete leaf node / voxel at given point
290  * \param[in] point_arg point addressing the voxel to be deleted.
291  */
292  void
293  deleteVoxelAtPoint(const PointT& point_arg);
294 
295  /** \brief Delete leaf node / voxel at given point from input cloud
296  * \param[in] point_idx_arg index of point addressing the voxel to be deleted.
297  */
298  void
299  deleteVoxelAtPoint(const int& point_idx_arg);
300 
301  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
302  // Bounding box methods
303  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
304 
305  /** \brief Investigate dimensions of pointcloud data set and define corresponding
306  * bounding box for octree. */
307  void
309 
310  /** \brief Define bounding box for octree
311  * \note Bounding box cannot be changed once the octree contains elements.
312  * \param[in] min_x_arg X coordinate of lower bounding box corner
313  * \param[in] min_y_arg Y coordinate of lower bounding box corner
314  * \param[in] min_z_arg Z coordinate of lower bounding box corner
315  * \param[in] max_x_arg X coordinate of upper bounding box corner
316  * \param[in] max_y_arg Y coordinate of upper bounding box corner
317  * \param[in] max_z_arg Z coordinate of upper bounding box corner
318  */
319  void
320  defineBoundingBox(const double min_x_arg,
321  const double min_y_arg,
322  const double min_z_arg,
323  const double max_x_arg,
324  const double max_y_arg,
325  const double max_z_arg);
326 
327  /** \brief Define bounding box for octree
328  * \note Lower bounding box point is set to (0, 0, 0)
329  * \note Bounding box cannot be changed once the octree contains elements.
330  * \param[in] max_x_arg X coordinate of upper bounding box corner
331  * \param[in] max_y_arg Y coordinate of upper bounding box corner
332  * \param[in] max_z_arg Z coordinate of upper bounding box corner
333  */
334  void
335  defineBoundingBox(const double max_x_arg,
336  const double max_y_arg,
337  const double max_z_arg);
338 
339  /** \brief Define bounding box cube for octree
340  * \note Lower bounding box corner is set to (0, 0, 0)
341  * \note Bounding box cannot be changed once the octree contains elements.
342  * \param[in] cubeLen_arg side length of bounding box cube.
343  */
344  void
345  defineBoundingBox(const double cubeLen_arg);
346 
347  /** \brief Get bounding box for octree
348  * \note Bounding box cannot be changed once the octree contains elements.
349  * \param[in] min_x_arg X coordinate of lower bounding box corner
350  * \param[in] min_y_arg Y coordinate of lower bounding box corner
351  * \param[in] min_z_arg Z coordinate of lower bounding box corner
352  * \param[in] max_x_arg X coordinate of upper bounding box corner
353  * \param[in] max_y_arg Y coordinate of upper bounding box corner
354  * \param[in] max_z_arg Z coordinate of upper bounding box corner
355  */
356  void
357  getBoundingBox(double& min_x_arg,
358  double& min_y_arg,
359  double& min_z_arg,
360  double& max_x_arg,
361  double& max_y_arg,
362  double& max_z_arg) const;
363 
364  /** \brief Calculates the squared diameter of a voxel at given tree depth
365  * \param[in] tree_depth_arg depth/level in octree
366  * \return squared diameter
367  */
368  double
369  getVoxelSquaredDiameter(unsigned int tree_depth_arg) const;
370 
371  /** \brief Calculates the squared diameter of a voxel at leaf depth
372  * \return squared diameter
373  */
374  inline double
376  {
377  return getVoxelSquaredDiameter(this->octree_depth_);
378  }
379 
380  /** \brief Calculates the squared voxel cube side length at given tree depth
381  * \param[in] tree_depth_arg depth/level in octree
382  * \return squared voxel cube side length
383  */
384  double
385  getVoxelSquaredSideLen(unsigned int tree_depth_arg) const;
386 
387  /** \brief Calculates the squared voxel cube side length at leaf level
388  * \return squared voxel cube side length
389  */
390  inline double
392  {
393  return getVoxelSquaredSideLen(this->octree_depth_);
394  }
395 
396  /** \brief Generate bounds of the current voxel of an octree iterator
397  * \param[in] iterator: octree iterator
398  * \param[out] min_pt lower bound of voxel
399  * \param[out] max_pt upper bound of voxel
400  */
401  inline void
403  Eigen::Vector3f& min_pt,
404  Eigen::Vector3f& max_pt) const
405  {
407  iterator.getCurrentOctreeDepth(),
408  min_pt,
409  max_pt);
410  }
411 
412  /** \brief Enable dynamic octree structure
413  * \note Leaf nodes are kept as close to the root as possible and are only expanded
414  * if the number of DataT objects within a leaf node exceeds a fixed limit.
415  * \param maxObjsPerLeaf: maximum number of DataT objects per leaf
416  * */
417  inline void
418  enableDynamicDepth(std::size_t maxObjsPerLeaf)
419  {
420  assert(this->leaf_count_ == 0);
421  max_objs_per_leaf_ = maxObjsPerLeaf;
422 
423  this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 0;
424  }
425 
426 protected:
427  /** \brief Add point at index from input pointcloud dataset to octree
428  * \param[in] point_idx_arg the index representing the point in the dataset given by
429  * \a setInputCloud to be added
430  */
431  virtual void
432  addPointIdx(const int point_idx_arg);
433 
434  /** \brief Add point at index from input pointcloud dataset to octree
435  * \param[in] leaf_node to be expanded
436  * \param[in] parent_branch parent of leaf node to be expanded
437  * \param[in] child_idx child index of leaf node (in parent branch)
438  * \param[in] depth_mask of leaf node to be expanded
439  */
440  void
441  expandLeafNode(LeafNode* leaf_node,
442  BranchNode* parent_branch,
443  unsigned char child_idx,
444  unsigned int depth_mask);
445 
446  /** \brief Get point at index from input pointcloud dataset
447  * \param[in] index_arg index representing the point in the dataset given by \a
448  * setInputCloud
449  * \return PointT from input pointcloud dataset
450  */
451  const PointT&
452  getPointByIndex(const unsigned int index_arg) const;
453 
454  /** \brief Find octree leaf node at a given point
455  * \param[in] point_arg query point
456  * \return pointer to leaf node. If leaf node does not exist, pointer is 0.
457  */
458  LeafContainerT*
459  findLeafAtPoint(const PointT& point_arg) const
460  {
461  OctreeKey key;
462 
463  // generate key for point
464  this->genOctreeKeyforPoint(point_arg, key);
465 
466  return (this->findLeaf(key));
467  }
468 
469  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
470  // Protected octree methods based on octree keys
471  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
472 
473  /** \brief Define octree key setting and octree depth based on defined bounding box.
474  */
475  void
476  getKeyBitSize();
477 
478  /** \brief Grow the bounding box/octree until point fits
479  * \param[in] point_idx_arg point that should be within bounding box;
480  */
481  void
482  adoptBoundingBoxToPoint(const PointT& point_idx_arg);
483 
484  /** \brief Checks if given point is within the bounding box of the octree
485  * \param[in] point_idx_arg point to be checked for bounding box violations
486  * \return "true" - no bound violation
487  */
488  inline bool
489  isPointWithinBoundingBox(const PointT& point_idx_arg) const
490  {
491  return (!((point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_) ||
492  (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_) ||
493  (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_)));
494  }
495 
496  /** \brief Generate octree key for voxel at a given point
497  * \param[in] point_arg the point addressing a voxel
498  * \param[out] key_arg write octree key to this reference
499  */
500  void
501  genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const;
502 
503  /** \brief Generate octree key for voxel at a given point
504  * \param[in] point_x_arg X coordinate of point addressing a voxel
505  * \param[in] point_y_arg Y coordinate of point addressing a voxel
506  * \param[in] point_z_arg Z coordinate of point addressing a voxel
507  * \param[out] key_arg write octree key to this reference
508  */
509  void
510  genOctreeKeyforPoint(const double point_x_arg,
511  const double point_y_arg,
512  const double point_z_arg,
513  OctreeKey& key_arg) const;
514 
515  /** \brief Virtual method for generating octree key for a given point index.
516  * \note This method enables to assign indices to leaf nodes during octree
517  * deserialization.
518  * \param[in] data_arg index value representing a point in the dataset given by \a
519  * setInputCloud
520  * \param[out] key_arg write octree key to this reference \return "true" - octree keys
521  * are assignable
522  */
523  virtual bool
524  genOctreeKeyForDataT(const int& data_arg, OctreeKey& key_arg) const;
525 
526  /** \brief Generate a point at center of leaf node voxel
527  * \param[in] key_arg octree key addressing a leaf node.
528  * \param[out] point_arg write leaf node voxel center to this point reference
529  */
530  void
531  genLeafNodeCenterFromOctreeKey(const OctreeKey& key_arg, PointT& point_arg) const;
532 
533  /** \brief Generate a point at center of octree voxel at given tree level
534  * \param[in] key_arg octree key addressing an octree node.
535  * \param[in] tree_depth_arg octree depth of query voxel
536  * \param[out] point_arg write leaf node center point to this reference
537  */
538  void
539  genVoxelCenterFromOctreeKey(const OctreeKey& key_arg,
540  unsigned int tree_depth_arg,
541  PointT& point_arg) const;
542 
543  /** \brief Generate bounds of an octree voxel using octree key and tree depth
544  * arguments
545  * \param[in] key_arg octree key addressing an octree node.
546  * \param[in] tree_depth_arg octree depth of query voxel
547  * \param[out] min_pt lower bound of voxel
548  * \param[out] max_pt upper bound of voxel
549  */
550  void
551  genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg,
552  unsigned int tree_depth_arg,
553  Eigen::Vector3f& min_pt,
554  Eigen::Vector3f& max_pt) const;
555 
556  /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel
557  * centers.
558  * \param[in] node_arg current octree node to be explored
559  * \param[in] key_arg octree key addressing a leaf node.
560  * \param[out] voxel_center_list_arg results are written to this vector of PointT
561  * elements
562  * \return number of voxels found
563  */
564  int
566  const OctreeKey& key_arg,
567  AlignedPointTVector& voxel_center_list_arg) const;
568 
569  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
570  // Globals
571  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
572  /** \brief Pointer to input point cloud dataset. */
574 
575  /** \brief A pointer to the vector of point indices to use. */
577 
578  /** \brief Epsilon precision (error bound) for nearest neighbors searches. */
579  double epsilon_;
580 
581  /** \brief Octree resolution. */
582  double resolution_;
583 
584  // Octree bounding box coordinates
585  double min_x_;
586  double max_x_;
587 
588  double min_y_;
589  double max_y_;
590 
591  double min_z_;
592  double max_z_;
593 
594  /** \brief Flag indicating if octree has defined bounding box. */
596 
597  /** \brief Amount of DataT objects per leafNode before expanding branch
598  * \note zero indicates a fixed/maximum depth octree structure
599  * **/
600  std::size_t max_objs_per_leaf_;
601 };
602 
603 } // namespace octree
604 } // namespace pcl
605 
606 #ifdef PCL_NO_PRECOMPILE
607 #include <pcl/octree/impl/octree_pointcloud.hpp>
608 #endif
shared_ptr< const OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > ConstPtr
Octree pointcloud class
void getVoxelBounds(const OctreeIteratorBase< OctreeT > &iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt) const
Generate bounds of the current voxel of an octree iterator.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
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.
IndicesConstPtr const getIndices() const
Get a pointer to the vector of indices used.
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.
const PointT & getPointByIndex(const unsigned int index_arg) const
Get point at index from input pointcloud dataset.
bool isPointWithinBoundingBox(const PointT &point_idx_arg) const
Checks if given point is within the bounding box of the octree.
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
OctreePointCloud(const double resolution_arg)
Octree pointcloud constructor.
void adoptBoundingBoxToPoint(const PointT &point_idx_arg)
Grow the bounding box/octree until point fits.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void getBoundingBox(double &min_x_arg, double &min_y_arg, double &min_z_arg, double &max_x_arg, double &max_y_arg, double &max_z_arg) const
Get bounding box for octree.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
PointCloudConstPtr input_
Pointer to input point cloud dataset.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree...
unsigned int getCurrentOctreeDepth() const
Get the current depth level of octree.
void enableDynamicDepth(std::size_t maxObjsPerLeaf)
Enable dynamic octree structure.
Defines all the PCL implemented PointT point type structures.
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.
double getResolution() const
Get octree voxel resolution.
unsigned int getTreeDepth() const
Get the maximum depth of the octree.
void getKeyBitSize()
Define octree key setting and octree depth based on defined bounding box.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
std::size_t max_objs_per_leaf_
Amount of DataT objects per leafNode before expanding branch.
Octree key class
Definition: octree_key.h:52
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Abstract octree leaf class
Definition: octree_nodes.h:85
const OctreeKey & getCurrentOctreeKey() const
Get octree key for the current iterator octree node.
void deleteTree()
Delete the octree structure and its leaf nodes.
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.
LeafContainerT * findLeafAtPoint(const PointT &point_arg) const
Find octree leaf node at a given point.
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
std::vector< PointXYZ, Eigen::aligned_allocator< PointXYZ > > AlignedPointXYZVector
Abstract octree iterator class
double getVoxelSquaredSideLen() const
Calculates the squared voxel cube side length at leaf level.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double epsilon_
Epsilon precision (error bound) for nearest neighbors searches.
void deleteVoxelAtPoint(const PointT &point_arg)
Delete leaf node / voxel at given point.
Abstract octree branch class
Definition: octree_nodes.h:168
void setEpsilon(double eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
bool bounding_box_defined_
Flag indicating if octree has defined bounding box.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
double getVoxelSquaredDiameter() const
Calculates the squared diameter of a voxel at leaf depth.
double resolution_
Octree resolution.
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.
void setResolution(double resolution_arg)
Set/change the octree voxel resolution.
double getEpsilon() const
Get the search epsilon precision (error bound) for nearest neighbors searches.
shared_ptr< OctreePointCloud< PointT, LeafTWrap, BranchTWrap, OctreeBase< LeafTWrap, BranchTWrap > > > Ptr