Point Cloud Library (PCL)  1.14.0-dev
octree_base.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012, Urban Robotics, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  */
39 
40 #ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
42 
43 
44 #include <pcl/outofcore/octree_base.h>
45 
46 // JSON
47 #include <pcl/outofcore/cJSON.h>
48 
49 #include <pcl/filters/random_sample.h>
50 #include <pcl/filters/extract_indices.h>
51 
52 // C++
53 #include <iostream>
54 #include <fstream>
55 #include <sstream>
56 #include <string>
57 #include <exception>
58 
59 namespace pcl
60 {
61  namespace outofcore
62  {
63 
64  ////////////////////////////////////////////////////////////////////////////////
65  //Static constants
66  ////////////////////////////////////////////////////////////////////////////////
67 
68  template<typename ContainerT, typename PointT>
70 
71  template<typename ContainerT, typename PointT>
73 
74  ////////////////////////////////////////////////////////////////////////////////
75 
76  template<typename ContainerT, typename PointT>
77  OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::filesystem::path& root_name, const bool load_all)
78  : root_node_ ()
79  , read_write_mutex_ ()
80  , metadata_ (new OutofcoreOctreeBaseMetadata ())
81  , sample_percent_ (0.125)
82  , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
83  {
84  //validate the root filename
85  if (!this->checkExtension (root_name))
86  {
87  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
88  }
89 
90  // Create root_node_node
91  root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, nullptr, load_all);
92  // Set root_node_nodes tree to the newly created tree
93  root_node_->m_tree_ = this;
94 
95  // Set the path to the outofcore octree metadata (unique to the root folder) ending in .octree
96  boost::filesystem::path treepath = root_name.parent_path () / (root_name.stem ().string () + TREE_EXTENSION_);
97 
98  //Load the JSON metadata
99  metadata_->loadMetadataFromDisk (treepath);
100  }
101 
102  ////////////////////////////////////////////////////////////////////////////////
103 
104  template<typename ContainerT, typename PointT>
105  OutofcoreOctreeBase<ContainerT, PointT>::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)
106  : root_node_()
107  , read_write_mutex_ ()
108  , metadata_ (new OutofcoreOctreeBaseMetadata ())
109  , sample_percent_ (0.125)
110  , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
111  {
112  //Enlarge the bounding box to a cube so our voxels will be cubes
113  Eigen::Vector3d tmp_min = min;
114  Eigen::Vector3d tmp_max = max;
115  this->enlargeToCube (tmp_min, tmp_max);
116 
117  //Compute the depth of the tree given the resolution
118  std::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
119 
120  //Create a new outofcore tree
121  this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
122  }
123 
124  ////////////////////////////////////////////////////////////////////////////////
125 
126  template<typename ContainerT, typename PointT>
127  OutofcoreOctreeBase<ContainerT, PointT>::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)
128  : root_node_()
129  , read_write_mutex_ ()
130  , metadata_ (new OutofcoreOctreeBaseMetadata ())
131  , sample_percent_ (0.125)
132  , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
133  {
134  //Create a new outofcore tree
135  this->init (max_depth, min, max, root_node_name, coord_sys);
136  }
137 
138  ////////////////////////////////////////////////////////////////////////////////
139  template<typename ContainerT, typename PointT> void
140  OutofcoreOctreeBase<ContainerT, PointT>::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)
141  {
142  //Validate the extension of the pathname
143  if (!this->checkExtension (root_name))
144  {
145  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
146  }
147 
148  //Check to make sure that we are not overwriting existing data
149  if (boost::filesystem::exists (root_name.parent_path ()))
150  {
151  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase] A dir named %s already exists. Overwriting an existing tree is not supported.\n", root_name.parent_path ().c_str () );
152  PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
153  }
154 
155  // Get fullpath and recreate directories
156  boost::filesystem::path dir = root_name.parent_path ();
157 
158  if (!boost::filesystem::exists (dir))
159  {
160  boost::filesystem::create_directory (dir);
161  }
162 
163  Eigen::Vector3d tmp_min = min;
164  Eigen::Vector3d tmp_max = max;
165  this->enlargeToCube (tmp_min, tmp_max);
166 
167  // Create root node
168  root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name);
169  root_node_->m_tree_ = this;
170 
171  // Set root nodes file path
172  boost::filesystem::path treepath = dir / (root_name.stem ().string () + TREE_EXTENSION_);
173 
174  //fill the fields of the metadata
175  metadata_->setCoordinateSystem (coord_sys);
176  metadata_->setDepth (depth);
177  metadata_->setLODPoints (depth+1);
178  metadata_->setMetadataFilename (treepath);
179  metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
180  //metadata_->setPointType ( <point type string here> );
181 
182  //save to disk
183  metadata_->serializeMetadataToDisk ();
184  }
185 
186 
187  ////////////////////////////////////////////////////////////////////////////////
188  template<typename ContainerT, typename PointT>
190  {
191  root_node_->flushToDiskRecursive ();
192 
193  saveToFile ();
194  delete (root_node_);
195  }
196 
197  ////////////////////////////////////////////////////////////////////////////////
198 
199  template<typename ContainerT, typename PointT> void
201  {
202  this->metadata_->serializeMetadataToDisk ();
203  }
204 
205  ////////////////////////////////////////////////////////////////////////////////
206 
207  template<typename ContainerT, typename PointT> std::uint64_t
209  {
210  std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
211 
212  constexpr bool _FORCE_BB_CHECK = true;
213 
214  std::uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
215 
216  assert (p.size () == pt_added);
217 
218  return (pt_added);
219  }
220 
221  ////////////////////////////////////////////////////////////////////////////////
222 
223  template<typename ContainerT, typename PointT> std::uint64_t
225  {
226  return (addDataToLeaf (point_cloud->points));
227  }
228 
229  ////////////////////////////////////////////////////////////////////////////////
230 
231  template<typename ContainerT, typename PointT> std::uint64_t
233  {
234  std::uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
235 // assert (input_cloud->width*input_cloud->height == pt_added);
236  return (pt_added);
237  }
238 
239 
240  ////////////////////////////////////////////////////////////////////////////////
241 
242  template<typename ContainerT, typename PointT> std::uint64_t
244  {
245  // Lock the tree while writing
246  std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
247  std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points, false);
248  return (pt_added);
249  }
250 
251  ////////////////////////////////////////////////////////////////////////////////
252 
253  template<typename ContainerT, typename PointT> std::uint64_t
255  {
256  // Lock the tree while writing
257  std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
258  std::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
259 
260  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
261 
262  assert ( input_cloud->width*input_cloud->height == pt_added );
263 
264  return (pt_added);
265  }
266 
267  ////////////////////////////////////////////////////////////////////////////////
268 
269  template<typename ContainerT, typename PointT> std::uint64_t
271  {
272  // Lock the tree while writing
273  std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
274  std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src, false);
275  return (pt_added);
276  }
277 
278  ////////////////////////////////////////////////////////////////////////////////
279 
280  template<typename Container, typename PointT> void
281  OutofcoreOctreeBase<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) const
282  {
283  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
284  root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
285  }
286 
287  ////////////////////////////////////////////////////////////////////////////////
288 
289  template<typename Container, typename PointT> void
290  OutofcoreOctreeBase<Container, PointT>::queryFrustum(const double *planes, std::list<std::string>& file_names, const std::uint32_t query_depth) const
291  {
292  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
293  root_node_->queryFrustum (planes, file_names, query_depth);
294  }
295 
296  ////////////////////////////////////////////////////////////////////////////////
297 
298  template<typename Container, typename PointT> void
300  const double *planes,
301  const Eigen::Vector3d &eye,
302  const Eigen::Matrix4d &view_projection_matrix,
303  std::list<std::string>& file_names,
304  const std::uint32_t query_depth) const
305  {
306  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
307  root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
308  }
309 
310  ////////////////////////////////////////////////////////////////////////////////
311 
312  template<typename ContainerT, typename PointT> void
313  OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, AlignedPointTVector& dst) const
314  {
315  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
316  dst.clear ();
317  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode] Querying Bounding Box %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf", min[0], min[1], min[2], max[0], max[1], max[2]);
318  root_node_->queryBBIncludes (min, max, query_depth, dst);
319  }
320 
321  ////////////////////////////////////////////////////////////////////////////////
322 
323  template<typename ContainerT, typename PointT> void
324  OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) const
325  {
326  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
327 
328  dst_blob->data.clear ();
329  dst_blob->width = 0;
330  dst_blob->height =1;
331 
332  root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
333  }
334 
335  ////////////////////////////////////////////////////////////////////////////////
336 
337  template<typename ContainerT, typename PointT> void
338  OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, const double percent, AlignedPointTVector& dst) const
339  {
340  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
341  dst.clear ();
342  root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
343  }
344 
345  ////////////////////////////////////////////////////////////////////////////////
346  template<typename ContainerT, typename PointT> void
347  OutofcoreOctreeBase<ContainerT, PointT>::queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent)
348  {
349  if (percent==1.0)
350  {
351  root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
352  }
353  else
354  {
355  root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
356  }
357  }
358 
359  ////////////////////////////////////////////////////////////////////////////////
360 
361  template<typename ContainerT, typename PointT> bool
362  OutofcoreOctreeBase<ContainerT, PointT>::getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const
363  {
364  if (root_node_!= nullptr)
365  {
366  root_node_->getBoundingBox (min, max);
367  return true;
368  }
369  return false;
370  }
371 
372  ////////////////////////////////////////////////////////////////////////////////
373 
374  template<typename ContainerT, typename PointT> void
376  {
377  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
378  root_node_->printBoundingBox (query_depth);
379  }
380 
381  ////////////////////////////////////////////////////////////////////////////////
382 
383  template<typename ContainerT, typename PointT> void
385  {
386  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
387  if (query_depth > metadata_->getDepth ())
388  {
389  root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
390  }
391  else
392  {
393  root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
394  }
395  }
396 
397  ////////////////////////////////////////////////////////////////////////////////
398 
399  template<typename ContainerT, typename PointT> void
400  OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth) const
401  {
402  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
403  if (query_depth > metadata_->getDepth ())
404  {
405  root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
406  }
407  else
408  {
409  root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
410  }
411  }
412 
413  ////////////////////////////////////////////////////////////////////////////////
414 
415  template<typename ContainerT, typename PointT> void
416  OutofcoreOctreeBase<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name) const
417  {
418  std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
419  bin_name.clear ();
420 #if defined _MSC_VER
421  #pragma warning(push)
422  #pragma warning(disable : 4267)
423 #endif
424  root_node_->queryBBIntersects (min, max, query_depth, bin_name);
425 #if defined _MSC_VER
426  #pragma warning(pop)
427 #endif
428  }
429 
430  ////////////////////////////////////////////////////////////////////////////////
431 
432  template<typename ContainerT, typename PointT> void
433  OutofcoreOctreeBase<ContainerT, PointT>::writeVPythonVisual (const boost::filesystem::path& filename)
434  {
435  std::ofstream f (filename.c_str ());
436 
437  f << "from visual import *\n\n";
438 
439  root_node_->writeVPythonVisual (f);
440  }
441 
442  ////////////////////////////////////////////////////////////////////////////////
443 
444  template<typename ContainerT, typename PointT> void
446  {
447  root_node_->flushToDisk ();
448  }
449 
450  ////////////////////////////////////////////////////////////////////////////////
451 
452  template<typename ContainerT, typename PointT> void
454  {
455  root_node_->flushToDiskLazy ();
456  }
457 
458  ////////////////////////////////////////////////////////////////////////////////
459 
460  template<typename ContainerT, typename PointT> void
462  {
463  saveToFile ();
464  root_node_->convertToXYZ ();
465  }
466 
467  ////////////////////////////////////////////////////////////////////////////////
468 
469  template<typename ContainerT, typename PointT> void
471  {
472  DeAllocEmptyNodeCache (root_node_);
473  }
474 
475  ////////////////////////////////////////////////////////////////////////////////
476 
477  template<typename ContainerT, typename PointT> void
479  {
480  if (current == nullptr)
481  current = root_node_;
482 
483  if (current->size () == 0)
484  {
485  current->flush_DeAlloc_this_only ();
486  }
487 
488  for (int i = 0; i < current->numchildren (); i++)
489  {
490  DeAllocEmptyNodeCache (current->children[i]);
491  }
492  }
493 
494  ////////////////////////////////////////////////////////////////////////////////
495  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
496  OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
497  {
498  return (branch_arg.getChildPtr (childIdx_arg));
499  }
500 
501  ////////////////////////////////////////////////////////////////////////////////
502  template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
504  {
505  return (lod_filter_ptr_);
506  }
507 
508  ////////////////////////////////////////////////////////////////////////////////
509 
510  template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
512  {
513  return (lod_filter_ptr_);
514  }
515 
516  ////////////////////////////////////////////////////////////////////////////////
517 
518  template<typename ContainerT, typename PointT> void
520  {
521  lod_filter_ptr_ = std::static_pointer_cast<decltype(lod_filter_ptr_)>(filter_arg);
522  }
523 
524  ////////////////////////////////////////////////////////////////////////////////
525 
526  template<typename ContainerT, typename PointT> bool
528  {
529  if (root_node_== nullptr)
530  {
531  x = 0;
532  y = 0;
533  return (false);
534  }
535 
536  Eigen::Vector3d min, max;
537  this->getBoundingBox (min, max);
538 
539  double depth = static_cast<double> (metadata_->getDepth ());
540  Eigen::Vector3d diff = max-min;
541 
542  y = diff[1] * pow (.5, depth);
543  x = diff[0] * pow (.5, depth);
544 
545  return (true);
546  }
547 
548  ////////////////////////////////////////////////////////////////////////////////
549 
550  template<typename ContainerT, typename PointT> double
552  {
553  Eigen::Vector3d min, max;
554  this->getBoundingBox (min, max);
555  double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
556  return (result);
557  }
558 
559  ////////////////////////////////////////////////////////////////////////////////
560 
561  template<typename ContainerT, typename PointT> void
563  {
564  if (root_node_== nullptr)
565  {
566  PCL_ERROR ("Root node is null; aborting buildLOD.\n");
567  return;
568  }
569 
570  std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
571 
572  constexpr int number_of_nodes = 1;
573 
574  std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(nullptr));
575  current_branch[0] = root_node_;
576  assert (current_branch.back () != nullptr);
577  this->buildLODRecursive (current_branch);
578  }
579 
580  ////////////////////////////////////////////////////////////////////////////////
581 
582  template<typename ContainerT, typename PointT> void
584  {
585  Eigen::Vector3d min, max;
586  node.getBoundingBox (min,max);
587  PCL_INFO ("[pcl::outofcore::OutofcoreOctreeBase::%s] min(%lf,%lf,%lf), max(%lf,%lf,%lf)\n", __FUNCTION__, min[0], min[1], min[2], max[0], max[1], max[2]);
588  }
589 
590 
591  ////////////////////////////////////////////////////////////////////////////////
592 
593  template<typename ContainerT, typename PointT> void
594  OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
595  {
596  PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
597 
598  if (!current_branch.back ())
599  {
600  return;
601  }
602 
603  if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
604  {
605  assert (current_branch.back ()->getDepth () == this->getDepth ());
606 
607  BranchNode* leaf = current_branch.back ();
608 
609  pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
610  //read the data from the PCD file associated with the leaf; it is full resolution
611  leaf->read (leaf_input_cloud);
612  assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
613 
614  //go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution
615  for (auto level = static_cast<std::int64_t>(current_branch.size ()-1); level >= 1; level--)
616  {
617  BranchNode* target_parent = current_branch[level-1];
618  assert (target_parent != nullptr);
619  double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
620  double current_depth_sample_percent = pow (sample_percent_, exponent);
621 
622  assert (current_depth_sample_percent > 0.0);
623  //------------------------------------------------------------
624  //subsample data:
625  // 1. Get indices from a random sample
626  // 2. Extract those indices with the extract indices class (in order to also get the complement)
627  //------------------------------------------------------------
628 
629  lod_filter_ptr_->setInputCloud (leaf_input_cloud);
630 
631  //set sample size to 1/8 of total points (12.5%)
632  auto sample_size = static_cast<std::uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
633 
634  if (sample_size == 0)
635  sample_size = 1;
636 
637  lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
638 
639  //create our destination
640  pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
641 
642  //create destination for indices
643  pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ());
644  lod_filter_ptr_->filter (*downsampled_cloud_indices);
645 
646  //extract the "random subset", size by setSampleSize
648  extractor.setInputCloud (leaf_input_cloud);
649  extractor.setIndices (downsampled_cloud_indices);
650  extractor.filter (*downsampled_cloud);
651 
652  //write to the target
653  if (downsampled_cloud->width*downsampled_cloud->height > 0)
654  {
655  target_parent->payload_->insertRange (downsampled_cloud);
656  this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
657  }
658  }
659  }
660  else//not at leaf, keep going down
661  {
662  //clear this node while walking down the tree in case we are updating the LOD
663  current_branch.back ()->clearData ();
664 
665  std::vector<BranchNode*> next_branch (current_branch);
666 
667  if (current_branch.back ()->hasUnloadedChildren ())
668  {
669  current_branch.back ()->loadChildren (false);
670  }
671 
672  for (std::size_t i = 0; i < 8; i++)
673  {
674  next_branch.push_back (current_branch.back ()->getChildPtr (i));
675  //skip that child if it doesn't exist
676  if (next_branch.back () != nullptr)
677  buildLODRecursive (next_branch);
678 
679  next_branch.pop_back ();
680  }
681  }
682  }
683  ////////////////////////////////////////////////////////////////////////////////
684 
685  template<typename ContainerT, typename PointT> void
686  OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (std::uint64_t depth, std::uint64_t new_point_count)
687  {
688  if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
689  {
690  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase::incrementPointsInLOD] Overflow error. Too many points in depth %d of outofcore octree with root at %s\n", depth, metadata_->getMetadataFilename().c_str());
691  PCL_THROW_EXCEPTION (PCLException, "Overflow error");
692  }
693 
694  metadata_->setLODPoints (depth, new_point_count, true /*true->increment*/);
695  }
696 
697  ////////////////////////////////////////////////////////////////////////////////
698 
699  template<typename ContainerT, typename PointT> bool
700  OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
701  {
702  if (path_name.extension ().string () != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
703  {
704  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBase] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", path_name.extension ().string ().c_str (), OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension.c_str () );
705  return (false);
706  }
707 
708  return (true);
709  }
710 
711  ////////////////////////////////////////////////////////////////////////////////
712 
713  template<typename ContainerT, typename PointT> void
714  OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
715  {
716  Eigen::Vector3d diff = bb_max - bb_min;
717  assert (diff[0] > 0);
718  assert (diff[1] > 0);
719  assert (diff[2] > 0);
720  Eigen::Vector3d center = (bb_max + bb_min)/2.0;
721 
722  double max_sidelength = std::max (std::max (std::abs (diff[0]), std::abs (diff[1])), std::abs (diff[2]));
723  assert (max_sidelength > 0);
724  bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
725  bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
726  }
727 
728  ////////////////////////////////////////////////////////////////////////////////
729 
730  template<typename ContainerT, typename PointT> std::uint64_t
731  OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
732  {
733  //Assume cube
734  double side_length = max_bb[0] - min_bb[0];
735 
736  if (side_length < leaf_resolution)
737  return (0);
738 
739  auto res = static_cast<std::uint64_t> (std::ceil (std::log2 (side_length / leaf_resolution)));
740 
741  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
742  return (res);
743  }
744  }//namespace outofcore
745 }//namespace pcl
746 
747 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
ExtractIndices extracts a set of indices from a point cloud.
Filter represents the base filter class.
Definition: filter.h:81
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:66
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:150
void DeAllocEmptyNodeCache()
Flush empty nodes only.
OutofcoreOctreeBaseMetadata::Ptr metadata_
Definition: octree_base.h:631
std::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
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::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:77
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.
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...
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:461
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...
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.
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
Definition: octree_base.h:188
void flushToDisk()
Flush all nodes' cache.
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
Definition: octree_base.h:494
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:636
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
friend class OutofcoreOctreeBaseNode< ContainerT, PointT >
Definition: octree_base.h:151
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.
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 saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_base.h:186
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:626
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
Encapsulated class to read JSON metadata into memory, and write the JSON metadata associated with the...
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
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
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
std::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
std::uint64_t size() const
Number of points in the payload.
virtual std::size_t getDepth() const
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
shared_ptr< ::pcl::PCLPointCloud2 > Ptr