Point Cloud Library (PCL)  1.11.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 #pragma once
41 
42 #include <memory>
43 #include <mutex>
44 #include <random>
45 
46 #include <pcl/common/io.h>
47 #include <pcl/PCLPointCloud2.h>
48 
49 #include <pcl/outofcore/boost.h>
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>
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 
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  void
165  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);
166 
167  //point extraction
168  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
169  *
170  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
171  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
172  * \param[in] query_depth the maximum depth to query in the octree for points within the bounding box
173  * \param[out] dst destion of points returned by the queries
174  */
175  virtual void
176  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst);
177 
178  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
179  *
180  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
181  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
182  * \param[in] query_depth the maximum depth to query in the octree for points within the bounding box
183  * \param[out] dst_blob destion of points returned by the queries
184  */
185  virtual void
186  queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob);
187 
188  /** \brief Recursively add points that fall into the queried bounding box up to the \b query_depth
189  *
190  * \param[in] min_bb the minimum corner of the bounding box, indexed by X,Y,Z coordinates
191  * \param[in] max_bb the maximum corner of the bounding box, indexed by X,Y,Z coordinates
192  * \param[in] query_depth
193  * \param percent
194  * \param[out] v std::list of points returned by the query
195  */
196  virtual void
197  queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v);
198 
199  virtual void
200  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);
201 
202  /** \brief Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_depth only).
203  */
204  virtual void
205  queryBBIntersects (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::uint32_t query_depth, std::list<std::string> &file_names);
206 
207  /** \brief Write the voxel size to stdout at \c query_depth
208  * \param[in] query_depth The depth at which to print the size of the voxel/bounding boxes
209  */
210  virtual void
211  printBoundingBox (const std::size_t query_depth) const;
212 
213  /** \brief add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point
214  * \param[in] p vector of points to add to the leaf
215  * \param[in] skip_bb_check whether to check if the point's coordinates fall within the bounding box
216  */
217  virtual std::uint64_t
218  addDataToLeaf (const AlignedPointTVector &p, const bool skip_bb_check = true);
219 
220  virtual std::uint64_t
221  addDataToLeaf (const std::vector<const PointT*> &p, const bool skip_bb_check = true);
222 
223  /** \brief Add a single PCLPointCloud2 object into the octree.
224  *
225  * \param[in] input_cloud
226  * \param[in] skip_bb_check (default = false)
227  */
228  virtual std::uint64_t
229  addPointCloud (const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);
230 
231  /** \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. */
232  virtual std::uint64_t
233  addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud); //, const bool skip_bb_check);
234 
235  /** \brief Recursively add points to the leaf and children subsampling LODs
236  * on the way down.
237  *
238  * \note rng_mutex_ lock occurs
239  */
240  virtual std::uint64_t
241  addDataToLeaf_and_genLOD (const AlignedPointTVector &p, const bool skip_bb_check);
242 
243  /** \brief Write a python visual script to @b file
244  * \param[in] file output file stream to write the python visual script
245  */
246  void
247  writeVPythonVisual (std::ofstream &file);
248 
249  virtual int
250  read (pcl::PCLPointCloud2::Ptr &output_cloud);
251 
252  inline node_type_t
253  getNodeType () const override
254  {
255  if(this->getNumChildren () > 0)
256  {
257  return (pcl::octree::BRANCH_NODE);
258  }
259  return (pcl::octree::LEAF_NODE);
260  }
261 
262 
264  deepCopy () const override
265  {
266  OutofcoreOctreeBaseNode* res = nullptr;
267  PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
268  return (res);
269  }
270 
271  virtual inline std::size_t
272  getDepth () const
273  {
274  return (this->depth_);
275  }
276 
277  /** \brief Returns the total number of children on disk */
278  virtual std::size_t
279  getNumChildren () const
280  {
281  std::size_t res = this->countNumChildren ();
282  return (res);
283  }
284 
285  /** \brief Count loaded chilren */
286  virtual std::size_t
288  {
289  std::size_t res = this->countNumLoadedChildren ();
290  return (res);
291  }
292 
293  /** \brief Returns a pointer to the child in octant index_arg */
294  virtual OutofcoreOctreeBaseNode*
295  getChildPtr (std::size_t index_arg) const;
296 
297  /** \brief Gets the number of points available in the PCD file */
298  virtual std::uint64_t
299  getDataSize () const;
300 
301  inline virtual void
303  {
304  //clears write cache and removes PCD file from disk
305  this->payload_->clear ();
306  }
307 
308  ///////////////////////////////////////////////////////////////////////////////
309  // PROTECTED METHODS
310  ////////////////////////////////////////////////////////////////////////////////
311  protected:
312  /** \brief Load from disk
313  * If creating root, path is full name. If creating any other
314  * node, path is dir; throws exception if directory or metadata not found
315  *
316  * \param[in] directory_path pathname
317  * \param[in] super
318  * \param[in] load_all
319  * \throws PCLException if directory is missing
320  * \throws PCLException if node index is missing
321  */
322  OutofcoreOctreeBaseNode (const boost::filesystem::path &directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all);
323 
324  /** \brief Create root node and directory
325  *
326  * Initializes the root node and performs initial filesystem checks for the octree;
327  * throws OctreeException::OCT_BAD_PATH if root directory is an existing file
328  *
329  * \param bb_min triple of x,y,z minima for bounding box
330  * \param bb_max triple of x,y,z maxima for bounding box
331  * \param tree address of the tree data structure that will hold this initial root node
332  * \param rootname Root directory for location of on-disk octree storage; if directory
333  * doesn't exist, it is created; if "rootname" is an existing file,
334  *
335  * \throws PCLException if the specified path already exists
336  */
337  void init_root_node (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &rootname);
338 
339  /** \brief no copy construction right now */
341 
342  /** \brief Operator= is not implemented */
344  operator= (const OutofcoreOctreeBaseNode &rval);
345 
346  /** \brief Counts the number of child directories on disk; used to update num_children_ */
347  virtual std::size_t
348  countNumChildren () const;
349 
350  /** \brief Counts the number of loaded chilren by testing the \c children_ array;
351  * used to update num_loaded_chilren_ internally
352  */
353  virtual std::size_t
354  countNumLoadedChildren () const;
355 
356  /** \brief Save node's metadata to file
357  * \param[in] recursive if false, save only this node's metadata to file; if true, recursively
358  * save all children's metadata to files as well
359  */
360  void
361  saveIdx (bool recursive);
362 
363  /** \brief Randomly sample point data
364  */
365  void
366  randomSample (const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check);
367 
368  /** \brief Subdivide points to pass to child nodes */
369  void
370  subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check);
371  /** \brief Subdivide a single point into a specific child node */
372  void
373  subdividePoint (const PointT &point, std::vector< AlignedPointTVector > &c);
374 
375  /** \brief Add data to the leaf when at max depth of tree. If
376  * skip_bb_check is true, adds to the node regardless of the
377  * bounding box it represents; otherwise only adds points that
378  * fall within the bounding box
379  *
380  * \param[in] p vector of points to attempt to add to the tree
381  * \param[in] skip_bb_check if @b true, doesn't check that points
382  * are in the proper bounding box; if @b false, only adds the
383  * points that fall into the bounding box to this node
384  * \return number of points successfully added
385  */
387  addDataAtMaxDepth (const AlignedPointTVector &p, const bool skip_bb_check = true);
388 
389  /** \brief Add data to the leaf when at max depth of tree. If
390  * \c skip_bb_check is true, adds to the node regardless of the
391  * bounding box it represents; otherwise only adds points that
392  * fall within the bounding box
393  *
394  * \param[in] input_cloud PCLPointCloud2 points to attempt to add to the tree;
395  * \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
396  * \param[in] skip_bb_check (default true) if @b true, doesn't check that points
397  * are in the proper bounding box; if @b false, only adds the
398  * points that fall into the bounding box to this node
399  * \return number of points successfully added
400  */
402  addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check = true);
403 
404  /** \brief Tests whether the input bounding box intersects with the current node's bounding box
405  * \param[in] min_bb The minimum corner of the input bounding box
406  * \param[in] max_bb The maximum corner of the input bounding box
407  * \return bool True if any portion of the bounding box intersects with this node's bounding box; false otherwise
408  */
409  inline bool
410  intersectsWithBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
411 
412  /** \brief Tests whether the input bounding box falls inclusively within this node's bounding box
413  * \param[in] min_bb The minimum corner of the input bounding box
414  * \param[in] max_bb The maximum corner of the input bounding box
415  * \return bool True if the input bounding box falls inclusively within the boundaries of this node's bounding box
416  **/
417  inline bool
418  inBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;
419 
420  /** \brief Tests whether \c point falls within the input bounding box
421  * \param[in] min_bb The minimum corner of the input bounding box
422  * \param[in] max_bb The maximum corner of the input bounding box
423  * \param[in] point The test point
424  */
425  bool
426  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point);
427 
428  /** \brief Tests whether \c p falls within the input bounding box
429  * \param[in] min_bb The minimum corner of the input bounding box
430  * \param[in] max_bb The maximum corner of the input bounding box
431  * \param[in] p The point to be tested
432  **/
433  static bool
434  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p);
435 
436  /** \brief Tests whether \c x, \c y, and \c z fall within the input bounding box
437  * \param[in] min_bb The minimum corner of the input bounding box
438  * \param[in] max_bb The maximum corner of the input bounding box
439  * \param x
440  * \param y
441  * \param z
442  **/
443  static bool
444  pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z);
445 
446  /** \brief Tests if specified point is within bounds of current node's bounding box */
447  inline bool
448  pointInBoundingBox (const PointT &p) const;
449 
450  /** \brief Creates child node \c idx
451  * \param[in] idx Index (0-7) of the child node
452  */
453  void
454  createChild (const std::size_t idx);
455 
456  /** \brief Write JSON metadata for this node to file */
457  void
458  saveMetadataToFile (const boost::filesystem::path &path);
459 
460  /** \brief Method which recursively free children of this node
461  */
462  void
463  recFreeChildren ();
464 
465  /** \brief Number of points in the payload */
466  inline std::uint64_t
467  size () const
468  {
469  return (payload_->size ());
470  }
471 
472  void
474 
475  /** \brief Loads the nodes metadata from the JSON file
476  */
477  void
478  loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super);
479 
480  /** \brief Recursively converts data files to ascii XZY files */
481  void
483 
484  /** \brief Private constructor used for children
485  */
486  OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
487 
488  /** \brief Copies points from this and all children into a single point container (std::list)
489  */
490  void
491  copyAllCurrentAndChildPointsRec (std::list<PointT> &v);
492 
493  void
494  copyAllCurrentAndChildPointsRec_sub (std::list<PointT> &v, const double percent);
495 
496  /** \brief Returns whether or not a node has unloaded children data */
497  bool
498  hasUnloadedChildren () const;
499 
500  /** \brief Load nodes child data creating new nodes for each */
501  virtual void
502  loadChildren (bool recursive);
503 
504  /** \brief Gets a vector of occupied voxel centers
505  * \param[out] voxel_centers
506  * \param[in] query_depth
507  */
508  void
509  getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const std::size_t query_depth);
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 (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth);
517 
518  /** \brief Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector;
519  * This could be overloaded with a parallelized implementation
520  */
521  void
522  sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz);
523 
524  /** \brief Enlarges the shortest two sidelengths of the
525  * bounding box to a cubic shape; operation is done in
526  * place.
527  */
528  void
529  enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max);
530 
531  /** \brief The tree we belong to */
533  /** \brief The root node of the tree we belong to */
535  /** \brief super-node */
537  /** \brief Depth in the tree, root is 0, root's children are 1, ... */
538  std::size_t depth_;
539  /** \brief The children of this node */
540  std::vector<OutofcoreOctreeBaseNode*> children_;
541 
542  /** \brief Number of children on disk. This is only changed when a new node is created */
544 
545  /** \brief Number of loaded children this node has
546  *
547  * "Loaded" means child OctreeBaseNodes have been allocated,
548  * and their metadata files have been loaded into
549  * memory. num_loaded_children_ <= num_children_
550  */
552 
553  /** \brief what holds the points. currently a custom class, but in theory
554  * you could use an stl container if you rewrote some of this class. I used
555  * to use deques for this... */
556  std::shared_ptr<ContainerT> payload_;
557 
558  /** \brief Random number generator mutex */
559  static std::mutex rng_mutex_;
560 
561  /** \brief Mersenne Twister: A 623-dimensionally equidistributed uniform
562  * pseudo-random number generator */
563  static std::mt19937 rng_;
564 
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
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
shared_ptr< OutofcoreOctreeNodeMetadata > Ptr
std::uint64_t size() const
Number of points in the payload.
virtual std::size_t getNumChildren() const
Returns the total number of children on disk.
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
virtual std::size_t getNumLoadedChildren() const
Count loaded chilren.
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list) ...
std::size_t depth_
Depth in the tree, root is 0, root&#39;s children are 1, ...
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
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.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
std::uint64_t uint64_t
Definition: types.h:60
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
const boost::filesystem::path & getPCDFilename() const
std::uint32_t uint32_t
Definition: types.h:58
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...
std::vector< OutofcoreOctreeBaseNode * > children_
The children of this node.
~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
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.
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at 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...
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.
static const std::string node_index_basename
void recFreeChildren()
Method which recursively free children of this node.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
OutofcoreOctreeBaseNode & operator=(const OutofcoreOctreeBaseNode &rval)
Operator= is not implemented.
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.
void createChild(const std::size_t idx)
Creates child node idx.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
node_type_t getNodeType() const override
Pure virtual method for receiving the type of octree node (branch or leaf)
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
std::uint64_t num_loaded_children_
Number of loaded children this node has.
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.
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
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...
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
const boost::filesystem::path & getMetadataFilename() const
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
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. ...
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector< int > > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant&#39;s vector; This co...
std::shared_ptr< ContainerT > payload_
what holds the points.
static const std::string node_index_extension
static const std::string node_container_basename
void saveIdx(bool recursive)
Save node&#39;s metadata to file.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
virtual std::size_t getDepth() const
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...
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the pcl::outofcore::OutofcoreOctreeDiskContainer class or pcl::outofcore::OutofcoreOctreeRamContainer class, whichever it is templated against.
OutofcoreOctreeBaseNode * parent_
super-node
OutofcoreOctreeBaseNode * deepCopy() const override
Pure virtual method to perform a deep copy of the octree.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
static const std::string node_container_extension
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator...
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
A point structure representing Euclidean xyz coordinates, and the RGB color.
static std::mutex rng_mutex_
Random number generator mutex.
std::uint64_t num_children_
Number of children on disk.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node&#39;s bounding box...
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:150
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node&#39;s bounding box.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
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
Abstract octree node class
Definition: octree_nodes.h:61
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 saveMetadataToFile(const boost::filesystem::path &path)
Write JSON metadata for this node to file.
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...
OutofcoreOctreeNodeMetadata::Ptr node_metadata_