Point Cloud Library (PCL)  1.7.1
octree_base_node.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012, Urban Robotics, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  */
39 
40 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_H_
41 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_H_
42 
43 #include <pcl/common/io.h>
44 #include <pcl/PCLPointCloud2.h>
45 
46 #include <pcl/outofcore/boost.h>
47 #include <pcl/outofcore/octree_base.h>
48 #include <pcl/outofcore/octree_disk_container.h>
49 #include <pcl/outofcore/outofcore_node_data.h>
50 
51 #include <pcl/octree/octree_nodes.h>
52 
53 namespace pcl
54 {
55  namespace outofcore
56  {
57  // Forward Declarations
58  template<typename ContainerT, typename PointT>
60 
61  template<typename ContainerT, typename PointT>
62  class OutofcoreOctreeBase;
63 
64  /** \brief Non-class function which creates a single child leaf; used with \ref queryBBIntersects_noload to avoid loading the data from disk */
65  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
66  makenode_norec (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
67 
68  /** \brief Non-class method which performs a bounding box query without loading any of the point cloud data from disk */
69  template<typename ContainerT, typename PointT> void
70  queryBBIntersects_noload (const boost::filesystem::path &root_node, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);
71 
72  /** \brief Non-class method overload */
73  template<typename ContainerT, typename PointT> void
74  queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d&, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);
75 
76  /** \class OutofcoreOctreeBaseNode
77  *
78  * \note Code was adapted from the Urban Robotics out of core octree implementation.
79  * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions.
80  * http://www.urbanrobotics.net/
81  *
82  * \brief OutofcoreOctreeBaseNode Class internally representing nodes of an
83  * outofcore octree, with accessors to its data via the \ref
84  * octree_disk_container class or \ref octree_ram_container class,
85  * whichever it is templated against.
86  *
87  * \ingroup outofcore
88  * \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
89  *
90  */
91  template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ>
93  {
94  friend class OutofcoreOctreeBase<ContainerT, PointT> ;
95 
96  //these methods can be rewritten with the iterators.
98  makenode_norec<ContainerT, PointT> (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
99 
100  friend void
101  queryBBIntersects_noload<ContainerT, PointT> (const boost::filesystem::path &rootnode, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);
102 
103  friend void
104  queryBBIntersects_noload<ContainerT, PointT> (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);
105 
106  public:
109 
110  typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;
111 
113 
114  const static std::string node_index_basename;
115  const static std::string node_container_basename;
116  const static std::string node_index_extension;
117  const static std::string node_container_extension;
118  const static double sample_percent_;
119 
120  /** \brief Empty constructor; sets pointers for children and for bounding boxes to 0
121  */
123 
124  /** \brief Create root node and directory */
125  OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &root_name);
126 
127  /** \brief Will recursively delete all children calling recFreeChildrein */
128  virtual
130 
131  //query
132  /** \brief gets the minimum and maximum corner of the bounding box represented by this node
133  * \param[out] minCoord returns the minimum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z
134  * \param[out] maxCoord returns the maximum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z
135  */
136  virtual inline void
137  getBoundingBox (Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
138  {
139  node_metadata_->getBoundingBox (min_bb, max_bb);
140  }
141 
142 
143  const boost::filesystem::path&
144  getPCDFilename () const
145  {
146  return node_metadata_->getPCDFilename ();
147  }
148 
149  const boost::filesystem::path&
151  {
152  return node_metadata_->getMetadataFilename ();
153  }
154 
155  void
156  queryFrustum (const double planes[24], std::list<std::string>& file_names);
157 
158  void
159  queryFrustum (const double planes[24], std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check = false);
160 
161  void
162  queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check = false);
163 
164  //point extraction
165  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
166  *
167  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
168  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
169  * \param[in] query_depth the maximum depth to query in the octree for points within the bounding box
170  * \param[out] dst destion of points returned by the queries
171  */
172  virtual void
173  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, AlignedPointTVector &dst);
174 
175  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
176  *
177  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
178  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
179  * \param[in] query_depth the maximum depth to query in the octree for points within the bounding box
180  * \param[out] dst_blob destion of points returned by the queries
181  */
182  virtual void
183  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob);
184 
185  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
186  *
187  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
188  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
189  * \param[in] query_depth
190  * \param[out] v std::list of points returned by the query
191  */
192  virtual void
193  queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::uint64_t query_depth, const double percent, AlignedPointTVector &v);
194 
195  virtual void
196  queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent = 1.0);
197 
198  /** \brief Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_depth only).
199  */
200  virtual void
201  queryBBIntersects (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const boost::uint32_t query_depth, std::list<std::string> &file_names);
202 
203  /** \brief Write the voxel size to stdout at \ref query_depth
204  * \param[in] query_depth The depth at which to print the size of the voxel/bounding boxes
205  */
206  virtual void
207  printBoundingBox (const size_t query_depth) const;
208 
209  /** \brief add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point
210  * \param[in] p vector of points to add to the leaf
211  * \param[in] skipBBCheck whether to check if the point's coordinates fall within the bounding box
212  */
213  virtual boost::uint64_t
214  addDataToLeaf (const AlignedPointTVector &p, const bool skip_bb_check = true);
215 
216  virtual boost::uint64_t
217  addDataToLeaf (const std::vector<const PointT*> &p, const bool skip_bb_check = true);
218 
219  /** \brief Add a single PCLPointCloud2 object into the octree.
220  *
221  * \param[in] input_cloud
222  * \param[in] skip_bb_check (default = false)
223  */
224  virtual boost::uint64_t
225  addPointCloud (const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);
226 
227  /** \brief Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this method of LOD construction is <b>not</b> multiresolution. Rather, there are no redundant data. */
228  virtual boost::uint64_t
229  addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud); //, const bool skip_bb_check);
230 
231  /** \brief Recursively add points to the leaf and children subsampling LODs
232  * on the way down.
233  *
234  * \note rng_mutex_ lock occurs
235  */
236  virtual boost::uint64_t
237  addDataToLeaf_and_genLOD (const AlignedPointTVector &p, const bool skip_bb_check);
238 
239  /** \brief Write a python visual script to @b file
240  * \param[in] file output file stream to write the python visual script
241  */
242  void
243  writeVPythonVisual (std::ofstream &file);
244 
245  virtual int
246  read (pcl::PCLPointCloud2::Ptr &output_cloud);
247 
248  virtual inline node_type_t
249  getNodeType () const
250  {
251  if(this->getNumChildren () > 0)
252  {
253  return (pcl::octree::BRANCH_NODE);
254  }
255  else
256  {
257  return (pcl::octree::LEAF_NODE);
258  }
259  }
260 
261  virtual
263  deepCopy () const
264  {
265  OutofcoreOctreeBaseNode* res = NULL;
266  PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
267  return (res);
268  }
269 
270  virtual inline size_t
271  getDepth () const
272  {
273  return (this->depth_);
274  }
275 
276  /** \brief Returns the total number of children on disk */
277  virtual size_t
278  getNumChildren () const
279  {
280  size_t res = this->countNumChildren ();
281  return (res);
282  }
283 
284  /** \brief Count loaded chilren */
285  virtual size_t
287  {
288  size_t res = this->countNumLoadedChildren ();
289  return (res);
290  }
291 
292  /** \brief Returns a pointer to the child in octant index_arg */
293  virtual OutofcoreOctreeBaseNode*
294  getChildPtr (size_t index_arg) const;
295 
296  /** \brief Gets the number of points available in the PCD file */
297  virtual boost::uint64_t
298  getDataSize () const;
299 
300  inline virtual void
302  {
303  //clears write cache and removes PCD file from disk
304  this->payload_->clear ();
305  }
306 
307  ///////////////////////////////////////////////////////////////////////////////
308  // PROTECTED METHODS
309  ////////////////////////////////////////////////////////////////////////////////
310  protected:
311  /** \brief Load from disk
312  * If creating root, path is full name. If creating any other
313  * node, path is dir; throws exception if directory or metadata not found
314  *
315  * \param[in] Directory pathname
316  * \param[in] super
317  * \param[in] loadAll
318  * \throws PCLException if directory is missing
319  * \throws PCLException if node index is missing
320  */
321  OutofcoreOctreeBaseNode (const boost::filesystem::path &directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all);
322 
323  /** \brief Create root node and directory
324  *
325  * Initializes the root node and performs initial filesystem checks for the octree;
326  * throws OctreeException::OCT_BAD_PATH if root directory is an existing file
327  *
328  * \param bb_min triple of x,y,z minima for bounding box
329  * \param bb_max triple of x,y,z maxima for bounding box
330  * \param tree adress of the tree data structure that will hold this initial root node
331  * \param rootname Root directory for location of on-disk octree storage; if directory
332  * doesn't exist, it is created; if "rootname" is an existing file,
333  *
334  * \throws PCLException if the specified path already exists
335  */
336  void init_root_node (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &rootname);
337 
338  /** \brief no copy construction right now */
340 
341  /** \brief Operator= is not implemented */
343  operator= (const OutofcoreOctreeBaseNode &rval);
344 
345  /** \brief Counts the number of child directories on disk; used to update num_children_ */
346  virtual size_t
347  countNumChildren () const;
348 
349  /** \brief Counts the number of loaded chilren by testing the \ref children_ array;
350  * used to update num_loaded_chilren_ internally
351  */
352  virtual size_t
353  countNumLoadedChildren () const;
354 
355  /** \brief Save node's metadata to file
356  * \param[in] recursive: if false, save only this node's metadata to file; if true, recursively
357  * save all children's metadata to files as well
358  */
359  void
360  saveIdx (bool recursive);
361 
362  /** \brief Randomly sample point data
363  */
364  void
365  randomSample (const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check);
366 
367  /** \brief Subdivide points to pass to child nodes */
368  void
369  subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check);
370  /** \brief Subdivide a single point into a specific child node */
371  void
372  subdividePoint (const PointT &point, std::vector< AlignedPointTVector > &c);
373 
374  /** \brief Add data to the leaf when at max depth of tree. If
375  * skip_bb_check is true, adds to the node regardless of the
376  * bounding box it represents; otherwise only adds points that
377  * fall within the bounding box
378  *
379  * \param[in] p vector of points to attempt to add to the tree
380  * \param[in] skip_bb_check if @b true, doesn't check that points
381  * are in the proper bounding box; if @b false, only adds the
382  * points that fall into the bounding box to this node
383  * \return number of points successfully added
384  */
385  boost::uint64_t
386  addDataAtMaxDepth (const AlignedPointTVector &p, const bool skip_bb_check = true);
387 
388  /** \brief Add data to the leaf when at max depth of tree. If
389  * \ref skip_bb_check is true, adds to the node regardless of the
390  * bounding box it represents; otherwise only adds points that
391  * fall within the bounding box
392  *
393  * \param[in] input_cloud PCLPointCloud2 points to attempt to add to the tree;
394  * \warning PCLPointCloud2 inserted into the tree must have x,y,z fields, and must be of same type of any other points inserted in the tree
395  * \param[in] skip_bb_check (default true) if @b true, doesn't check that points
396  * are in the proper bounding box; if @b false, only adds the
397  * points that fall into the bounding box to this node
398  * \return number of points successfully added
399  */
400  boost::uint64_t
401  addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check = true);
402 
403  /** \brief Tests whether the input bounding box intersects with the current node's bounding box
404  * \param[in] min_bb The minimum corner of the input bounding box
405  * \param[in] min_bb The maximum corner of the input bounding box
406  * \return bool True if any portion of the bounding box intersects with this node's bounding box; false otherwise
407  */
408  inline bool
409  intersectsWithBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
410 
411  /** \brief Tests whether the input bounding box falls inclusively within this node's bounding box
412  * \param[in] min_bb The minimum corner of the input bounding box
413  * \param[in] max_bb The maximum corner of the input bounding box
414  * \return bool True if the input bounding box falls inclusively within the boundaries of this node's bounding box
415  **/
416  inline bool
417  inBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
418 
419  /** \brief Tests whether \ref point falls within the input bounding box
420  * \param[in] min_bb The minimum corner of the input bounding box
421  * \param[in] max_bb The maximum corner of the input bounding box
422  * \param[in] point The test point
423  */
424  bool
425  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point);
426 
427  /** \brief Tests whether \ref p falls within the input bounding box
428  * \param[in] min_bb The minimum corner of the input bounding box
429  * \param[in] max_bb The maximum corner of the input bounding box
430  * \param[in] p The point to be tested
431  **/
432  static bool
433  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p);
434 
435  /** \brief Tests whether \ref x, \ref y, and \ref z fall within the input bounding box
436  * \param[in] min_bb The minimum corner of the input bounding box
437  * \param[in] max_bb The maximum corner of the input bounding box
438  **/
439  static bool
440  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z);
441 
442  /** \brief Tests if specified point is within bounds of current node's bounding box */
443  inline bool
444  pointInBoundingBox (const PointT &p) const;
445 
446  /** \brief Creates child node \ref idx
447  * \param[in] idx Index (0-7) of the child node
448  */
449  void
450  createChild (const std::size_t idx);
451 
452  /** \brief Write JSON metadata for this node to file */
453  void
454  saveMetadataToFile (const boost::filesystem::path &path);
455 
456  /** \brief Method which recursively free children of this node
457  */
458  void
459  recFreeChildren ();
460 
461  /** \brief Number of points in the payload */
462  inline boost::uint64_t
463  size () const
464  {
465  return (payload_->size ());
466  }
467 
468  void
470 
471  /** \brief Loads the nodes metadata from the JSON file
472  */
473  void
474  loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super);
475 
476  /** \brief Recursively converts data files to ascii XZY files
477  * \note This will be deprecated soon
478  */
479  void
481 
482  /** \brief Private constructor used for children
483  */
484  OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
485 
486  /** \brief Copies points from this and all children into a single point container (std::list)
487  */
488  void
489  copyAllCurrentAndChildPointsRec (std::list<PointT> &v);
490 
491  void
492  copyAllCurrentAndChildPointsRec_sub (std::list<PointT> &v, const double percent);
493 
494  /** \brief Returns whether or not a node has unloaded children data */
495  bool
496  hasUnloadedChildren () const;
497 
498  /** \brief Load nodes child data creating new nodes for each */
499  virtual void
500  loadChildren (bool recursive);
501 
502  /** \brief Gets a vector of occupied voxel centers
503  * \param[out] voxel_centers
504  * \param[in] query_depth
505  */
506  void
507  getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const size_t query_depth);
508 
509  /** \brief Gets a vector of occupied voxel centers
510  * \param[out] voxel_centers
511  * \param[in] query_depth
512  */
513  void
514  getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const size_t query_depth);
515 
516  /** \brief Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector;
517  * This could be overloaded with a parallelized implementation
518  */
519  void
520  sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz);
521 
522  /** \brief Enlarges the shortest two sidelengths of the
523  * bounding box to a cubic shape; operation is done in
524  * place.
525  */
526  void
527  enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max);
528 
529  /** \brief The tree we belong to */
531  /** \brief The root node of the tree we belong to */
533  /** \brief super-node */
535  /** \brief Depth in the tree, root is 0, root's children are 1, ... */
536  size_t depth_;
537  /** \brief The children of this node */
538  std::vector<OutofcoreOctreeBaseNode*> children_;
539 
540  /** \brief Number of children on disk. This is only changed when a new node is created */
541  uint64_t num_children_;
542 
543  /** \brief Number of loaded children this node has
544  *
545  * "Loaded" means child OctreeBaseNodes have been allocated,
546  * and their metadata files have been loaded into
547  * memory. num_loaded_children_ <= num_children_
548  */
550 
551  /** \brief what holds the points. currently a custom class, but in theory
552  * you could use an stl container if you rewrote some of this class. I used
553  * to use deques for this... */
554  boost::shared_ptr<ContainerT> payload_;
555 
556  /** \brief Random number generator mutex */
557  static boost::mutex rng_mutex_;
558 
559  /** \brief Mersenne Twister: A 623-dimensionally equidistributed uniform
560  * pseudo-random number generator */
561  static boost::mt19937 rand_gen_;
562 
563  /** \brief Random number generator seed */
564  const static boost::uint32_t rngseed = 0xAABBCCDD;
565  /** \brief Extension for this class to find the pcd files on disk */
566  const static std::string pcd_extension;
567 
569  };
570  }//namespace outofcore
571 }//namespace pcl
572 
573 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_H_