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