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