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