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