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