Point Cloud Library (PCL)  1.15.0-dev
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 #pragma once
41 
42 #include <memory>
43 #include <mutex>
44 #include <random>
45 #include <list>
46 
47 #include <pcl/common/io.h>
48 #include <pcl/PCLPointCloud2.h>
49 
50 #include <pcl/outofcore/octree_base.h>
51 #include <pcl/outofcore/octree_disk_container.h>
52 #include <pcl/outofcore/outofcore_node_data.h>
53 
54 #include <pcl/octree/octree_nodes.h>
55 
56 namespace pcl
57 {
58  namespace outofcore
59  {
60  // Forward Declarations
61  template<typename ContainerT, typename PointT>
62  class OutofcoreOctreeBaseNode;
63 
64  template<typename ContainerT, typename PointT>
65  class OutofcoreOctreeBase;
66 
67  /** \brief Non-class function which creates a single child leaf; used with \ref queryBBIntersects_noload to avoid loading the data from disk */
68  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
69  makenode_norec (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
70 
71  /** \brief Non-class method which performs a bounding box query without loading any of the point cloud data from disk */
72  template<typename ContainerT, typename PointT> void
73  queryBBIntersects_noload (const boost::filesystem::path &root_node, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list<std::string> &bin_name);
74 
75  /** \brief Non-class method overload */
76  template<typename ContainerT, typename PointT> void
77  queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d&, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list<std::string> &bin_name);
78 
79  /** \class OutofcoreOctreeBaseNode
80  *
81  * \note Code was adapted from the Urban Robotics out of core octree implementation.
82  * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions.
83  * http://www.urbanrobotics.net/
84  *
85  * \brief OutofcoreOctreeBaseNode Class internally representing nodes of an
86  * outofcore octree, with accessors to its data via the \ref
87  * pcl::outofcore::OutofcoreOctreeDiskContainer class or \ref pcl::outofcore::OutofcoreOctreeRamContainer class,
88  * whichever it is templated against.
89  *
90  * \ingroup outofcore
91  * \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
92  *
93  */
94  template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ>
96  {
97  friend class OutofcoreOctreeBase<ContainerT, PointT> ;
98 
99  //these methods can be rewritten with the iterators.
101  makenode_norec<ContainerT, PointT> (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
102 
103  friend void
104  queryBBIntersects_noload<ContainerT, PointT> (const boost::filesystem::path &rootnode, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list<std::string> &bin_name);
105 
106  friend void
107  queryBBIntersects_noload<ContainerT, PointT> (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list<std::string> &bin_name);
108 
109  public:
112 
113  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
114 
116 
117  const static std::string node_index_basename;
118  const static std::string node_container_basename;
119  const static std::string node_index_extension;
120  const static std::string node_container_extension;
121  const static double sample_percent_;
122 
123  /** \brief Empty constructor; sets pointers for children and for bounding boxes to 0
124  */
126 
127  /** \brief Create root node and directory */
128  OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &root_name);
129 
130  /** \brief Will recursively delete all children calling recFreeChildrein */
131 
132  ~OutofcoreOctreeBaseNode () override;
133 
134  //query
135  /** \brief gets the minimum and maximum corner of the bounding box represented by this node
136  * \param[out] min_bb returns the minimum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z
137  * \param[out] max_bb returns the maximum corner of the bounding box indexed by 0-->X, 1-->Y, 2-->Z
138  */
139  virtual inline void
140  getBoundingBox (Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
141  {
142  node_metadata_->getBoundingBox (min_bb, max_bb);
143  }
144 
145 
146  const boost::filesystem::path&
147  getPCDFilename () const
148  {
149  return node_metadata_->getPCDFilename ();
150  }
151 
152  const boost::filesystem::path&
154  {
155  return node_metadata_->getMetadataFilename ();
156  }
157 
158  void
159  queryFrustum (const double planes[24], std::list<std::string>& file_names);
160 
161  void
162  queryFrustum (const double planes[24], std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check = false);
163 
164  /** \warning This function is only available if the visualization module is available and the preprocessor symbol `PCL_VISUALIZATION_AVAILABLE` is defined.
165  */
166 #ifdef PCL_VISUALIZATION_AVAILABLE
167  void
168  queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check = false);
169 #else
170  void
171  queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check = false) = delete;
172 #endif
173 
174  //point extraction
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 destion of points returned by the queries
181  */
182  virtual void
183  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst);
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 the maximum depth to query in the octree for points within the bounding box
190  * \param[out] dst_blob destion of points returned by the queries
191  */
192  virtual void
193  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob);
194 
195  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
196  *
197  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
198  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
199  * \param[in] query_depth
200  * \param percent
201  * \param[out] v std::list of points returned by the query
202  */
203  virtual void
204  queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v);
205 
206  virtual void
207  queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent = 1.0);
208 
209  /** \brief Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_depth only).
210  */
211  virtual void
212  queryBBIntersects (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::uint32_t query_depth, std::list<std::string> &file_names);
213 
214  /** \brief Write the voxel size to stdout at \c query_depth
215  * \param[in] query_depth The depth at which to print the size of the voxel/bounding boxes
216  */
217  virtual void
218  printBoundingBox (const std::size_t query_depth) const;
219 
220  /** \brief add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point
221  * \param[in] p vector of points to add to the leaf
222  * \param[in] skip_bb_check whether to check if the point's coordinates fall within the bounding box
223  */
224  virtual std::uint64_t
225  addDataToLeaf (const AlignedPointTVector &p, const bool skip_bb_check = true);
226 
227  virtual std::uint64_t
228  addDataToLeaf (const std::vector<const PointT*> &p, const bool skip_bb_check = true);
229 
230  /** \brief Add a single PCLPointCloud2 object into the octree.
231  *
232  * \param[in] input_cloud
233  * \param[in] skip_bb_check (default = false)
234  */
235  virtual std::uint64_t
236  addPointCloud (const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);
237 
238  /** \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. */
239  virtual std::uint64_t
240  addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud); //, const bool skip_bb_check);
241 
242  /** \brief Recursively add points to the leaf and children subsampling LODs
243  * on the way down.
244  *
245  * \note rng_mutex_ lock occurs
246  */
247  virtual std::uint64_t
248  addDataToLeaf_and_genLOD (const AlignedPointTVector &p, const bool skip_bb_check);
249 
250  /** \brief Write a python visual script to @b file
251  * \param[in] file output file stream to write the python visual script
252  */
253  void
254  writeVPythonVisual (std::ofstream &file);
255 
256  virtual int
257  read (pcl::PCLPointCloud2::Ptr &output_cloud);
258 
259  inline node_type_t
260  getNodeType () const override
261  {
262  if(this->getNumChildren () > 0)
263  {
264  return (pcl::octree::BRANCH_NODE);
265  }
266  return (pcl::octree::LEAF_NODE);
267  }
268 
269 
271  deepCopy () const override
272  {
273  OutofcoreOctreeBaseNode* res = nullptr;
274  PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
275  return (res);
276  }
277 
278  virtual inline std::size_t
279  getDepth () const
280  {
281  return (this->depth_);
282  }
283 
284  /** \brief Returns the total number of children on disk */
285  virtual std::size_t
286  getNumChildren () const
287  {
288  std::size_t res = this->countNumChildren ();
289  return (res);
290  }
291 
292  /** \brief Count loaded children */
293  virtual std::size_t
295  {
296  std::size_t res = this->countNumLoadedChildren ();
297  return (res);
298  }
299 
300  /** \brief Returns a pointer to the child in octant index_arg */
301  virtual OutofcoreOctreeBaseNode*
302  getChildPtr (std::size_t index_arg) const;
303 
304  /** \brief Gets the number of points available in the PCD file */
305  virtual std::uint64_t
306  getDataSize () const;
307 
308  inline virtual void
310  {
311  //clears write cache and removes PCD file from disk
312  this->payload_->clear ();
313  }
314 
315  ///////////////////////////////////////////////////////////////////////////////
316  // PROTECTED METHODS
317  ////////////////////////////////////////////////////////////////////////////////
318  protected:
319  /** \brief Load from disk
320  * If creating root, path is full name. If creating any other
321  * node, path is dir; throws exception if directory or metadata not found
322  *
323  * \param[in] directory_path pathname
324  * \param[in] super
325  * \param[in] load_all
326  * \throws PCLException if directory is missing
327  * \throws PCLException if node index is missing
328  */
329  OutofcoreOctreeBaseNode (const boost::filesystem::path &directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all);
330 
331  /** \brief Create root node and directory
332  *
333  * Initializes the root node and performs initial filesystem checks for the octree;
334  * throws OctreeException::OCT_BAD_PATH if root directory is an existing file
335  *
336  * \param bb_min triple of x,y,z minima for bounding box
337  * \param bb_max triple of x,y,z maxima for bounding box
338  * \param tree address of the tree data structure that will hold this initial root node
339  * \param rootname Root directory for location of on-disk octree storage; if directory
340  * doesn't exist, it is created; if "rootname" is an existing file,
341  *
342  * \throws PCLException if the specified path already exists
343  */
344  void init_root_node (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &rootname);
345 
346  /** \brief no copy construction right now */
348 
349  /** \brief Operator= is not implemented */
352 
353  /** \brief Counts the number of child directories on disk; used to update num_children_ */
354  virtual std::size_t
355  countNumChildren () const;
356 
357  /** \brief Counts the number of loaded children by testing the \c children_ array;
358  * used to update num_loaded_children_ internally
359  */
360  virtual std::size_t
361  countNumLoadedChildren () const;
362 
363  /** \brief Save node's metadata to file
364  * \param[in] recursive if false, save only this node's metadata to file; if true, recursively
365  * save all children's metadata to files as well
366  */
367  void
368  saveIdx (bool recursive);
369 
370  /** \brief Randomly sample point data
371  */
372  void
373  randomSample (const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check);
374 
375  /** \brief Subdivide points to pass to child nodes */
376  void
377  subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check);
378  /** \brief Subdivide a single point into a specific child node */
379  void
380  subdividePoint (const PointT &point, std::vector< AlignedPointTVector > &c);
381 
382  /** \brief Add data to the leaf when at max depth of tree. If
383  * skip_bb_check is true, adds to the node regardless of the
384  * bounding box it represents; otherwise only adds points that
385  * fall within the bounding box
386  *
387  * \param[in] p vector of points to attempt to add to the tree
388  * \param[in] skip_bb_check if @b true, doesn't check that points
389  * are in the proper bounding box; if @b false, only adds the
390  * points that fall into the bounding box to this node
391  * \return number of points successfully added
392  */
393  std::uint64_t
394  addDataAtMaxDepth (const AlignedPointTVector &p, const bool skip_bb_check = true);
395 
396  /** \brief Add data to the leaf when at max depth of tree. If
397  * \c skip_bb_check is true, adds to the node regardless of the
398  * bounding box it represents; otherwise only adds points that
399  * fall within the bounding box
400  *
401  * \param[in] input_cloud PCLPointCloud2 points to attempt to add to the tree;
402  * \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
403  * \param[in] skip_bb_check (default true) if @b true, doesn't check that points
404  * are in the proper bounding box; if @b false, only adds the
405  * points that fall into the bounding box to this node
406  * \return number of points successfully added
407  */
408  std::uint64_t
409  addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check = true);
410 
411  /** \brief Tests whether the input bounding box intersects with the current 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 any portion of the bounding box intersects with this node's bounding box; false otherwise
415  */
416  inline bool
417  intersectsWithBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
418 
419  /** \brief Tests whether the input bounding box falls inclusively within this node's 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  * \return bool True if the input bounding box falls inclusively within the boundaries of this node's bounding box
423  **/
424  inline bool
425  inBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
426 
427  /** \brief Tests whether \c point 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] point The test point
431  */
432  bool
433  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point);
434 
435  /** \brief Tests whether \c p falls 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  * \param[in] p The point to be tested
439  **/
440  static bool
441  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p);
442 
443  /** \brief Tests whether \c x, \c y, and \c z fall within the input bounding box
444  * \param[in] min_bb The minimum corner of the input bounding box
445  * \param[in] max_bb The maximum corner of the input bounding box
446  * \param x
447  * \param y
448  * \param z
449  **/
450  static bool
451  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z);
452 
453  /** \brief Tests if specified point is within bounds of current node's bounding box */
454  inline bool
455  pointInBoundingBox (const PointT &p) const;
456 
457  /** \brief Creates child node \c idx
458  * \param[in] idx Index (0-7) of the child node
459  */
460  void
461  createChild (const std::size_t idx);
462 
463  /** \brief Write JSON metadata for this node to file */
464  void
465  saveMetadataToFile (const boost::filesystem::path &path);
466 
467  /** \brief Method which recursively free children of this node
468  */
469  void
470  recFreeChildren ();
471 
472  /** \brief Number of points in the payload */
473  inline std::uint64_t
474  size () const
475  {
476  return (payload_->size ());
477  }
478 
479  void
481 
482  /** \brief Loads the nodes metadata from the JSON file
483  */
484  void
485  loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super);
486 
487  /** \brief Recursively converts data files to ascii XZY files */
488  void
490 
491  /** \brief Private constructor used for children
492  */
493  OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
494 
495  /** \brief Copies points from this and all children into a single point container (std::list)
496  */
497  void
498  copyAllCurrentAndChildPointsRec (std::list<PointT> &v);
499 
500  void
501  copyAllCurrentAndChildPointsRec_sub (std::list<PointT> &v, const double percent);
502 
503  /** \brief Returns whether or not a node has unloaded children data */
504  bool
505  hasUnloadedChildren () const;
506 
507  /** \brief Load nodes child data creating new nodes for each */
508  virtual void
509  loadChildren (bool recursive);
510 
511  /** \brief Gets a vector of occupied voxel centers
512  * \param[out] voxel_centers
513  * \param[in] query_depth
514  */
515  void
516  getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const std::size_t query_depth);
517 
518  /** \brief Gets a vector of occupied voxel centers
519  * \param[out] voxel_centers
520  * \param[in] query_depth
521  */
522  void
523  getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth);
524 
525  /** \brief Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector;
526  * This could be overloaded with a parallelized implementation
527  */
528  void
529  sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz);
530 
531  /** \brief Enlarges the shortest two sidelengths of the
532  * bounding box to a cubic shape; operation is done in
533  * place.
534  */
535  void
536  enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max);
537 
538  /** \brief The tree we belong to */
540  /** \brief The root node of the tree we belong to */
542  /** \brief super-node */
544  /** \brief Depth in the tree, root is 0, root's children are 1, ... */
545  std::size_t depth_;
546  /** \brief The children of this node */
547  std::vector<OutofcoreOctreeBaseNode*> children_;
548 
549  /** \brief Number of children on disk. This is only changed when a new node is created */
550  std::uint64_t num_children_;
551 
552  /** \brief Number of loaded children this node has
553  *
554  * "Loaded" means child OctreeBaseNodes have been allocated,
555  * and their metadata files have been loaded into
556  * memory. num_loaded_children_ <= num_children_
557  */
558  std::uint64_t num_loaded_children_;
559 
560  /** \brief what holds the points. currently a custom class, but in theory
561  * you could use an stl container if you rewrote some of this class. I used
562  * to use deques for this... */
563  std::shared_ptr<ContainerT> payload_;
564 
565  /** \brief Random number generator mutex */
566  static std::mutex rng_mutex_;
567 
568  /** \brief Mersenne Twister: A 623-dimensionally equidistributed uniform
569  * pseudo-random number generator */
570  static std::mt19937 rng_;
571 
572  /** \brief Extension for this class to find the pcd files on disk */
573  const static std::string pcd_extension;
574 
576  };
577  }//namespace outofcore
578 }//namespace pcl
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:67
Abstract octree node class
Definition: octree_nodes.h:59
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:151
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
std::size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
virtual void getBoundingBox(Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
gets the minimum and maximum corner of the bounding box represented by this node
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node's bounding box.
static const std::string node_index_basename
virtual std::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)
Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this me...
static const std::string node_index_extension
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
~OutofcoreOctreeBaseNode() override
Will recursively delete all children calling recFreeChildrein.
const boost::filesystem::path & getPCDFilename() const
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
OutofcoreOctreeBaseNode & operator=(const OutofcoreOctreeBaseNode &rval)
Operator= is not implemented.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
virtual std::uint64_t addDataToLeaf_and_genLOD(const AlignedPointTVector &p, const bool skip_bb_check)
Recursively add points to the leaf and children subsampling LODs on the way down.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
static std::mutex rng_mutex_
Random number generator mutex.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v)
Recursively add points that fall into the queried bounding box up to the query_depth.
const boost::filesystem::path & getMetadataFilename() const
virtual std::size_t getNumChildren() const
Returns the total number of children on disk.
std::uint64_t num_children_
Number of children on disk.
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::uint32_t query_depth, std::list< std::string > &file_names)
Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_dep...
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
static bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z)
Tests whether x, y, and z fall within the input bounding box.
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
OutofcoreOctreeBaseNode * parent_
super-node
void init_root_node(const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase< ContainerT, PointT > *const tree, const boost::filesystem::path &rootname)
Create root node and directory.
std::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
std::uint64_t num_loaded_children_
Number of loaded children this node has.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
Tests whether point falls within the input bounding box.
void saveIdx(bool recursive)
Save node's metadata to file.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
void queryFrustum(const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list< std::string > &file_names, const std::uint32_t query_depth, const bool skip_vfc_check=false)=delete
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
OutofcoreOctreeBaseNode * deepCopy() const override
Pure virtual method to perform a deep copy of the octree.
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector; This co...
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
std::vector< OutofcoreOctreeBaseNode * > children_
The children of this node.
static const std::string node_container_basename
std::uint64_t size() const
Number of points in the payload.
OutofcoreOctreeBaseNode(const OutofcoreOctreeBaseNode &rval)
no copy construction right now
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
virtual std::size_t getNumLoadedChildren() const
Count loaded children.
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list)
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
virtual std::size_t getDepth() const
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded children by testing the children_ array; used to update num_loaded_childr...
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node's bounding box.
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
virtual std::uint64_t addDataToLeaf(const AlignedPointTVector &p, const bool skip_bb_check=true)
add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point
void saveMetadataToFile(const boost::filesystem::path &path)
Write JSON metadata for this node to file.
static const std::string node_container_extension
void recFreeChildren()
Method which recursively free children of this node.
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
void enlargeToCube(Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max)
Enlarges the shortest two sidelengths of the bounding box to a cubic shape; operation is done in plac...
void createChild(const std::size_t idx)
Creates child node idx.
virtual std::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
node_type_t getNodeType() const override
Pure virtual method for retrieving the type of octree node (branch or leaf)
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
shared_ptr< OutofcoreOctreeNodeMetadata > Ptr
OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
void queryBBIntersects_noload(const boost::filesystem::path &root_node, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list< std::string > &bin_name)
Non-class method which performs a bounding box query without loading any of the point cloud data from...
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.