Point Cloud Library (PCL)  1.7.1
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, NULL, 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 () / (boost::filesystem::basename (root_name) + 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  boost::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 boost::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 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 / (boost::filesystem::basename (root_name) + 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> boost::uint64_t
209  {
210  boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
211 
212  const bool _FORCE_BB_CHECK = true;
213 
214  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> boost::uint64_t
225  {
226  return (addDataToLeaf (point_cloud->points));
227  }
228 
229  ////////////////////////////////////////////////////////////////////////////////
230 
231  template<typename ContainerT, typename PointT> boost::uint64_t
233  {
234  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> boost::uint64_t
244  {
245  // Lock the tree while writing
246  boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
247  boost::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> boost::uint64_t
255  {
256  // Lock the tree while writing
257  boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
258  boost::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> boost::uint64_t
271  {
272  // Lock the tree while writing
273  boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
274  boost::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  boost::shared_lock < boost::shared_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 boost::uint32_t query_depth) const
291  {
292  boost::shared_lock < boost::shared_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 boost::uint32_t query_depth) const
305  {
306  boost::shared_lock < boost::shared_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 boost::uint64_t query_depth, AlignedPointTVector& dst) const
314  {
315  boost::shared_lock < boost::shared_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 boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) const
325  {
326  boost::shared_lock < boost::shared_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 boost::uint64_t query_depth, const double percent, AlignedPointTVector& dst) const
339  {
340  boost::shared_lock < boost::shared_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_!= NULL)
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  boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
378  root_node_->printBoundingBox (query_depth);
379  }
380 
381  ////////////////////////////////////////////////////////////////////////////////
382 
383  template<typename ContainerT, typename PointT> void
385  {
386  boost::shared_lock < boost::shared_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 size_t query_depth) const
401  {
402  boost::shared_lock < boost::shared_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 boost::uint32_t query_depth, std::list<std::string>& bin_name) const
417  {
418  boost::shared_lock < boost::shared_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->size () == 0)
481  {
482  current->flush_DeAlloc_this_only ();
483  }
484 
485  for (int i = 0; i < current->numchildren (); i++)
486  {
487  DeAllocEmptyNodeCache (current->children[i]);
488  }
489 
490  }
491 
492  ////////////////////////////////////////////////////////////////////////////////
493  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
494  OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
495  {
496  return (branch_arg.getChildPtr (childIdx_arg));
497  }
498 
499  ////////////////////////////////////////////////////////////////////////////////
500  template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
502  {
503  return (lod_filter_ptr_);
504  }
505 
506  ////////////////////////////////////////////////////////////////////////////////
507 
508  template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
510  {
511  return (lod_filter_ptr_);
512  }
513 
514  ////////////////////////////////////////////////////////////////////////////////
515 
516  template<typename ContainerT, typename PointT> void
518  {
519  lod_filter_ptr_ = filter_arg;
520  }
521 
522  ////////////////////////////////////////////////////////////////////////////////
523 
524  template<typename ContainerT, typename PointT> bool
526  {
527  if (root_node_== NULL)
528  {
529  x = 0;
530  y = 0;
531  return (false);
532  }
533 
534  Eigen::Vector3d min, max;
535  this->getBoundingBox (min, max);
536 
537  double depth = static_cast<double> (metadata_->getDepth ());
538  Eigen::Vector3d diff = max-min;
539 
540  y = diff[1] * pow (.5, depth);
541  x = diff[0] * pow (.5, depth);
542 
543  return (true);
544  }
545 
546  ////////////////////////////////////////////////////////////////////////////////
547 
548  template<typename ContainerT, typename PointT> double
550  {
551  Eigen::Vector3d min, max;
552  this->getBoundingBox (min, max);
553  double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
554  return (result);
555  }
556 
557  ////////////////////////////////////////////////////////////////////////////////
558 
559  template<typename ContainerT, typename PointT> void
561  {
562  if (root_node_== NULL)
563  {
564  PCL_ERROR ("Root node is null; aborting buildLOD.\n");
565  return;
566  }
567 
568  boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
569 
570  const int number_of_nodes = 1;
571 
572  std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(0));
573  current_branch[0] = root_node_;
574  assert (current_branch.back () != 0);
575  this->buildLODRecursive (current_branch);
576  }
577 
578  ////////////////////////////////////////////////////////////////////////////////
579 
580  template<typename ContainerT, typename PointT> void
582  {
583  Eigen::Vector3d min, max;
584  node.getBoundingBox (min,max);
585  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]);
586  }
587 
588 
589  ////////////////////////////////////////////////////////////////////////////////
590 
591  template<typename ContainerT, typename PointT> void
592  OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
593  {
594  PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
595 
596  if (!current_branch.back ())
597  {
598  return;
599  }
600 
601  if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
602  {
603  assert (current_branch.back ()->getDepth () == this->getDepth ());
604 
605  BranchNode* leaf = current_branch.back ();
606 
607  pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
608  //read the data from the PCD file associated with the leaf; it is full resolution
609  leaf->read (leaf_input_cloud);
610  assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
611 
612  //go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution
613  for (int64_t level = static_cast<int64_t>(current_branch.size ()-1); level >= 1; level--)
614  {
615  BranchNode* target_parent = current_branch[level-1];
616  assert (target_parent != 0);
617  double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
618  double current_depth_sample_percent = pow (sample_percent_, exponent);
619 
620  assert (current_depth_sample_percent > 0.0);
621  //------------------------------------------------------------
622  //subsample data:
623  // 1. Get indices from a random sample
624  // 2. Extract those indices with the extract indices class (in order to also get the complement)
625  //------------------------------------------------------------
626 
627  lod_filter_ptr_->setInputCloud (leaf_input_cloud);
628 
629  //set sample size to 1/8 of total points (12.5%)
630  uint64_t sample_size = static_cast<uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
631 
632  if (sample_size == 0)
633  sample_size = 1;
634 
635  lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
636 
637  //create our destination
638  pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
639 
640  //create destination for indices
641  pcl::IndicesPtr downsampled_cloud_indices (new std::vector< int > ());
642  lod_filter_ptr_->filter (*downsampled_cloud_indices);
643 
644  //extract the "random subset", size by setSampleSize
646  extractor.setInputCloud (leaf_input_cloud);
647  extractor.setIndices (downsampled_cloud_indices);
648  extractor.filter (*downsampled_cloud);
649 
650  //write to the target
651  if (downsampled_cloud->width*downsampled_cloud->height > 0)
652  {
653  target_parent->payload_->insertRange (downsampled_cloud);
654  this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
655  }
656  }
657  }
658  else//not at leaf, keep going down
659  {
660  //clear this node while walking down the tree in case we are updating the LOD
661  current_branch.back ()->clearData ();
662 
663  std::vector<BranchNode*> next_branch (current_branch);
664 
665  if (current_branch.back ()->hasUnloadedChildren ())
666  {
667  current_branch.back ()->loadChildren (false);
668  }
669 
670  for (size_t i = 0; i < 8; i++)
671  {
672  next_branch.push_back (current_branch.back ()->getChildPtr (i));
673  //skip that child if it doesn't exist
674  if (next_branch.back () != 0)
675  buildLODRecursive (next_branch);
676 
677  next_branch.pop_back ();
678  }
679  }
680  }
681  ////////////////////////////////////////////////////////////////////////////////
682 
683  template<typename ContainerT, typename PointT> void
684  OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (boost::uint64_t depth, boost::uint64_t new_point_count)
685  {
686  if (std::numeric_limits<uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
687  {
688  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());
689  PCL_THROW_EXCEPTION (PCLException, "Overflow error");
690  }
691 
692  metadata_->setLODPoints (depth, new_point_count, true /*true->increment*/);
693  }
694 
695  ////////////////////////////////////////////////////////////////////////////////
696 
697  template<typename ContainerT, typename PointT> bool
698  OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
699  {
700  if (boost::filesystem::extension (path_name) != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
701  {
702  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBase] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", boost::filesystem::extension (path_name).c_str (), OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension.c_str () );
703  return (0);
704  }
705 
706  return (1);
707  }
708 
709  ////////////////////////////////////////////////////////////////////////////////
710 
711  template<typename ContainerT, typename PointT> void
712  OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
713  {
714  Eigen::Vector3d diff = bb_max - bb_min;
715  assert (diff[0] > 0);
716  assert (diff[1] > 0);
717  assert (diff[2] > 0);
718  Eigen::Vector3d center = (bb_max + bb_min)/2.0;
719 
720  double max_sidelength = std::max (std::max (fabs (diff[0]), fabs (diff[1])), fabs (diff[2]));
721  assert (max_sidelength > 0);
722  bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
723  bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
724  }
725 
726  ////////////////////////////////////////////////////////////////////////////////
727 
728  template<typename ContainerT, typename PointT> boost::uint64_t
729  OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
730  {
731  //Assume cube
732  double side_length = max_bb[0] - min_bb[0];
733 
734  if (side_length < leaf_resolution)
735  return (0);
736 
737  boost::uint64_t res = static_cast<boost::uint64_t> (std::ceil (log2f (static_cast<float> (side_length / leaf_resolution))));
738 
739  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
740  return (res);
741  }
742  }//namespace outofcore
743 }//namespace pcl
744 
745 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_