Point Cloud Library (PCL)  1.14.1-dev
octree_base.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 <pcl/common/io.h>
43 
44 //outofcore classes
45 #include <pcl/outofcore/octree_base_node.h>
46 #include <pcl/outofcore/octree_disk_container.h>
47 #include <pcl/outofcore/octree_ram_container.h>
48 
49 //outofcore iterators
50 #include <pcl/outofcore/outofcore_iterator_base.h>
51 #include <pcl/outofcore/outofcore_breadth_first_iterator.h>
52 #include <pcl/outofcore/outofcore_depth_first_iterator.h>
53 #include <pcl/outofcore/impl/outofcore_breadth_first_iterator.hpp>
54 #include <pcl/outofcore/impl/outofcore_depth_first_iterator.hpp>
55 
56 //outofcore metadata
57 #include <pcl/outofcore/metadata.h>
58 #include <pcl/outofcore/outofcore_base_data.h>
59 
60 #include <pcl/filters/filter.h>
61 #include <pcl/filters/random_sample.h>
62 
63 #include <pcl/PCLPointCloud2.h>
64 
65 #include <shared_mutex>
66 #include <list>
67 
68 namespace pcl
69 {
70  namespace outofcore
71  {
73  {
74  std::string node_index_basename_;
76  std::string node_index_extension_;
79  };
80 
81  /** \class OutofcoreOctreeBase
82  * \brief This code defines the octree used for point storage at Urban Robotics.
83  *
84  * \note Code was adapted from the Urban Robotics out of core octree implementation.
85  * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions.
86  * http://www.urbanrobotics.net/. This code was integrated for the Urban Robotics
87  * Code Sprint (URCS) by Stephen Fox (foxstephend@gmail.com). Additional development notes can be found at
88  * http://www.pointclouds.org/blog/urcs/.
89  *
90  * The primary purpose of this class is an interface to the
91  * recursive traversal (recursion handled by \ref pcl::outofcore::OutofcoreOctreeBaseNode) of the
92  * in-memory/top-level octree structure. The metadata in each node
93  * can be loaded entirely into main memory, from which the tree can be traversed
94  * recursively in this state. This class provides an the interface
95  * for:
96  * -# Point/Region insertion methods
97  * -# Frustum/box/region queries
98  * -# Parameterization of resolution, container type, etc...
99  *
100  * For lower-level node access, there is a Depth-First iterator
101  * for traversing the trees with direct access to the nodes. This
102  * can be used for implementing other algorithms, and other
103  * iterators can be written in a similar fashion.
104  *
105  * The format of the octree is stored on disk in a hierarchical
106  * octree structure, where .oct_idx are the JSON-based node
107  * metadata files managed by \ref pcl::outofcore::OutofcoreOctreeNodeMetadata,
108  * and .octree is the JSON-based octree metadata file managed by
109  * \ref pcl::outofcore::OutofcoreOctreeBaseMetadata. Children of each node live
110  * in up to eight subdirectories named from 0 to 7, where a
111  * metadata and optionally a pcd file will exist. The PCD files
112  * are stored in compressed binary PCD format, containing all of
113  * the fields existing in the PCLPointCloud2 objects originally
114  * inserted into the out of core object.
115  *
116  * A brief outline of the out of core octree can be seen
117  * below. The files in [brackets] exist only when the LOD are
118  * built.
119  *
120  * At this point in time, there is not support for multiple trees
121  * existing in a single directory hierarchy.
122  *
123  * \verbatim
124  tree_name/
125  tree_name.oct_idx
126  tree_name.octree
127  [tree_name-uuid.pcd]
128  0/
129  tree_name.oct_idx
130  [tree_name-uuid.pcd]
131  0/
132  ...
133  1/
134  ...
135  ...
136  0/
137  tree_name.oct_idx
138  tree_name.pcd
139  1/
140  ...
141  7/
142  \endverbatim
143  *
144  * \ingroup outofcore
145  * \author Jacob Schloss (jacob.schloss@urbanrobotics.net)
146  * \author Stephen Fox, Urban Robotics Code Sprint (foxstephend@gmail.com)
147  *
148  */
149  template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ>
151  {
152  friend class OutofcoreOctreeBaseNode<ContainerT, PointT>;
153  friend class pcl::outofcore::OutofcoreIteratorBase<PointT, ContainerT>;
154 
155  public:
156 
157  // public typedefs
160 
163 
165 
168 
171 
174 
177 
178  using Ptr = shared_ptr<OutofcoreOctreeBase<ContainerT, PointT> >;
179  using ConstPtr = shared_ptr<const OutofcoreOctreeBase<ContainerT, PointT> >;
180 
182 
183  using IndicesPtr = shared_ptr<pcl::Indices>;
184  using IndicesConstPtr = shared_ptr<const pcl::Indices>;
185 
186  using PointCloudPtr = typename PointCloud::Ptr;
188 
189  using AlignedPointTVector = std::vector<PointT, Eigen::aligned_allocator<PointT> >;
190 
191  // Constructors
192  // -----------------------------------------------------------------------
193 
194  /** \brief Load an existing tree
195  *
196  * If load_all is set, the BB and point count for every node is loaded,
197  * otherwise only the root node is actually created, and the rest will be
198  * generated on insertion or query.
199  *
200  * \param root_node_name Path to the top-level tree/tree.oct_idx metadata file
201  * \param load_all Load entire tree metadata (does not load any points from disk)
202  * \throws PCLException for bad extension (root node metadata must be .oct_idx extension)
203  */
204  OutofcoreOctreeBase (const boost::filesystem::path &root_node_name, const bool load_all);
205 
206  /** \brief Create a new tree
207  *
208  * Create a new tree rootname with specified bounding box; will remove and overwrite existing tree with the same name
209  *
210  * Computes the depth of the tree based on desired leaf , then calls the other constructor.
211  *
212  * \param min Bounding box min
213  * \param max Bounding box max
214  * \param resolution_arg Node dimension in meters (assuming your point data is in meters)
215  * \param root_node_name must end in ".oct_idx"
216  * \param coord_sys Coordinate system which is stored in the JSON metadata
217  * \throws PCLException if root file extension does not match \ref pcl::outofcore::OutofcoreOctreeBaseNode::node_index_extension
218  */
219  OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path &root_node_name, const std::string &coord_sys);
220 
221  /** \brief Create a new tree; will not overwrite existing tree of same name
222  *
223  * Create a new tree rootname with specified bounding box; will not overwrite an existing tree
224  *
225  * \param max_depth Specifies a fixed number of LODs to generate, which is the depth of the tree
226  * \param min Bounding box min
227  * \param max Bounding box max
228  * \note Bounding box of the tree must be set before inserting any points. The tree \b cannot be resized at this time.
229  * \param root_node_name must end in ".oct_idx"
230  * \param coord_sys Coordinate system which is stored in the JSON metadata
231  * \throws PCLException if the parent directory has existing children (detects an existing tree)
232  * \throws PCLException if file extension is not ".oct_idx"
233  */
234  OutofcoreOctreeBase (const std::uint64_t max_depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_node_name, const std::string &coord_sys);
235 
236  virtual
238 
239  // Point/Region INSERTION methods
240  // --------------------------------------------------------------------------------
241  /** \brief Recursively add points to the tree
242  * \note shared read_write_mutex lock occurs
243  */
244  std::uint64_t
246 
247  /** \brief Copies the points from the point_cloud falling within the bounding box of the octree to the
248  * out-of-core octree; this is an interface to addDataToLeaf and can be used multiple times.
249  * \param point_cloud Pointer to the point cloud data to copy to the outofcore octree; Assumes templated
250  * PointT matches for each.
251  * \return Number of points successfully copied from the point cloud to the octree.
252  */
253  std::uint64_t
254  addPointCloud (PointCloudConstPtr point_cloud);
255 
256  /** \brief Recursively copies points from input_cloud into the leaf nodes of the out-of-core octree, and stores them to disk.
257  *
258  * \param[in] input_cloud The cloud of points to be inserted into the out-of-core octree. Note if multiple PCLPointCloud2 objects are added to the tree, this assumes that they all have exactly the same fields.
259  * \param[in] skip_bb_check (default=false) whether to skip the bounding box check on insertion. Note the bounding box check is never skipped in the current implementation.
260  * \return Number of points successfully copied from the point cloud to the octree
261  */
262  std::uint64_t
263  addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);
264 
265  /** \brief Recursively add points to the tree.
266  *
267  * Recursively add points to the tree. 1/8 of the remaining
268  * points at each LOD are stored at each internal node of the
269  * octree until either (a) runs out of points, in which case
270  * the leaf is not at the maximum depth of the tree, or (b)
271  * a larger set of points falls in the leaf at the maximum depth.
272  * Note unlike the old implementation, multiple
273  * copies of the same point will \b not be added at multiple
274  * LODs as it walks the tree. Once the point is added to the
275  * octree, it is no longer propagated further down the tree.
276  *
277  *\param[in] input_cloud The input cloud of points which will
278  * be copied into the sorted nodes of the out-of-core octree
279  * \return The total number of points added to the out-of-core
280  * octree.
281  */
282  std::uint64_t
284 
285  std::uint64_t
287 
288  std::uint64_t
290 
291  /** \brief Recursively add points to the tree subsampling LODs on the way.
292  *
293  * shared read_write_mutex lock occurs
294  */
295  std::uint64_t
297 
298  // Frustum/Box/Region REQUESTS/QUERIES: DB Accessors
299  // -----------------------------------------------------------------------
300  void
301  queryFrustum (const double *planes, std::list<std::string>& file_names) const;
302 
303  void
304  queryFrustum (const double *planes, std::list<std::string>& file_names, const std::uint32_t query_depth) const;
305 
306  void
307  queryFrustum (const double *planes, const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix,
308  std::list<std::string>& file_names, const std::uint32_t query_depth) const;
309 
310  //--------------------------------------------------------------------------------
311  //templated PointT methods
312  //--------------------------------------------------------------------------------
313 
314  /** \brief Get a list of file paths at query_depth that intersect with your bounding box specified by \c min and \c max.
315  * When querying with this method, you may be stuck with extra data (some outside of your query bounds) that reside in the files.
316  *
317  * \param[in] min The minimum corner of the bounding box
318  * \param[in] max The maximum corner of the bounding box
319  * \param[in] query_depth 0 is root, (this->depth) is full
320  * \param[out] bin_name List of paths to point data files (PCD currently) which satisfy the query
321  */
322  void
323  queryBBIntersects (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list<std::string> &bin_name) const;
324 
325  /** \brief Get Points in BB, only points inside BB. The query
326  * processes the data at each node, filtering points that fall
327  * out of the query bounds, and returns a single, concatenated
328  * point cloud.
329  *
330  * \param[in] min The minimum corner of the bounding box for querying
331  * \param[in] max The maximum corner of the bounding box for querying
332  * \param[in] query_depth The depth from which point data will be taken
333  * \note If the LODs of the tree have not been built, you must specify the maximum depth in order to retrieve any data
334  * \param[out] dst The destination vector of points
335  */
336  void
337  queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint64_t query_depth, AlignedPointTVector &dst) const;
338 
339  /** \brief Query all points falling within the input bounding box at \c query_depth and return a PCLPointCloud2 object in \c dst_blob.
340  *
341  * \param[in] min The minimum corner of the input bounding box.
342  * \param[in] max The maximum corner of the input bounding box.
343  * \param[in] query_depth The query depth at which to search for points; only points at this depth are returned
344  * \param[out] dst_blob Storage location for the points satisfying the query.
345  **/
346  void
347  queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob) const;
348 
349  /** \brief Returns a random subsample of points within the given bounding box at \c query_depth.
350  *
351  * \param[in] min The minimum corner of the bounding box to query.
352  * \param[out] max The maximum corner of the bounding box to query.
353  * \param[in] query_depth The depth in the tree at which to look for the points. Only returns points within the given bounding box at the specified \c query_depth.
354  * \param percent
355  * \param[out] dst The destination in which to return the points.
356  *
357  */
358  void
359  queryBBIncludes_subsample (const Eigen::Vector3d &min, const Eigen::Vector3d &max, std::uint64_t query_depth, const double percent, AlignedPointTVector &dst) const;
360 
361  //--------------------------------------------------------------------------------
362  //PCLPointCloud2 methods
363  //--------------------------------------------------------------------------------
364 
365  /** \brief Query all points falling within the input bounding box at \c query_depth and return a PCLPointCloud2 object in \c dst_blob.
366  * If the optional argument for filter is given, points are processed by that filter before returning.
367  * \param[in] min The minimum corner of the input bounding box.
368  * \param[in] max The maximum corner of the input bounding box.
369  * \param[in] query_depth The depth of tree at which to query; only points at this depth are returned
370  * \param[out] dst_blob The destination in which points within the bounding box are stored.
371  * \param[in] percent optional sampling percentage which is applied after each time data are read from disk
372  */
373  virtual void
374  queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent = 1.0);
375 
376  /** \brief Returns list of pcd files from nodes whose bounding boxes intersect with the input bounding box.
377  * \param[in] min The minimum corner of the input bounding box.
378  * \param[in] max The maximum corner of the input bounding box.
379  * \param query_depth
380  * \param[out] filenames The list of paths to the PCD files which can be loaded and processed.
381  */
382  inline virtual void
383  queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, std::list<std::string> &filenames) const
384  {
385  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
386  filenames.clear ();
387  this->root_node_->queryBBIntersects (min, max, query_depth, filenames);
388  }
389 
390  // Parameterization: getters and setters
391  // --------------------------------------------------------------------------------
392 
393  /** \brief Get the overall bounding box of the outofcore
394  * octree; this is the same as the bounding box of the \c root_node_ node
395  * \param min
396  * \param max
397  */
398  bool
399  getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const;
400 
401  /** \brief Get number of points at specified LOD
402  * \param[in] depth_index the level of detail at which we want the number of points (0 is root, 1, 2,...)
403  * \return number of points in the tree at \b depth
404  */
405  inline std::uint64_t
406  getNumPointsAtDepth (const std::uint64_t& depth_index) const
407  {
408  return (metadata_->getLODPoints (depth_index));
409  }
410 
411  /** \brief Queries the number of points in a bounding box
412  *
413  * \param[in] min The minimum corner of the input bounding box
414  * \param[out] max The maximum corner of the input bounding box
415  * \param[in] query_depth The depth of the nodes to restrict the search to (only this depth is searched)
416  * \param[in] load_from_disk (default true) Whether to load PCD files to count exactly the number of points within the bounding box; setting this to false will return an upper bound by just reading the number of points from the PCD header, even if there may be some points in that node do not fall within the query bounding box.
417  * \return Number of points in the bounding box at depth \b query_depth
418  **/
419  std::uint64_t
420  queryBoundingBoxNumPoints (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const int query_depth, bool load_from_disk = true);
421 
422 
423  /** \brief Get number of points at each LOD
424  * \return vector of number of points in each LOD indexed by each level of depth, 0 to the depth of the tree.
425  */
426  inline const std::vector<std::uint64_t>&
428  {
429  return (metadata_->getLODPoints ());
430  }
431 
432  /** \brief Get number of LODs, which is the height of the tree
433  */
434  inline std::uint64_t
435  getDepth () const
436  {
437  return (metadata_->getDepth ());
438  }
439 
440  inline std::uint64_t
441  getTreeDepth () const
442  {
443  return (this->getDepth ());
444  }
445 
446  /** \brief Computes the expected voxel dimensions at the leaves
447  */
448  bool
449  getBinDimension (double &x, double &y) const;
450 
451  /** \brief gets the side length of an (assumed) perfect cubic voxel.
452  * \note If the initial bounding box specified in constructing the octree is not square, then this method does not return a sensible value
453  * \return the side length of the cubic voxel size at the specified depth
454  */
455  double
456  getVoxelSideLength (const std::uint64_t& depth) const;
457 
458  /** \brief Gets the smallest (assumed) cubic voxel side lengths. The smallest voxels are located at the max depth of the tree.
459  * \return The side length of a the cubic voxel located at the leaves
460  */
461  double
463  {
464  return (this->getVoxelSideLength (metadata_->getDepth ()));
465  }
466 
467  /** \brief Get coordinate system tag from the JSON metadata file
468  */
469  const std::string&
470  getCoordSystem () const
471  {
472  return (metadata_->getCoordinateSystem ());
473  }
474 
475  // Mutators
476  // -----------------------------------------------------------------------
477 
478  /** \brief Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs below the node.
479  */
480  void
481  buildLOD ();
482 
483  /** \brief Prints size of BBox to stdout
484  */
485  void
486  printBoundingBox (const std::size_t query_depth) const;
487 
488  /** \brief Prints the coordinates of the bounding box of the node to stdout */
489  void
491 
492  /** \brief Prints size of the bounding boxes to stdou
493  */
494  inline void
496  {
497  this->printBoundingBox (metadata_->getDepth ());
498  }
499 
500  /** \brief Returns the voxel centers of all existing voxels at \c query_depth
501  \param[out] voxel_centers Vector of PointXYZ voxel centers for nodes that exist at that depth
502  \param[in] query_depth the depth of the tree at which to retrieve occupied/existing voxels
503  */
504  void
505  getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, std::size_t query_depth) const;
506 
507  /** \brief Returns the voxel centers of all existing voxels at \c query_depth
508  \param[out] voxel_centers Vector of PointXYZ voxel centers for nodes that exist at that depth
509  \param[in] query_depth the depth of the tree at which to retrieve occupied/existing voxels
510  */
511  void
512  getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, std::size_t query_depth) const;
513 
514  /** \brief Gets the voxel centers of all occupied/existing leaves of the tree */
515  void
517  {
518  getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ());
519  }
520 
521  /** \brief Returns the voxel centers of all occupied/existing leaves of the tree
522  * \param[out] voxel_centers std::vector of the centers of all occupied leaves of the octree
523  */
524  void
525  getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers) const
526  {
527  getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ());
528  }
529 
530  // Serializers
531  // -----------------------------------------------------------------------
532 
533  /** \brief Save each .bin file as an XYZ file */
534  void
535  convertToXYZ ();
536 
537  /** \brief Write a python script using the vpython module containing all
538  * the bounding boxes */
539  void
540  writeVPythonVisual (const boost::filesystem::path& filename);
541 
543  getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const;
544 
546  getLODFilter ();
547 
549  getLODFilter () const;
550 
551  /** \brief Sets the filter to use when building the levels of depth. Recommended filters are pcl::RandomSample<pcl::PCLPointCloud2> or pcl::VoxelGrid */
552  void
554 
555  /** \brief Returns the sample_percent_ used when constructing the LOD. */
556  double
558  {
559  return (sample_percent_);
560  }
561 
562  /** \brief Sets the sampling percent for constructing LODs. Each LOD gets sample_percent^d points.
563  * \param[in] sample_percent_arg Percentage between 0 and 1. */
564  inline void
565  setSamplePercent (const double sample_percent_arg)
566  {
567  this->sample_percent_ = std::fabs (sample_percent_arg) > 1.0 ? 1.0 : std::fabs (sample_percent_arg);
568  }
569 
570  protected:
571  void
572  init (const std::uint64_t& depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_name, const std::string& coord_sys);
573 
575 
577 
580 
583 
584  inline OutofcoreNodeType*
586  {
587  return (this->root_node_);
588  }
589 
590  /** \brief flush empty nodes only */
591  void
593 
594  /** \brief Write octree definition ".octree" (defined by octree_extension_) to disk */
595  void
596  saveToFile ();
597 
598  /** \brief recursive portion of lod builder */
599  void
600  buildLODRecursive (const std::vector<BranchNode*>& current_branch);
601 
602  /** \brief Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in OutofcoreOctreeBaseNode
603  */
604  inline void
605  incrementPointsInLOD (std::uint64_t depth, std::uint64_t inc);
606 
607  /** \brief Auxiliary function to validate path_name extension is .octree
608  *
609  * \return 0 if bad; 1 if extension is .oct_idx
610  */
611  bool
612  checkExtension (const boost::filesystem::path& path_name);
613 
614  /** \brief Flush all nodes' cache */
615  void
616  flushToDisk ();
617 
618  /** \brief Flush all non leaf nodes' cache */
619  void
620  flushToDiskLazy ();
621 
622  /** \brief Flush empty nodes only */
623  void
625 
626  /** \brief Pointer to the root node of the octree data structure */
628 
629  /** \brief shared mutex for controlling read/write access to disk */
630  mutable std::shared_timed_mutex read_write_mutex_;
631 
633 
634  /** \brief defined as ".octree" to append to treepath files
635  * \note this might change
636  */
637  const static std::string TREE_EXTENSION_;
638  const static int OUTOFCORE_VERSION_;
639 
640  const static std::uint64_t LOAD_COUNT_ = static_cast<std::uint64_t>(2e9);
641 
642  private:
643 
644  /** \brief Auxiliary function to enlarge a bounding box to a cube. */
645  void
646  enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max);
647 
648  /** \brief Auxiliary function to compute the depth of the tree given the bounding box and the desired size of the leaf voxels */
649  std::uint64_t
650  calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution);
651 
652  double sample_percent_;
653 
655 
656  };
657  }
658 }
Filter represents the base filter class.
Definition: filter.h:81
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
RandomSample applies a random sampling with uniform probability.
Definition: random_sample.h:56
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:151
void DeAllocEmptyNodeCache()
Flush empty nodes only.
OutofcoreOctreeBaseMetadata::Ptr metadata_
Definition: octree_base.h:632
shared_ptr< pcl::Indices > IndicesPtr
Definition: octree_base.h:183
std::uint64_t queryBoundingBoxNumPoints(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, bool load_from_disk=true)
Queries the number of points in a bounding box.
virtual void queryBoundingBox(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, std::list< std::string > &filenames) const
Returns list of pcd files from nodes whose bounding boxes intersect with the input bounding box.
Definition: octree_base.h:383
std::uint64_t getNumPointsAtDepth(const std::uint64_t &depth_index) const
Get number of points at specified LOD.
Definition: octree_base.h:406
OutofcoreOctreeBase & operator=(OutofcoreOctreeBase &rval)
std::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
const std::vector< std::uint64_t > & getNumPointsVector() const
Get number of points at each LOD.
Definition: octree_base.h:427
void incrementPointsInLOD(std::uint64_t depth, std::uint64_t inc)
Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in Outofcore...
void queryFrustum(const double *planes, std::list< std::string > &file_names) const
std::shared_timed_mutex read_write_mutex_
shared mutex for controlling read/write access to disk
Definition: octree_base.h:630
void DeAllocEmptyNodeCache(OutofcoreNodeType *current)
flush empty nodes only
std::uint64_t addPointCloud_and_genLOD(pcl::PCLPointCloud2::Ptr &input_cloud)
Recursively add points to the tree.
void convertToXYZ()
Save each .bin file as an XYZ file.
OutofcoreOctreeBase(const boost::filesystem::path &root_node_name, const bool load_all)
Load an existing tree.
Definition: octree_base.hpp:82
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, std::size_t query_depth) const
Returns the voxel centers of all existing voxels at query_depth.
void writeVPythonVisual(const boost::filesystem::path &filename)
Write a python script using the vpython module containing all the bounding boxes.
void buildLOD()
Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs belo...
void queryBBIncludes(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint64_t query_depth, AlignedPointTVector &dst) const
Get Points in BB, only points inside BB.
const std::string & getCoordSystem() const
Get coordinate system tag from the JSON metadata file.
Definition: octree_base.h:470
OutofcoreOctreeBaseNode< ContainerT, PointT > BranchNode
Definition: octree_base.h:166
typename PointCloud::Ptr PointCloudPtr
Definition: octree_base.h:186
void queryBBIntersects(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list< std::string > &bin_name) const
Get a list of file paths at query_depth that intersect with your bounding box specified by min and ma...
std::uint64_t getDepth() const
Get number of LODs, which is the height of the tree.
Definition: octree_base.h:435
bool getBoundingBox(Eigen::Vector3d &min, Eigen::Vector3d &max) const
Get the overall bounding box of the outofcore octree; this is the same as the bounding box of the roo...
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
Definition: octree_base.h:462
void flushToDiskLazy()
Flush all non leaf nodes' cache.
virtual void queryBoundingBox(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent=1.0)
Query all points falling within the input bounding box at query_depth and return a PCLPointCloud2 obj...
std::uint64_t getTreeDepth() const
Definition: octree_base.h:441
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.
std::uint64_t addDataToLeaf_and_genLOD(AlignedPointTVector &p)
Recursively add points to the tree subsampling LODs on the way.
OutofcoreOctreeBaseNode< ContainerT, PointT > OutofcoreNodeType
Definition: octree_base.h:164
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
shared_ptr< OutofcoreOctreeBase< ContainerT, PointT > > Ptr
Definition: octree_base.h:178
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_base.h:189
shared_ptr< const OutofcoreOctreeBase< ContainerT, PointT > > ConstPtr
Definition: octree_base.h:179
void flushToDisk()
Flush all nodes' cache.
OutofcoreNodeType * getRootNode()
Definition: octree_base.h:585
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
Definition: octree_base.h:495
void buildLODRecursive(const std::vector< BranchNode * > &current_branch)
recursive portion of lod builder
static const std::string TREE_EXTENSION_
defined as ".octree" to append to treepath files
Definition: octree_base.h:637
shared_ptr< const pcl::Indices > IndicesConstPtr
Definition: octree_base.h:184
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers) const
Gets the voxel centers of all occupied/existing leaves of the tree.
Definition: octree_base.h:516
OutofcoreOctreeBase(const OutofcoreOctreeBase &rval)
void queryBBIncludes_subsample(const Eigen::Vector3d &min, const Eigen::Vector3d &max, std::uint64_t query_depth, const double percent, AlignedPointTVector &dst) const
Returns a random subsample of points within the given bounding box at query_depth.
void printBoundingBox(OutofcoreNodeType &node) const
Prints the coordinates of the bounding box of the node to stdout.
std::uint64_t addPointCloud(PointCloudConstPtr point_cloud)
Copies the points from the point_cloud falling within the bounding box of the octree to the out-of-co...
void init(const std::uint64_t &depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_name, const std::string &coord_sys)
void setSamplePercent(const double sample_percent_arg)
Sets the sampling percent for constructing LODs.
Definition: octree_base.h:565
std::uint64_t addPointCloud(pcl::PCLPointCloud2::Ptr &input_cloud)
void saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
OutofcoreOctreeBase(OutofcoreOctreeBase &rval)
static const std::uint64_t LOAD_COUNT_
Definition: octree_base.h:640
double getSamplePercent() const
Returns the sample_percent_ used when constructing the LOD.
Definition: octree_base.h:557
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_base.h:187
void setLODFilter(const pcl::Filter< pcl::PCLPointCloud2 >::Ptr &filter_arg)
Sets the filter to use when building the levels of depth.
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
Definition: octree_base.h:627
void getOccupiedVoxelCenters(std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > &voxel_centers) const
Returns the voxel centers of all occupied/existing leaves of the tree.
Definition: octree_base.h:525
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
shared_ptr< OutofcoreOctreeBaseMetadata > Ptr
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
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...
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.