Point Cloud Library (PCL)  1.9.1-dev
octree_base_node.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 
41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
43 
44 // C++
45 #include <iostream>
46 #include <fstream>
47 #include <random>
48 #include <sstream>
49 #include <string>
50 #include <exception>
51 
52 #include <pcl/common/common.h>
53 #include <pcl/visualization/common/common.h>
54 #include <pcl/outofcore/octree_base_node.h>
55 #include <pcl/filters/random_sample.h>
56 #include <pcl/filters/extract_indices.h>
57 
58 // JSON
59 #include <pcl/outofcore/cJSON.h>
60 
61 namespace pcl
62 {
63  namespace outofcore
64  {
65 
66  template<typename ContainerT, typename PointT>
68 
69  template<typename ContainerT, typename PointT>
71 
72  template<typename ContainerT, typename PointT>
74 
75  template<typename ContainerT, typename PointT>
77 
78  template<typename ContainerT, typename PointT>
80 
81  template<typename ContainerT, typename PointT>
83 
84  template<typename ContainerT, typename PointT>
86 
87  template<typename ContainerT, typename PointT>
89 
90  template<typename ContainerT, typename PointT>
92  : m_tree_ ()
93  , root_node_ (NULL)
94  , parent_ (NULL)
95  , depth_ (0)
96  , children_ (8, nullptr)
97  , num_children_ (0)
98  , num_loaded_children_ (0)
99  , payload_ ()
100  , node_metadata_ (new OutofcoreOctreeNodeMetadata)
101  {
102  node_metadata_->setOutofcoreVersion (3);
103  }
104 
105  ////////////////////////////////////////////////////////////////////////////////
106 
107  template<typename ContainerT, typename PointT>
109  : m_tree_ ()
110  , root_node_ ()
111  , parent_ (super)
112  , depth_ ()
113  , children_ (8, nullptr)
114  , num_children_ (0)
116  , payload_ ()
118  {
119  node_metadata_->setOutofcoreVersion (3);
120 
121  //Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL)
122  if (super == nullptr)
123  {
124  node_metadata_->setDirectoryPathname (directory_path.parent_path ());
125  node_metadata_->setMetadataFilename (directory_path);
126  depth_ = 0;
127  root_node_ = this;
128 
129  //Check if the specified directory to load currently exists; if not, don't continue
130  if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
131  {
132  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ());
133  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
134  }
135  }
136  else
137  {
138  node_metadata_->setDirectoryPathname (directory_path);
139  depth_ = super->getDepth () + 1;
140  root_node_ = super->root_node_;
141 
142  boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator
143 
144  //flag to test if the desired metadata file was found
145  bool b_loaded = false;
146 
147  for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
148  {
149  const boost::filesystem::path& file = *directory_it;
150 
151  if (!boost::filesystem::is_directory (file))
152  {
153  if (boost::filesystem::extension (file) == node_index_extension)
154  {
155  b_loaded = node_metadata_->loadMetadataFromDisk (file);
156  break;
157  }
158  }
159  }
160 
161  if (!b_loaded)
162  {
163  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
164  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
165  }
166  }
167 
168  //load the metadata
169  loadFromFile (node_metadata_->getMetadataFilename (), super);
170 
171  //set the number of children in this node
172  num_children_ = this->countNumChildren ();
173 
174  if (load_all)
175  {
176  loadChildren (true);
177  }
178  }
179 ////////////////////////////////////////////////////////////////////////////////
180 
181  template<typename ContainerT, typename PointT>
182  OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
183  : m_tree_ (tree)
184  , root_node_ ()
185  , parent_ ()
186  , depth_ ()
187  , children_ (8, nullptr)
188  , num_children_ (0)
190  , payload_ ()
192  {
193  assert (tree != NULL);
194  node_metadata_->setOutofcoreVersion (3);
195  init_root_node (bb_min, bb_max, tree, root_name);
196  }
197 
198  ////////////////////////////////////////////////////////////////////////////////
199 
200  template<typename ContainerT, typename PointT> void
201  OutofcoreOctreeBaseNode<ContainerT, PointT>::init_root_node (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
202  {
203  assert (tree != NULL);
204 
205  parent_ = nullptr;
206  root_node_ = this;
207  m_tree_ = tree;
208  depth_ = 0;
209 
210  //Mark the children as unallocated
211  num_children_ = 0;
212 
213  Eigen::Vector3d tmp_max = bb_max;
214  Eigen::Vector3d tmp_min = bb_min;
215 
216  // Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded
217  double epsilon = 1e-8;
218  tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
219 
220  node_metadata_->setBoundingBox (tmp_min, tmp_max);
221  node_metadata_->setDirectoryPathname (root_name.parent_path ());
222  node_metadata_->setOutofcoreVersion (3);
223 
224  // If the root directory doesn't exist create it
225  if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
226  {
227  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
228  }
229  // If the root directory is a file, do not continue
230  else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
231  {
232  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
233  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
234  }
235 
236  // Create a unique id for node file name
237  std::string uuid;
238 
240 
241  std::string node_container_name;
242 
243  node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension;
244 
245  node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
246  node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
247 
248  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
249  node_metadata_->serializeMetadataToDisk ();
250 
251  // Create data container, ie octree_disk_container, octree_ram_container
252  payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
253  }
254 
255  ////////////////////////////////////////////////////////////////////////////////
256 
257  template<typename ContainerT, typename PointT>
259  {
260  // Recursively delete all children and this nodes data
261  recFreeChildren ();
262  }
263 
264  ////////////////////////////////////////////////////////////////////////////////
265 
266  template<typename ContainerT, typename PointT> std::size_t
268  {
269  std::size_t child_count = 0;
270 
271  for(std::size_t i=0; i<8; i++)
272  {
273  boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
274  if (boost::filesystem::exists (child_path))
275  child_count++;
276  }
277  return (child_count);
278  }
279 
280  ////////////////////////////////////////////////////////////////////////////////
281 
282  template<typename ContainerT, typename PointT> void
284  {
285  node_metadata_->serializeMetadataToDisk ();
286 
287  if (recursive)
288  {
289  for (std::size_t i = 0; i < 8; i++)
290  {
291  if (children_[i])
292  children_[i]->saveIdx (true);
293  }
294  }
295  }
296 
297  ////////////////////////////////////////////////////////////////////////////////
298 
299  template<typename ContainerT, typename PointT> bool
301  {
302  return (this->getNumLoadedChildren () < this->getNumChildren ());
303  }
304  ////////////////////////////////////////////////////////////////////////////////
305 
306  template<typename ContainerT, typename PointT> void
308  {
309  //if we have fewer children loaded than exist on disk, load them
310  if (num_loaded_children_ < this->getNumChildren ())
311  {
312  //check all 8 possible child directories
313  for (int i = 0; i < 8; i++)
314  {
315  boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
316  //if the directory exists and the child hasn't been created (set to 0 by this node's constructor)
317  if (boost::filesystem::exists (child_dir) && this->children_[i] == nullptr)
318  {
319  //load the child node
320  this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive);
321  //keep track of the children loaded
323  }
324  }
325  }
326  assert (num_loaded_children_ == this->getNumChildren ());
327  }
328  ////////////////////////////////////////////////////////////////////////////////
329 
330  template<typename ContainerT, typename PointT> void
332  {
333  if (num_children_ == 0)
334  {
335  return;
336  }
337 
338  for (std::size_t i = 0; i < 8; i++)
339  {
340  if (children_[i])
341  {
343  delete (current);
344  }
345  }
346  children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (nullptr));
347  num_children_ = 0;
348  }
349  ////////////////////////////////////////////////////////////////////////////////
350 
351  template<typename ContainerT, typename PointT> std::uint64_t
353  {
354  //quit if there are no points to add
355  if (p.empty ())
356  {
357  return (0);
358  }
359 
360  //if this depth is the max depth of the tree, then add the points
361  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
362  return (addDataAtMaxDepth( p, skip_bb_check));
363 
364  if (hasUnloadedChildren ())
365  loadChildren (false);
366 
367  std::vector < std::vector<const PointT*> > c;
368  c.resize (8);
369  for (std::size_t i = 0; i < 8; i++)
370  {
371  c[i].reserve (p.size () / 8);
372  }
373 
374  const std::size_t len = p.size ();
375  for (std::size_t i = 0; i < len; i++)
376  {
377  const PointT& pt = p[i];
378 
379  if (!skip_bb_check)
380  {
381  if (!this->pointInBoundingBox (pt))
382  {
383  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
384  continue;
385  }
386  }
387 
388  std::uint8_t box = 0;
389  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
390 
391  box = static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
392  c[static_cast<std::size_t>(box)].push_back (&pt);
393  }
394 
395  std::uint64_t points_added = 0;
396  for (std::size_t i = 0; i < 8; i++)
397  {
398  if (c[i].empty ())
399  continue;
400  if (!children_[i])
401  createChild (i);
402  points_added += children_[i]->addDataToLeaf (c[i], true);
403  c[i].clear ();
404  }
405  return (points_added);
406  }
407  ////////////////////////////////////////////////////////////////////////////////
408 
409 
410  template<typename ContainerT, typename PointT> std::uint64_t
411  OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check)
412  {
413  if (p.empty ())
414  {
415  return (0);
416  }
417 
418  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
419  {
420  //trust me, just add the points
421  if (skip_bb_check)
422  {
423  root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
424 
425  payload_->insertRange (p.data (), p.size ());
426 
427  return (p.size ());
428  }
429  //check which points belong to this node, throw away the rest
430  std::vector<const PointT*> buff;
431  for (const PointT* pt : p)
432  {
433  if(pointInBoundingBox(*pt))
434  {
435  buff.push_back(pt);
436  }
437  }
438 
439  if (!buff.empty ())
440  {
441  root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
442  payload_->insertRange (buff.data (), buff.size ());
443 // payload_->insertRange ( buff );
444 
445  }
446  return (buff.size ());
447  }
448 
449  if (this->hasUnloadedChildren ())
450  {
451  loadChildren (false);
452  }
453 
454  std::vector < std::vector<const PointT*> > c;
455  c.resize (8);
456  for (std::size_t i = 0; i < 8; i++)
457  {
458  c[i].reserve (p.size () / 8);
459  }
460 
461  const std::size_t len = p.size ();
462  for (std::size_t i = 0; i < len; i++)
463  {
464  //const PointT& pt = p[i];
465  if (!skip_bb_check)
466  {
467  if (!this->pointInBoundingBox (*p[i]))
468  {
469  // std::cerr << "failed to place point!!!" << std::endl;
470  continue;
471  }
472  }
473 
474  std::uint8_t box = 00;
475  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
476  //hash each coordinate to the appropriate octant
477  box = static_cast<std::uint8_t> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] )));
478  //3 bit, 8 octants
479  c[box].push_back (p[i]);
480  }
481 
482  std::uint64_t points_added = 0;
483  for (std::size_t i = 0; i < 8; i++)
484  {
485  if (c[i].empty ())
486  continue;
487  if (!children_[i])
488  createChild (i);
489  points_added += children_[i]->addDataToLeaf (c[i], true);
490  c[i].clear ();
491  }
492  return (points_added);
493  }
494  ////////////////////////////////////////////////////////////////////////////////
495 
496 
497  template<typename ContainerT, typename PointT> std::uint64_t
498  OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check)
499  {
500  assert (this->root_node_->m_tree_ != NULL);
501 
502  if (input_cloud->height*input_cloud->width == 0)
503  return (0);
504 
505  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
506  return (addDataAtMaxDepth (input_cloud, true));
507 
508  if( num_children_ < 8 )
509  if(hasUnloadedChildren ())
510  loadChildren (false);
511 
512  if( !skip_bb_check )
513  {
514 
515  //indices to store the points for each bin
516  //these lists will be used to copy data to new point clouds and pass down recursively
517  std::vector < std::vector<int> > indices;
518  indices.resize (8);
519 
520  this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
521 
522  for(std::size_t k=0; k<indices.size (); k++)
523  {
524  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
525  }
526 
527  std::uint64_t points_added = 0;
528 
529  for(std::size_t i=0; i<8; i++)
530  {
531  if ( indices[i].empty () )
532  continue;
533 
534  if (children_[i] == nullptr)
535  {
536  createChild (i);
537  }
538 
539  pcl::PCLPointCloud2::Ptr dst_cloud (new pcl::PCLPointCloud2 () );
540 
541  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
542 
543  //copy the points from extracted indices from input cloud to destination cloud
544  pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ;
545 
546  //recursively add the new cloud to the data
547  points_added += children_[i]->addPointCloud (dst_cloud, false);
548  indices[i].clear ();
549  }
550 
551  return (points_added);
552  }
553 
554  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
555 
556  return 0;
557  }
558 
559 
560  ////////////////////////////////////////////////////////////////////////////////
561  template<typename ContainerT, typename PointT> void
563  {
564  assert (this->root_node_->m_tree_ != NULL);
565 
566  AlignedPointTVector sampleBuff;
567  if (!skip_bb_check)
568  {
569  for (const PointT& pt: p)
570  {
571  if (pointInBoundingBox(pt))
572  {
573  sampleBuff.push_back(pt);
574  }
575  }
576  }
577  else
578  {
579  sampleBuff = p;
580  }
581 
582  // Derive percentage from specified sample_percent and tree depth
583  const double percent = pow(sample_percent_, double((this->root_node_->m_tree_->getDepth () - depth_)));
584  const std::uint64_t samplesize = static_cast<std::uint64_t>(percent * static_cast<double>(sampleBuff.size()));
585  const std::uint64_t inputsize = sampleBuff.size();
586 
587  if(samplesize > 0)
588  {
589  // Resize buffer to sample size
590  insertBuff.resize(samplesize);
591 
592  // Create random number generator
593  std::lock_guard<std::mutex> lock(rng_mutex_);
594  std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
595 
596  // Randomly pick sampled points
597  for(std::uint64_t i = 0; i < samplesize; ++i)
598  {
599  std::uint64_t buffstart = buffdist(rng_);
600  insertBuff[i] = ( sampleBuff[buffstart] );
601  }
602  }
603  // Have to do it the slow way
604  else
605  {
606  std::lock_guard<std::mutex> lock(rng_mutex_);
607  std::bernoulli_distribution buffdist(percent);
608 
609  for(std::uint64_t i = 0; i < inputsize; ++i)
610  if(buffdist(rng_))
611  insertBuff.push_back( p[i] );
612  }
613  }
614  ////////////////////////////////////////////////////////////////////////////////
615 
616  template<typename ContainerT, typename PointT> std::uint64_t
618  {
619  assert (this->root_node_->m_tree_ != NULL);
620 
621  // Trust me, just add the points
622  if (skip_bb_check)
623  {
624  // Increment point count for node
625  root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
626 
627  // Insert point data
628  payload_->insertRange ( p );
629 
630  return (p.size ());
631  }
632 
633  // Add points found within the current node's bounding box
634  AlignedPointTVector buff;
635  const std::size_t len = p.size ();
636 
637  for (std::size_t i = 0; i < len; i++)
638  {
639  if (pointInBoundingBox (p[i]))
640  {
641  buff.push_back (p[i]);
642  }
643  }
644 
645  if (!buff.empty ())
646  {
647  root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
648  payload_->insertRange ( buff );
649  }
650  return (buff.size ());
651  }
652  ////////////////////////////////////////////////////////////////////////////////
653  template<typename ContainerT, typename PointT> std::uint64_t
655  {
656  //this assumes data is already in the correct bin
657  if(skip_bb_check)
658  {
659  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
660 
661  this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
662  payload_->insertRange (input_cloud);
663 
664  return (input_cloud->width*input_cloud->height);
665  }
666  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
667  return (0);
668  }
669 
670 
671  ////////////////////////////////////////////////////////////////////////////////
672  template<typename ContainerT, typename PointT> void
673  OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
674  {
675  // Reserve space for children nodes
676  c.resize(8);
677  for(std::size_t i = 0; i < 8; i++)
678  c[i].reserve(p.size() / 8);
679 
680  const std::size_t len = p.size();
681  for(std::size_t i = 0; i < len; i++)
682  {
683  const PointT& pt = p[i];
684 
685  if(!skip_bb_check)
686  if(!this->pointInBoundingBox(pt))
687  continue;
688 
689  subdividePoint (pt, c);
690  }
691  }
692  ////////////////////////////////////////////////////////////////////////////////
693 
694  template<typename ContainerT, typename PointT> void
695  OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c)
696  {
697  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
698  std::size_t octant = 0;
699  octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
700  c[octant].push_back (point);
701  }
702 
703  ////////////////////////////////////////////////////////////////////////////////
704  template<typename ContainerT, typename PointT> std::uint64_t
706  {
707  std::uint64_t points_added = 0;
708 
709  if ( input_cloud->width * input_cloud->height == 0 )
710  {
711  return (0);
712  }
713 
714  if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
715  {
716  std::uint64_t points_added = addDataAtMaxDepth (input_cloud, true);
717  assert (points_added > 0);
718  return (points_added);
719  }
720 
721  if (num_children_ < 8 )
722  {
723  if ( hasUnloadedChildren () )
724  {
725  loadChildren (false);
726  }
727  }
728 
729  //------------------------------------------------------------
730  //subsample data:
731  // 1. Get indices from a random sample
732  // 2. Extract those indices with the extract indices class (in order to also get the complement)
733  //------------------------------------------------------------
735  random_sampler.setInputCloud (input_cloud);
736 
737  //set sample size to 1/8 of total points (12.5%)
738  std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
739  random_sampler.setSample (static_cast<unsigned int> (sample_size));
740 
741  //create our destination
742  pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () );
743 
744  //create destination for indices
745  pcl::IndicesPtr downsampled_cloud_indices ( new std::vector< int > () );
746  random_sampler.filter (*downsampled_cloud_indices);
747 
748  //extract the "random subset", size by setSampleSize
750  extractor.setInputCloud (input_cloud);
751  extractor.setIndices (downsampled_cloud_indices);
752  extractor.filter (*downsampled_cloud);
753 
754  //extract the complement of those points (i.e. everything remaining)
755  pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () );
756  extractor.setNegative (true);
757  extractor.filter (*remaining_points);
758 
759  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Random sampled: %lu of %lu\n", __FUNCTION__, downsampled_cloud->width * downsampled_cloud->height, input_cloud->width * input_cloud->height );
760 
761  //insert subsampled data to the node's disk container payload
762  if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
763  {
764  root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
765  payload_->insertRange (downsampled_cloud);
766  points_added += downsampled_cloud->width*downsampled_cloud->height ;
767  }
768 
769  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
770 
771  //subdivide remaining data by destination octant
772  std::vector<std::vector<int> > indices;
773  indices.resize (8);
774 
775  this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
776 
777  //pass each set of points to the appropriate child octant
778  for(std::size_t i=0; i<8; i++)
779  {
780 
781  if(indices[i].empty ())
782  continue;
783 
784  if (children_[i] == nullptr)
785  {
786  assert (i < 8);
787  createChild (i);
788  }
789 
790  //copy correct indices into a temporary cloud
791  pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ());
792  pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud);
793 
794  //recursively add points and keep track of how many were successfully added to the tree
795  points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
796  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] points_added: %lu, indices[i].size: %lu, tmp_local_point_cloud size: %lu\n", __FUNCTION__, points_added, indices[i].size (), tmp_local_point_cloud->width*tmp_local_point_cloud->height);
797 
798  }
799  assert (points_added == input_cloud->width*input_cloud->height);
800  return (points_added);
801  }
802  ////////////////////////////////////////////////////////////////////////////////
803 
804  template<typename ContainerT, typename PointT> std::uint64_t
806  {
807  // If there are no points return
808  if (p.empty ())
809  return (0);
810 
811  // when adding data and generating sampled LOD
812  // If the max depth has been reached
813  assert (this->root_node_->m_tree_ != NULL );
814 
815  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
816  {
817  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
818  return (addDataAtMaxDepth(p, false));
819  }
820 
821  // Create child nodes of the current node but not grand children+
822  if (this->hasUnloadedChildren ())
823  loadChildren (false /*no recursive loading*/);
824 
825  // Randomly sample data
826  AlignedPointTVector insertBuff;
827  randomSample(p, insertBuff, skip_bb_check);
828 
829  if(!insertBuff.empty())
830  {
831  // Increment point count for node
832  root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
833  // Insert sampled point data
834  payload_->insertRange (insertBuff);
835 
836  }
837 
838  //subdivide vec to pass data down lower
839  std::vector<AlignedPointTVector> c;
840  subdividePoints(p, c, skip_bb_check);
841 
842  std::uint64_t points_added = 0;
843  for(std::size_t i = 0; i < 8; i++)
844  {
845  // If child doesn't have points
846  if(c[i].empty())
847  continue;
848 
849  // If child doesn't exist
850  if(!children_[i])
851  createChild(i);
852 
853  // Recursively build children
854  points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true);
855  c[i].clear();
856  }
857 
858  return (points_added);
859  }
860  ////////////////////////////////////////////////////////////////////////////////
861 
862  template<typename ContainerT, typename PointT> void
864  {
865  assert (idx < 8);
866 
867  //if already has 8 children, return
868  if (children_[idx] || (num_children_ == 8))
869  {
870  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
871  return;
872  }
873 
874  Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
875  Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0);
876 
877  Eigen::Vector3d childbb_min;
878  Eigen::Vector3d childbb_max;
879 
880  int x, y, z;
881  if (idx > 3)
882  {
883  x = ((idx == 5) || (idx == 7)) ? 1 : 0;
884  y = ((idx == 6) || (idx == 7)) ? 1 : 0;
885  z = 1;
886  }
887  else
888  {
889  x = ((idx == 1) || (idx == 3)) ? 1 : 0;
890  y = ((idx == 2) || (idx == 3)) ? 1 : 0;
891  z = 0;
892  }
893 
894  childbb_min[2] = start[2] + static_cast<double> (z) * step[2];
895  childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2];
896 
897  childbb_min[1] = start[1] + static_cast<double> (y) * step[1];
898  childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1];
899 
900  childbb_min[0] = start[0] + static_cast<double> (x) * step[0];
901  childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0];
902 
903  boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
904  children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this);
905 
906  num_children_++;
907  }
908  ////////////////////////////////////////////////////////////////////////////////
909 
910  template<typename ContainerT, typename PointT> bool
911  pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point)
912  {
913  if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
914  ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
915  ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
916  {
917  return (true);
918 
919  }
920  return (false);
921  }
922 
923 
924  ////////////////////////////////////////////////////////////////////////////////
925  template<typename ContainerT, typename PointT> bool
927  {
928  const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
929  const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
930 
931  if (((min[0] <= p.x) && (p.x < max[0])) &&
932  ((min[1] <= p.y) && (p.y < max[1])) &&
933  ((min[2] <= p.z) && (p.z < max[2])))
934  {
935  return (true);
936 
937  }
938  return (false);
939  }
940 
941  ////////////////////////////////////////////////////////////////////////////////
942  template<typename ContainerT, typename PointT> void
944  {
945  Eigen::Vector3d min;
946  Eigen::Vector3d max;
947  node_metadata_->getBoundingBox (min, max);
948 
949  if (this->depth_ < query_depth){
950  for (std::size_t i = 0; i < this->depth_; i++)
951  std::cout << " ";
952 
953  std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - ";
954  std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - ";
955  std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1];
956  std::cout << ", " << max[2] - min[2] << "]" << std::endl;
957 
958  if (num_children_ > 0)
959  {
960  for (std::size_t i = 0; i < 8; i++)
961  {
962  if (children_[i])
963  children_[i]->printBoundingBox (query_depth);
964  }
965  }
966  }
967  }
968 
969  ////////////////////////////////////////////////////////////////////////////////
970  template<typename ContainerT, typename PointT> void
972  {
973  if (this->depth_ < query_depth){
974  if (num_children_ > 0)
975  {
976  for (std::size_t i = 0; i < 8; i++)
977  {
978  if (children_[i])
979  children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
980  }
981  }
982  }
983  else
984  {
985  PointT voxel_center;
986  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
987  voxel_center.x = static_cast<float>(mid_xyz[0]);
988  voxel_center.y = static_cast<float>(mid_xyz[1]);
989  voxel_center.z = static_cast<float>(mid_xyz[2]);
990 
991  voxel_centers.push_back(voxel_center);
992  }
993  }
994 
995  ////////////////////////////////////////////////////////////////////////////////
996 // Eigen::Vector3d cornerOffsets[] =
997 // {
998 // Eigen::Vector3d(-1.0, -1.0, -1.0), // - - -
999 // Eigen::Vector3d( 1.0, -1.0, -1.0), // - - +
1000 // Eigen::Vector3d(-1.0, 1.0, -1.0), // - + -
1001 // Eigen::Vector3d( 1.0, 1.0, -1.0), // - + +
1002 // Eigen::Vector3d(-1.0, -1.0, 1.0), // + - -
1003 // Eigen::Vector3d( 1.0, -1.0, 1.0), // + - +
1004 // Eigen::Vector3d(-1.0, 1.0, 1.0), // + + -
1005 // Eigen::Vector3d( 1.0, 1.0, 1.0) // + + +
1006 // };
1007 //
1008 // // Note that the input vector must already be negated when using this code for halfplane tests
1009 // int
1010 // vectorToIndex(Eigen::Vector3d normal)
1011 // {
1012 // int index = 0;
1013 //
1014 // if (normal.z () >= 0) index |= 1;
1015 // if (normal.y () >= 0) index |= 2;
1016 // if (normal.x () >= 0) index |= 4;
1017 //
1018 // return index;
1019 // }
1020 //
1021 // void
1022 // get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb)
1023 // {
1024 //
1025 // p_vertex = min_bb;
1026 // n_vertex = max_bb;
1027 //
1028 // if (normal.x () >= 0)
1029 // {
1030 // n_vertex.x () = min_bb.x ();
1031 // p_vertex.x () = max_bb.x ();
1032 // }
1033 //
1034 // if (normal.y () >= 0)
1035 // {
1036 // n_vertex.y () = min_bb.y ();
1037 // p_vertex.y () = max_bb.y ();
1038 // }
1039 //
1040 // if (normal.z () >= 0)
1041 // {
1042 // p_vertex.z () = max_bb.z ();
1043 // n_vertex.z () = min_bb.z ();
1044 // }
1045 // }
1046 
1047  template<typename Container, typename PointT> void
1048  OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names)
1049  {
1050  queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1051  }
1052 
1053  template<typename Container, typename PointT> void
1054  OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check)
1055  {
1056 
1057  enum {INSIDE, INTERSECT, OUTSIDE};
1058 
1059  int result = INSIDE;
1060 
1061  if (this->depth_ > query_depth)
1062  {
1063  return;
1064  }
1065 
1066 // if (this->depth_ > query_depth)
1067 // return;
1068 
1069  if (!skip_vfc_check)
1070  {
1071  for(int i =0; i < 6; i++){
1072  double a = planes[(i*4)];
1073  double b = planes[(i*4)+1];
1074  double c = planes[(i*4)+2];
1075  double d = planes[(i*4)+3];
1076 
1077  //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1078 
1079  Eigen::Vector3d normal(a, b, c);
1080 
1081  Eigen::Vector3d min_bb;
1082  Eigen::Vector3d max_bb;
1083  node_metadata_->getBoundingBox(min_bb, max_bb);
1084 
1085  // Basic VFC algorithm
1086  Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1087  Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1088  std::abs (static_cast<double> (max_bb.y () - center.y ())),
1089  std::abs (static_cast<double> (max_bb.z () - center.z ())));
1090 
1091  double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1092  double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1093 
1094  if (m + n < 0){
1095  result = OUTSIDE;
1096  break;
1097  }
1098 
1099  if (m - n < 0) result = INTERSECT;
1100 
1101  // // n-p implementation
1102  // Eigen::Vector3d p_vertex; //pos vertex
1103  // Eigen::Vector3d n_vertex; //neg vertex
1104  // get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb);
1105  //
1106  // std::cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << std::endl;
1107  // std::cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << std::endl;
1108 
1109  // is the positive vertex outside?
1110  // if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0)
1111  // {
1112  // result = OUTSIDE;
1113  // }
1114  // // is the negative vertex outside?
1115  // else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
1116  // {
1117  // result = INTERSECT;
1118  // }
1119 
1120  //
1121  //
1122  // // This should be the same as below
1123  // if (normal.dot(n_vertex) + d > 0)
1124  // {
1125  // result = OUTSIDE;
1126  // }
1127  //
1128  // if (normal.dot(p_vertex) + d >= 0)
1129  // {
1130  // result = INTERSECT;
1131  // }
1132 
1133  // This should be the same as above
1134  // double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ());
1135  // std::cout << "m = " << m << std::endl;
1136  // if (m > -d)
1137  // {
1138  // result = OUTSIDE;
1139  // }
1140  //
1141  // double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ());
1142  // std::cout << "n = " << n << std::endl;
1143  // if (n > -d)
1144  // {
1145  // result = INTERSECT;
1146  // }
1147  }
1148  }
1149 
1150  if (result == OUTSIDE)
1151  {
1152  return;
1153  }
1154 
1155 // switch(result){
1156 // case OUTSIDE:
1157 // //std::cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1158 // return;
1159 // case INTERSECT:
1160 // //std::cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << std::endl;
1161 // break;
1162 // case INSIDE:
1163 // //std::cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1164 // break;
1165 // }
1166 
1167  // Add files breadth first
1168  if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1169  //if (payload_->getDataSize () > 0)
1170  {
1171  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1172  }
1173 
1174  if (hasUnloadedChildren ())
1175  {
1176  loadChildren (false);
1177  }
1178 
1179  if (this->getNumChildren () > 0)
1180  {
1181  for (std::size_t i = 0; i < 8; i++)
1182  {
1183  if (children_[i])
1184  children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1185  }
1186  }
1187 // else if (hasUnloadedChildren ())
1188 // {
1189 // loadChildren (false);
1190 //
1191 // for (std::size_t i = 0; i < 8; i++)
1192 // {
1193 // if (children_[i])
1194 // children_[i]->queryFrustum (planes, file_names, query_depth);
1195 // }
1196 // }
1197  //}
1198  }
1199 
1200 ////////////////////////////////////////////////////////////////////////////////
1201 
1202  template<typename Container, typename PointT> void
1203  OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check)
1204  {
1205 
1206  // If we're above our query depth
1207  if (this->depth_ > query_depth)
1208  {
1209  return;
1210  }
1211 
1212  // Bounding Box
1213  Eigen::Vector3d min_bb;
1214  Eigen::Vector3d max_bb;
1215  node_metadata_->getBoundingBox(min_bb, max_bb);
1216 
1217  // Frustum Culling
1218  enum {INSIDE, INTERSECT, OUTSIDE};
1219 
1220  int result = INSIDE;
1221 
1222  if (!skip_vfc_check)
1223  {
1224  for(int i =0; i < 6; i++){
1225  double a = planes[(i*4)];
1226  double b = planes[(i*4)+1];
1227  double c = planes[(i*4)+2];
1228  double d = planes[(i*4)+3];
1229 
1230  //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1231 
1232  Eigen::Vector3d normal(a, b, c);
1233 
1234  // Basic VFC algorithm
1235  Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1236  Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1237  std::abs (static_cast<double> (max_bb.y () - center.y ())),
1238  std::abs (static_cast<double> (max_bb.z () - center.z ())));
1239 
1240  double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1241  double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1242 
1243  if (m + n < 0){
1244  result = OUTSIDE;
1245  break;
1246  }
1247 
1248  if (m - n < 0) result = INTERSECT;
1249 
1250  }
1251  }
1252 
1253  if (result == OUTSIDE)
1254  {
1255  return;
1256  }
1257 
1258  // Bounding box projection
1259  // 3--------2
1260  // /| /| Y 0 = xmin, ymin, zmin
1261  // / | / | | 6 = xmax, ymax. zmax
1262  // 7--------6 | |
1263  // | | | | |
1264  // | 0-----|--1 +------X
1265  // | / | / /
1266  // |/ |/ /
1267  // 4--------5 Z
1268 
1269 // bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1270 // bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1271 // bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1272 // bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1273 // bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1274 // bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1275 // bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1276 // bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1277 
1278  int width = 500;
1279  int height = 500;
1280 
1281  float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height);
1282  //float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix);
1283 
1284 // for (int i=0; i < this->depth_; i++) std::cout << " ";
1285 // std::cout << this->depth_ << ": " << coverage << std::endl;
1286 
1287  // Add files breadth first
1288  if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1289  //if (payload_->getDataSize () > 0)
1290  {
1291  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1292  }
1293 
1294  //if (coverage <= 0.075)
1295  if (coverage <= 10000)
1296  return;
1297 
1298  if (hasUnloadedChildren ())
1299  {
1300  loadChildren (false);
1301  }
1302 
1303  if (this->getNumChildren () > 0)
1304  {
1305  for (std::size_t i = 0; i < 8; i++)
1306  {
1307  if (children_[i])
1308  children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1309  }
1310  }
1311  }
1312 
1313 ////////////////////////////////////////////////////////////////////////////////
1314  template<typename ContainerT, typename PointT> void
1315  OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth)
1316  {
1317  if (this->depth_ < query_depth){
1318  if (num_children_ > 0)
1319  {
1320  for (std::size_t i = 0; i < 8; i++)
1321  {
1322  if (children_[i])
1323  children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1324  }
1325  }
1326  }
1327  else
1328  {
1329  Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1330  voxel_centers.push_back(voxel_center);
1331  }
1332  }
1333 
1334 
1335  ////////////////////////////////////////////////////////////////////////////////
1336 
1337  template<typename ContainerT, typename PointT> void
1338  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const std::uint32_t query_depth, std::list<std::string>& file_names)
1339  {
1340 
1341  Eigen::Vector3d my_min = min_bb;
1342  Eigen::Vector3d my_max = max_bb;
1343 
1344  if (intersectsWithBoundingBox (my_min, my_max))
1345  {
1346  if (this->depth_ < query_depth)
1347  {
1348  if (this->getNumChildren () > 0)
1349  {
1350  for (std::size_t i = 0; i < 8; i++)
1351  {
1352  if (children_[i])
1353  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1354  }
1355  }
1356  else if (hasUnloadedChildren ())
1357  {
1358  loadChildren (false);
1359 
1360  for (std::size_t i = 0; i < 8; i++)
1361  {
1362  if (children_[i])
1363  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1364  }
1365  }
1366  return;
1367  }
1368 
1369  if (payload_->getDataSize () > 0)
1370  {
1371  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1372  }
1373  }
1374  }
1375  ////////////////////////////////////////////////////////////////////////////////
1376 
1377  template<typename ContainerT, typename PointT> void
1378  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::size_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob)
1379  {
1380  std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1381  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1382 
1383  // If the queried bounding box has any intersection with this node's bounding box
1384  if (intersectsWithBoundingBox (min_bb, max_bb))
1385  {
1386  // If we aren't at the max desired depth
1387  if (this->depth_ < query_depth)
1388  {
1389  //if this node doesn't have any children, we are at the max depth for this query
1390  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1391  loadChildren (false);
1392 
1393  //if this node has children
1394  if (num_children_ > 0)
1395  {
1396  //recursively store any points that fall into the queried bounding box into v and return
1397  for (std::size_t i = 0; i < 8; i++)
1398  {
1399  if (children_[i])
1400  children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1401  }
1402  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1403  return;
1404  }
1405  }
1406  else //otherwise if we are at the max depth
1407  {
1408  //get all the points from the payload and return (easy with PCLPointCloud2)
1410  pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ());
1411  //load all the data in this node from disk
1412  payload_->readRange (0, payload_->size (), tmp_blob);
1413 
1414  if( tmp_blob->width*tmp_blob->height == 0 )
1415  return;
1416 
1417  //if this node's bounding box falls completely within the queried bounding box, keep all the points
1418  if (inBoundingBox (min_bb, max_bb))
1419  {
1420  //concatenate all of what was just read into the main dst_blob
1421  //(is it safe to do in place?)
1422 
1423  //if there is already something in the destination blob (remember this method is recursive)
1424  if( dst_blob->width*dst_blob->height != 0 )
1425  {
1426  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1427  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1428  int res = pcl::concatenate (*dst_blob, *tmp_blob, *dst_blob);
1429  (void)res;
1430  assert (res == 1);
1431 
1432  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1433  }
1434  //otherwise, just copy the tmp_blob into the dst_blob
1435  else
1436  {
1437  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1438  pcl::copyPointCloud (*tmp_blob, *dst_blob);
1439  assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1440  }
1441  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1442  return;
1443  }
1444  //otherwise queried bounding box only partially intersects this
1445  //node's bounding box, so we have to check all the points in
1446  //this box for intersection with queried bounding box
1447 
1448 // PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Partial extraction of points in bounding box. Desired: %.2lf %.2lf %2lf, %.2lf %.2lf %.2lf; This node BB: %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf\n", __FUNCTION__, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2], min_[0], min_[1], min_[2], max_[0], max_[1], max_[2] );
1449  //put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox)
1450  typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () );
1451  pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud );
1452  assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height );
1453 
1454  Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
1455  Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
1456 
1457  std::vector<int> indices;
1458 
1459  pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices );
1460  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1461  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () );
1462 
1463  if ( !indices.empty () )
1464  {
1465  if( dst_blob->width*dst_blob->height > 0 )
1466  {
1467  //need a new tmp destination with extracted points within BB
1468  pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ());
1469 
1470  //copy just the points marked in indices
1471  pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb );
1472  assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1473  assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1474  //concatenate those points into the returned dst_blob
1475  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1476  std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1477  (void)orig_points_in_destination;
1478  int res = pcl::concatenate (*dst_blob, *tmp_blob_within_bb, *dst_blob);
1479  (void)res;
1480  assert (res == 1);
1481  assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1482 
1483  }
1484  else
1485  {
1486  pcl::copyPointCloud ( *tmp_blob, indices, *dst_blob );
1487  assert ( dst_blob->width*dst_blob->height == indices.size () );
1488  }
1489  }
1490  }
1491  }
1492 
1493  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1494  }
1495 
1496  template<typename ContainerT, typename PointT> void
1497  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::size_t query_depth, AlignedPointTVector& v)
1498  {
1499 
1500  //if the queried bounding box has any intersection with this node's bounding box
1501  if (intersectsWithBoundingBox (min_bb, max_bb))
1502  {
1503  //if we aren't at the max desired depth
1504  if (this->depth_ < query_depth)
1505  {
1506  //if this node doesn't have any children, we are at the max depth for this query
1507  if (this->hasUnloadedChildren ())
1508  {
1509  this->loadChildren (false);
1510  }
1511 
1512  //if this node has children
1513  if (getNumChildren () > 0)
1514  {
1515  if(hasUnloadedChildren ())
1516  loadChildren (false);
1517 
1518  //recursively store any points that fall into the queried bounding box into v and return
1519  for (std::size_t i = 0; i < 8; i++)
1520  {
1521  if (children_[i])
1522  children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1523  }
1524  return;
1525  }
1526  }
1527  //otherwise if we are at the max depth
1528  else
1529  {
1530  //if this node's bounding box falls completely within the queried bounding box
1531  if (inBoundingBox (min_bb, max_bb))
1532  {
1533  //get all the points from the payload and return
1534  AlignedPointTVector payload_cache;
1535  payload_->readRange (0, payload_->size (), payload_cache);
1536  v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1537  return;
1538  }
1539  //otherwise queried bounding box only partially intersects this
1540  //node's bounding box, so we have to check all the points in
1541  //this box for intersection with queried bounding box
1542  //read _all_ the points in from the disk container
1543  AlignedPointTVector payload_cache;
1544  payload_->readRange (0, payload_->size (), payload_cache);
1545 
1546  std::uint64_t len = payload_->size ();
1547  //iterate through each of them
1548  for (std::uint64_t i = 0; i < len; i++)
1549  {
1550  const PointT& p = payload_cache[i];
1551  //if it falls within this bounding box
1552  if (pointInBoundingBox (min_bb, max_bb, p))
1553  {
1554  //store it in the list
1555  v.push_back (p);
1556  }
1557  else
1558  {
1559  PCL_DEBUG ("[pcl::outofcore::queryBBIncludes] Point %.2lf %.2lf %.2lf not in bounding box %.2lf %.2lf %.2lf", p.x, p.y, p.z, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2]);
1560  }
1561  }
1562  }
1563  }
1564  }
1565 
1566  ////////////////////////////////////////////////////////////////////////////////
1567  template<typename ContainerT, typename PointT> void
1568  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent)
1569  {
1570  if (intersectsWithBoundingBox (min_bb, max_bb))
1571  {
1572  if (this->depth_ < query_depth)
1573  {
1574  if (this->hasUnloadedChildren ())
1575  this->loadChildren (false);
1576 
1577  if (this->getNumChildren () > 0)
1578  {
1579  for (std::size_t i=0; i<8; i++)
1580  {
1581  //recursively traverse (depth first)
1582  if (children_[i]!=nullptr)
1583  children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1584  }
1585  return;
1586  }
1587  }
1588  //otherwise, at max depth --> read from disk, subsample, concatenate
1589  else
1590  {
1591 
1592  if (inBoundingBox (min_bb, max_bb))
1593  {
1594  pcl::PCLPointCloud2::Ptr tmp_blob;
1595  this->payload_->read (tmp_blob);
1596  std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1597 
1598  double sample_points = static_cast<double>(num_pts) * percent;
1599  if (num_pts > 0)
1600  {
1601  //always sample at least one point
1602  sample_points = sample_points > 1 ? sample_points : 1;
1603  }
1604  else
1605  {
1606  return;
1607  }
1608 
1609 
1611  random_sampler.setInputCloud (tmp_blob);
1612 
1613  pcl::PCLPointCloud2::Ptr downsampled_points (new pcl::PCLPointCloud2 ());
1614 
1615  //set sample size as percent * number of points read
1616  random_sampler.setSample (static_cast<unsigned int> (sample_points));
1617 
1619  extractor.setInputCloud (tmp_blob);
1620 
1621  pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ());
1622  random_sampler.filter (*downsampled_cloud_indices);
1623  extractor.setIndices (downsampled_cloud_indices);
1624  extractor.filter (*downsampled_points);
1625 
1626  //concatenate the result into the destination cloud
1627  pcl::concatenate (*dst_blob, *downsampled_points, *dst_blob);
1628  }
1629  }
1630  }
1631  }
1632 
1633 
1634  ////////////////////////////////////////////////////////////////////////////////
1635  template<typename ContainerT, typename PointT> void
1636  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector& dst)
1637  {
1638  //check if the queried bounding box has any intersection with this node's bounding box
1639  if (intersectsWithBoundingBox (min_bb, max_bb))
1640  {
1641  //if we are not at the max depth for queried nodes
1642  if (this->depth_ < query_depth)
1643  {
1644  //check if we don't have children
1645  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1646  {
1647  loadChildren (false);
1648  }
1649  //if we do have children
1650  if (num_children_ > 0)
1651  {
1652  //recursively add their valid points within the queried bounding box to the list v
1653  for (std::size_t i = 0; i < 8; i++)
1654  {
1655  if (children_[i])
1656  children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1657  }
1658  return;
1659  }
1660  }
1661  //otherwise we are at the max depth, so we add all our points or some of our points
1662  else
1663  {
1664  //if this node's bounding box falls completely within the queried bounding box
1665  if (inBoundingBox (min_bb, max_bb))
1666  {
1667  //add a random sample of all the points
1668  AlignedPointTVector payload_cache;
1669  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1670  dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1671  return;
1672  }
1673  //otherwise the queried bounding box only partially intersects with this node's bounding box
1674  //brute force selection of all valid points
1675  AlignedPointTVector payload_cache_within_region;
1676  {
1677  AlignedPointTVector payload_cache;
1678  payload_->readRange (0, payload_->size (), payload_cache);
1679  for (std::size_t i = 0; i < payload_->size (); i++)
1680  {
1681  const PointT& p = payload_cache[i];
1682  if (pointInBoundingBox (min_bb, max_bb, p))
1683  {
1684  payload_cache_within_region.push_back (p);
1685  }
1686  }
1687  }//force the payload cache to deconstruct here
1688 
1689  //use STL random_shuffle and push back a random selection of the points onto our list
1690  std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1691  std::size_t numpick = static_cast<std::size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));;
1692 
1693  for (std::size_t i = 0; i < numpick; i++)
1694  {
1695  dst.push_back (payload_cache_within_region[i]);
1696  }
1697  }
1698  }
1699  }
1700  ////////////////////////////////////////////////////////////////////////////////
1701 
1702 //dir is current level. we put this nodes files into it
1703  template<typename ContainerT, typename PointT>
1704  OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super)
1705  : m_tree_ ()
1706  , root_node_ ()
1707  , parent_ ()
1708  , depth_ ()
1709  , children_ (8, nullptr)
1710  , num_children_ ()
1711  , num_loaded_children_ (0)
1712  , payload_ ()
1714  {
1715  node_metadata_->setOutofcoreVersion (3);
1716 
1717  if (super == nullptr)
1718  {
1719  PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1720  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1721  }
1722 
1723  this->parent_ = super;
1724  root_node_ = super->root_node_;
1725  m_tree_ = super->root_node_->m_tree_;
1726  assert (m_tree_ != NULL);
1727 
1728  depth_ = super->depth_ + 1;
1729  num_children_ = 0;
1730 
1731  node_metadata_->setBoundingBox (bb_min, bb_max);
1732 
1733  std::string uuid_idx;
1734  std::string uuid_cont;
1737 
1738  std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension;
1739 
1740  std::string node_container_name;
1741  node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension;
1742 
1743  node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1744  node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
1745  node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name));
1746 
1747  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
1748 
1749  payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
1750  this->saveIdx (false);
1751  }
1752 
1753  ////////////////////////////////////////////////////////////////////////////////
1754 
1755  template<typename ContainerT, typename PointT> void
1757  {
1758  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1759  {
1760  loadChildren (false);
1761  }
1762 
1763  for (std::size_t i = 0; i < num_children_; i++)
1764  {
1765  children_[i]->copyAllCurrentAndChildPointsRec (v);
1766  }
1767 
1768  AlignedPointTVector payload_cache;
1769  payload_->readRange (0, payload_->size (), payload_cache);
1770 
1771  {
1772  v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1773  }
1774  }
1775 
1776  ////////////////////////////////////////////////////////////////////////////////
1777 
1778  template<typename ContainerT, typename PointT> void
1780  {
1781  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1782  {
1783  loadChildren (false);
1784  }
1785 
1786  for (std::size_t i = 0; i < 8; i++)
1787  {
1788  if (children_[i])
1789  children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1790  }
1791 
1792  std::vector<PointT> payload_cache;
1793  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1794 
1795  for (std::size_t i = 0; i < payload_cache.size (); i++)
1796  {
1797  v.push_back (payload_cache[i]);
1798  }
1799  }
1800 
1801  ////////////////////////////////////////////////////////////////////////////////
1802 
1803  template<typename ContainerT, typename PointT> inline bool
1804  OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1805  {
1806  Eigen::Vector3d min, max;
1807  node_metadata_->getBoundingBox (min, max);
1808 
1809  //Check whether any portion of min_bb, max_bb falls within min,max
1810  if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1811  {
1812  if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1813  {
1814  if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1815  {
1816  return (true);
1817  }
1818  }
1819  }
1820 
1821  return (false);
1822  }
1823  ////////////////////////////////////////////////////////////////////////////////
1824 
1825  template<typename ContainerT, typename PointT> inline bool
1826  OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1827  {
1828  Eigen::Vector3d min, max;
1829 
1830  node_metadata_->getBoundingBox (min, max);
1831 
1832  if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1833  {
1834  if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1835  {
1836  if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1837  {
1838  return (true);
1839  }
1840  }
1841  }
1842 
1843  return (false);
1844  }
1845  ////////////////////////////////////////////////////////////////////////////////
1846 
1847  template<typename ContainerT, typename PointT> inline bool
1848  OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb,
1849  const PointT& p)
1850  {
1851  //by convention, minimum boundary is included; maximum boundary is not
1852  if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1853  {
1854  if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1855  {
1856  if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1857  {
1858  return (true);
1859  }
1860  }
1861  }
1862  return (false);
1863  }
1864 
1865  ////////////////////////////////////////////////////////////////////////////////
1866 
1867  template<typename ContainerT, typename PointT> void
1869  {
1870  Eigen::Vector3d min;
1871  Eigen::Vector3d max;
1872  node_metadata_->getBoundingBox (min, max);
1873 
1874  double l = max[0] - min[0];
1875  double h = max[1] - min[1];
1876  double w = max[2] - min[2];
1877  file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h
1878  << ", width=" << w << " )\n";
1879 
1880  for (std::size_t i = 0; i < num_children_; i++)
1881  {
1882  children_[i]->writeVPythonVisual (file);
1883  }
1884  }
1885 
1886  ////////////////////////////////////////////////////////////////////////////////
1887 
1888  template<typename ContainerT, typename PointT> int
1890  {
1891  return (this->payload_->read (output_cloud));
1892  }
1893 
1894  ////////////////////////////////////////////////////////////////////////////////
1895 
1896  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
1898  {
1899  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1900  return (children_[index_arg]);
1901  }
1902 
1903  ////////////////////////////////////////////////////////////////////////////////
1904  template<typename ContainerT, typename PointT> std::uint64_t
1906  {
1907  return (this->payload_->getDataSize ());
1908  }
1909 
1910  ////////////////////////////////////////////////////////////////////////////////
1911 
1912  template<typename ContainerT, typename PointT> std::size_t
1914  {
1915  std::size_t loaded_children_count = 0;
1916 
1917  for (std::size_t i=0; i<8; i++)
1918  {
1919  if (children_[i] != nullptr)
1920  loaded_children_count++;
1921  }
1922 
1923  return (loaded_children_count);
1924  }
1925 
1926  ////////////////////////////////////////////////////////////////////////////////
1927 
1928  template<typename ContainerT, typename PointT> void
1930  {
1931  PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1932  node_metadata_->loadMetadataFromDisk (path);
1933 
1934  //this shouldn't be part of 'loadFromFile'
1935  this->parent_ = super;
1936 
1937  if (num_children_ > 0)
1938  recFreeChildren ();
1939 
1940  this->num_children_ = 0;
1941  this->payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
1942  }
1943 
1944  ////////////////////////////////////////////////////////////////////////////////
1945 
1946  template<typename ContainerT, typename PointT> void
1948  {
1949  std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz");
1950  boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1951  payload_->convertToXYZ (xyzfile);
1952 
1953  if (hasUnloadedChildren ())
1954  {
1955  loadChildren (false);
1956  }
1957 
1958  for (std::size_t i = 0; i < 8; i++)
1959  {
1960  if (children_[i])
1961  children_[i]->convertToXYZ ();
1962  }
1963  }
1964 
1965  ////////////////////////////////////////////////////////////////////////////////
1966 
1967  template<typename ContainerT, typename PointT> void
1969  {
1970  for (std::size_t i = 0; i < 8; i++)
1971  {
1972  if (children_[i])
1973  children_[i]->flushToDiskRecursive ();
1974  }
1975  }
1976 
1977  ////////////////////////////////////////////////////////////////////////////////
1978 
1979  template<typename ContainerT, typename PointT> void
1980  OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz)
1981  {
1982  if (indices.size () < 8)
1983  indices.resize (8);
1984 
1985  int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") );
1986  int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") );
1987  int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") );
1988 
1989  int x_offset = input_cloud->fields[x_idx].offset;
1990  int y_offset = input_cloud->fields[y_idx].offset;
1991  int z_offset = input_cloud->fields[z_idx].offset;
1992 
1993  for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1994  {
1995  PointT local_pt;
1996 
1997  local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset]));
1998  local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset]));
1999  local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset]));
2000 
2001  if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
2002  continue;
2003 
2004  if(!this->pointInBoundingBox (local_pt))
2005  {
2006  PCL_ERROR ("pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
2007  }
2008 
2009  assert (this->pointInBoundingBox (local_pt) == true);
2010 
2011  //compute the box we are in
2012  std::size_t box = 0;
2013  box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2014  assert (box < 8);
2015 
2016  //insert to the vector of indices
2017  indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
2018  }
2019  }
2020  ////////////////////////////////////////////////////////////////////////////////
2021 
2022 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2023  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
2024  makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super)
2025  {
2027 //octree_disk_node ();
2028 
2029  if (super == NULL)
2030  {
2031  thisnode->thisdir_ = path.parent_path ();
2032 
2033  if (!boost::filesystem::exists (thisnode->thisdir_))
2034  {
2035  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2036  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2037  }
2038 
2039  thisnode->thisnodeindex_ = path;
2040 
2041  thisnode->depth_ = 0;
2042  thisnode->root_node_ = thisnode;
2043  }
2044  else
2045  {
2046  thisnode->thisdir_ = path;
2047  thisnode->depth_ = super->depth_ + 1;
2048  thisnode->root_node_ = super->root_node_;
2049 
2050  if (thisnode->depth_ > thisnode->root->max_depth_)
2051  {
2052  thisnode->root->max_depth_ = thisnode->depth_;
2053  }
2054 
2055  boost::filesystem::directory_iterator diterend;
2056  bool loaded = false;
2057  for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2058  {
2059  const boost::filesystem::path& file = *diter;
2060  if (!boost::filesystem::is_directory (file))
2061  {
2062  if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
2063  {
2064  thisnode->thisnodeindex_ = file;
2065  loaded = true;
2066  break;
2067  }
2068  }
2069  }
2070 
2071  if (!loaded)
2072  {
2073  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2074  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2075  }
2076 
2077  }
2078  thisnode->max_depth_ = 0;
2079 
2080  {
2081  std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2082 
2083  f >> thisnode->min_[0];
2084  f >> thisnode->min_[1];
2085  f >> thisnode->min_[2];
2086  f >> thisnode->max_[0];
2087  f >> thisnode->max_[1];
2088  f >> thisnode->max_[2];
2089 
2090  std::string filename;
2091  f >> filename;
2092  thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2093 
2094  f.close ();
2095 
2096  thisnode->payload_.reset (new ContainerT (thisnode->thisnodestorage_));
2097  }
2098 
2099  thisnode->parent_ = super;
2100  children_.clear ();
2101  children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0));
2102  thisnode->num_children_ = 0;
2103 
2104  return (thisnode);
2105  }
2106 
2107  ////////////////////////////////////////////////////////////////////////////////
2108 
2109 //accelerate search
2110  template<typename ContainerT, typename PointT> void
2111  queryBBIntersects_noload (const boost::filesystem::path& root_node, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name)
2112  {
2113  OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2114  if (root == NULL)
2115  {
2116  std::cout << "test";
2117  }
2118  if (root->intersectsWithBoundingBox (min, max))
2119  {
2120  if (query_depth == root->max_depth_)
2121  {
2122  if (!root->payload_->empty ())
2123  {
2124  bin_name.push_back (root->thisnodestorage_.string ());
2125  }
2126  return;
2127  }
2128 
2129  for (int i = 0; i < 8; i++)
2130  {
2131  boost::filesystem::path child_dir = root->thisdir_
2132  / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2133  if (boost::filesystem::exists (child_dir))
2134  {
2135  root->children_[i] = makenode_norec (child_dir, root);
2136  root->num_children_++;
2137  queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name);
2138  }
2139  }
2140  }
2141  delete root;
2142  }
2143 
2144  ////////////////////////////////////////////////////////////////////////////////
2145 
2146  template<typename ContainerT, typename PointT> void
2147  queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name)
2148  {
2149  if (current->intersectsWithBoundingBox (min, max))
2150  {
2151  if (current->depth_ == query_depth)
2152  {
2153  if (!current->payload_->empty ())
2154  {
2155  bin_name.push_back (current->thisnodestorage_.string ());
2156  }
2157  }
2158  else
2159  {
2160  for (int i = 0; i < 8; i++)
2161  {
2162  boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2163  if (boost::filesystem::exists (child_dir))
2164  {
2165  current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2166  current->num_children_++;
2167  queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name);
2168  }
2169  }
2170  }
2171  }
2172  }
2173 #endif
2174  ////////////////////////////////////////////////////////////////////////////////
2175 
2176  }//namespace outofcore
2177 }//namespace pcl
2178 
2179 //#define PCL_INSTANTIATE....
2180 
2181 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, std::vector< int > &indices)
Get a set of points residing in a box given its bounds.
Definition: common.hpp:101
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map...
Definition: conversions.h:168
std::uint64_t size() const
Number of points in the payload.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual std::size_t getNumChildren() const
Returns the total number of children on disk.
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:61
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
virtual std::size_t getNumLoadedChildren() const
Count loaded chilren.
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list) ...
std::size_t depth_
Depth in the tree, root is 0, root&#39;s children are 1, ...
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
RandomSample applies a random sampling with uniform probability.
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
virtual void filter(PCLPointCloud2 &output)
friend OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
std::vector< OutofcoreOctreeBaseNode * > children_
The children of this node.
~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
Tests whether point falls within the input bounding box.
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v)
Recursively add points that fall into the queried bounding box up to the query_depth.
static const std::string node_index_basename
void recFreeChildren()
Method which recursively free children of this node.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
void init_root_node(const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase< ContainerT, PointT > *const tree, const boost::filesystem::path &rootname)
Create root node and directory.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:397
Define standard C methods and C++ classes that are common to all methods.
void createChild(const std::size_t idx)
Creates child node idx.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
boost::shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:61
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
std::uint64_t num_loaded_children_
Number of loaded children this node has.
virtual std::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
PCL_EXPORTS float viewScreenArea(const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
virtual std::uint64_t addDataToLeaf_and_genLOD(const AlignedPointTVector &p, const bool skip_bb_check)
Recursively add points to the leaf and children subsampling LODs on the way down. ...
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector< int > > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant&#39;s vector; This co...
std::shared_ptr< ContainerT > payload_
what holds the points.
static const std::string node_index_extension
static const std::string node_container_basename
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:412
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
Definition: io.h:281
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:399
void saveIdx(bool recursive)
Save node&#39;s metadata to file.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual std::size_t getDepth() const
virtual std::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)
Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this me...
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the pcl::outofcore::OutofcoreOctreeDiskContainer class or pcl::outofcore::OutofcoreOctreeRamContainer class, whichever it is templated against.
OutofcoreOctreeBaseNode * parent_
super-node
ExtractIndices extracts a set of indices from a point cloud.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
static const std::string node_container_extension
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator...
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
A point structure representing Euclidean xyz coordinates, and the RGB color.
static std::mutex rng_mutex_
Random number generator mutex.
std::uint64_t num_children_
Number of children on disk.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node&#39;s bounding box...
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:150
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node&#39;s bounding box.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
void setSample(unsigned int sample)
Set number of indices to be sampled.
Encapsulated class to read JSON metadata into memory, and write the JSON metadata for each node...
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...
virtual std::uint64_t addDataToLeaf(const AlignedPointTVector &p, const bool skip_bb_check=true)
add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point...
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
friend void queryBBIntersects_noload(const boost::filesystem::path &rootnode, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list< std::string > &bin_name)
Non-class method which performs a bounding box query without loading any of the point cloud data from...