Point Cloud Library (PCL)  1.7.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  */
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 (OctreeIteratorBase<OctreeT>& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt)
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
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. */
544 
545  /** \brief A pointer to the vector of point indices to use. */
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