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