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