Point Cloud Library (PCL)  1.11.0
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>
67  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename = "node";
68 
69  template<typename ContainerT, typename PointT>
70  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename = "node";
71 
72  template<typename ContainerT, typename PointT>
73  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension = ".oct_idx";
74 
75  template<typename ContainerT, typename PointT>
76  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension = ".oct_dat";
77 
78  template<typename ContainerT, typename PointT>
79  std::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_;
80 
81  template<typename ContainerT, typename PointT>
82  std::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_;
83 
84  template<typename ContainerT, typename PointT>
85  const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125;
86 
87  template<typename ContainerT, typename PointT>
88  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension = ".pcd";
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)
115  , num_loaded_children_ (0)
116  , payload_ ()
117  , node_metadata_ (new OutofcoreOctreeNodeMetadata)
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)
189  , num_loaded_children_ (0)
190  , payload_ ()
191  , node_metadata_ (new OutofcoreOctreeNodeMetadata ())
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
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_ != NULL);
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 < std::vector<int> > 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_ != NULL);
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_, double((this->root_node_->m_tree_->getDepth () - depth_)));
580  const std::uint64_t 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_ != NULL);
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 std::vector< int > () );
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<std::vector<int> > 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_ != NULL );
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",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 
1337  Eigen::Vector3d my_min = min_bb;
1338  Eigen::Vector3d my_max = max_bb;
1339 
1340  if (intersectsWithBoundingBox (my_min, my_max))
1341  {
1342  if (this->depth_ < query_depth)
1343  {
1344  if (this->getNumChildren () > 0)
1345  {
1346  for (std::size_t i = 0; i < 8; i++)
1347  {
1348  if (children_[i])
1349  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1350  }
1351  }
1352  else if (hasUnloadedChildren ())
1353  {
1354  loadChildren (false);
1355 
1356  for (std::size_t i = 0; i < 8; i++)
1357  {
1358  if (children_[i])
1359  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1360  }
1361  }
1362  return;
1363  }
1364 
1365  if (payload_->getDataSize () > 0)
1366  {
1367  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1368  }
1369  }
1370  }
1371  ////////////////////////////////////////////////////////////////////////////////
1372 
1373  template<typename ContainerT, typename PointT> void
1374  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)
1375  {
1376  std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1377  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1378 
1379  // If the queried bounding box has any intersection with this node's bounding box
1380  if (intersectsWithBoundingBox (min_bb, max_bb))
1381  {
1382  // If we aren't at the max desired depth
1383  if (this->depth_ < query_depth)
1384  {
1385  //if this node doesn't have any children, we are at the max depth for this query
1386  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1387  loadChildren (false);
1388 
1389  //if this node has children
1390  if (num_children_ > 0)
1391  {
1392  //recursively store any points that fall into the queried bounding box into v and return
1393  for (std::size_t i = 0; i < 8; i++)
1394  {
1395  if (children_[i])
1396  children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1397  }
1398  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1399  return;
1400  }
1401  }
1402  else //otherwise if we are at the max depth
1403  {
1404  //get all the points from the payload and return (easy with PCLPointCloud2)
1406  pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ());
1407  //load all the data in this node from disk
1408  payload_->readRange (0, payload_->size (), tmp_blob);
1409 
1410  if( tmp_blob->width*tmp_blob->height == 0 )
1411  return;
1412 
1413  //if this node's bounding box falls completely within the queried bounding box, keep all the points
1414  if (inBoundingBox (min_bb, max_bb))
1415  {
1416  //concatenate all of what was just read into the main dst_blob
1417  //(is it safe to do in place?)
1418 
1419  //if there is already something in the destination blob (remember this method is recursive)
1420  if( dst_blob->width*dst_blob->height != 0 )
1421  {
1422  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1423  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1424  int res = pcl::concatenate (*dst_blob, *tmp_blob, *dst_blob);
1425  (void)res;
1426  assert (res == 1);
1427 
1428  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1429  }
1430  //otherwise, just copy the tmp_blob into the dst_blob
1431  else
1432  {
1433  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1434  pcl::copyPointCloud (*tmp_blob, *dst_blob);
1435  assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1436  }
1437  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1438  return;
1439  }
1440  //otherwise queried bounding box only partially intersects this
1441  //node's bounding box, so we have to check all the points in
1442  //this box for intersection with queried bounding box
1443 
1444 // 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] );
1445  //put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox)
1446  typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () );
1447  pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud );
1448  assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height );
1449 
1450  Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
1451  Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
1452 
1453  std::vector<int> indices;
1454 
1455  pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices );
1456  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1457  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () );
1458 
1459  if ( !indices.empty () )
1460  {
1461  if( dst_blob->width*dst_blob->height > 0 )
1462  {
1463  //need a new tmp destination with extracted points within BB
1464  pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ());
1465 
1466  //copy just the points marked in indices
1467  pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb );
1468  assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1469  assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1470  //concatenate those points into the returned dst_blob
1471  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1472  std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1473  (void)orig_points_in_destination;
1474  int res = pcl::concatenate (*dst_blob, *tmp_blob_within_bb, *dst_blob);
1475  (void)res;
1476  assert (res == 1);
1477  assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1478 
1479  }
1480  else
1481  {
1482  pcl::copyPointCloud ( *tmp_blob, indices, *dst_blob );
1483  assert ( dst_blob->width*dst_blob->height == indices.size () );
1484  }
1485  }
1486  }
1487  }
1488 
1489  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1490  }
1491 
1492  template<typename ContainerT, typename PointT> void
1493  OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::size_t query_depth, AlignedPointTVector& v)
1494  {
1495 
1496  //if the queried bounding box has any intersection with this node's bounding box
1497  if (intersectsWithBoundingBox (min_bb, max_bb))
1498  {
1499  //if we aren't at the max desired depth
1500  if (this->depth_ < query_depth)
1501  {
1502  //if this node doesn't have any children, we are at the max depth for this query
1503  if (this->hasUnloadedChildren ())
1504  {
1505  this->loadChildren (false);
1506  }
1507 
1508  //if this node has children
1509  if (getNumChildren () > 0)
1510  {
1511  if(hasUnloadedChildren ())
1512  loadChildren (false);
1513 
1514  //recursively store any points that fall into the queried bounding box into v and return
1515  for (std::size_t i = 0; i < 8; i++)
1516  {
1517  if (children_[i])
1518  children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1519  }
1520  return;
1521  }
1522  }
1523  //otherwise if we are at the max depth
1524  else
1525  {
1526  //if this node's bounding box falls completely within the queried bounding box
1527  if (inBoundingBox (min_bb, max_bb))
1528  {
1529  //get all the points from the payload and return
1530  AlignedPointTVector payload_cache;
1531  payload_->readRange (0, payload_->size (), payload_cache);
1532  v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1533  return;
1534  }
1535  //otherwise queried bounding box only partially intersects this
1536  //node's bounding box, so we have to check all the points in
1537  //this box for intersection with queried bounding box
1538  //read _all_ the points in from the disk container
1539  AlignedPointTVector payload_cache;
1540  payload_->readRange (0, payload_->size (), payload_cache);
1541 
1542  std::uint64_t len = payload_->size ();
1543  //iterate through each of them
1544  for (std::uint64_t i = 0; i < len; i++)
1545  {
1546  const PointT& p = payload_cache[i];
1547  //if it falls within this bounding box
1548  if (pointInBoundingBox (min_bb, max_bb, p))
1549  {
1550  //store it in the list
1551  v.push_back (p);
1552  }
1553  else
1554  {
1555  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]);
1556  }
1557  }
1558  }
1559  }
1560  }
1561 
1562  ////////////////////////////////////////////////////////////////////////////////
1563  template<typename ContainerT, typename PointT> void
1564  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)
1565  {
1566  if (intersectsWithBoundingBox (min_bb, max_bb))
1567  {
1568  if (this->depth_ < query_depth)
1569  {
1570  if (this->hasUnloadedChildren ())
1571  this->loadChildren (false);
1572 
1573  if (this->getNumChildren () > 0)
1574  {
1575  for (std::size_t i=0; i<8; i++)
1576  {
1577  //recursively traverse (depth first)
1578  if (children_[i]!=nullptr)
1579  children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1580  }
1581  return;
1582  }
1583  }
1584  //otherwise, at max depth --> read from disk, subsample, concatenate
1585  else
1586  {
1587 
1588  if (inBoundingBox (min_bb, max_bb))
1589  {
1590  pcl::PCLPointCloud2::Ptr tmp_blob;
1591  this->payload_->read (tmp_blob);
1592  std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1593 
1594  double sample_points = static_cast<double>(num_pts) * percent;
1595  if (num_pts > 0)
1596  {
1597  //always sample at least one point
1598  sample_points = sample_points > 1 ? sample_points : 1;
1599  }
1600  else
1601  {
1602  return;
1603  }
1604 
1605 
1607  random_sampler.setInputCloud (tmp_blob);
1608 
1609  pcl::PCLPointCloud2::Ptr downsampled_points (new pcl::PCLPointCloud2 ());
1610 
1611  //set sample size as percent * number of points read
1612  random_sampler.setSample (static_cast<unsigned int> (sample_points));
1613 
1615  extractor.setInputCloud (tmp_blob);
1616 
1617  pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ());
1618  random_sampler.filter (*downsampled_cloud_indices);
1619  extractor.setIndices (downsampled_cloud_indices);
1620  extractor.filter (*downsampled_points);
1621 
1622  //concatenate the result into the destination cloud
1623  pcl::concatenate (*dst_blob, *downsampled_points, *dst_blob);
1624  }
1625  }
1626  }
1627  }
1628 
1629 
1630  ////////////////////////////////////////////////////////////////////////////////
1631  template<typename ContainerT, typename PointT> void
1632  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)
1633  {
1634  //check if the queried bounding box has any intersection with this node's bounding box
1635  if (intersectsWithBoundingBox (min_bb, max_bb))
1636  {
1637  //if we are not at the max depth for queried nodes
1638  if (this->depth_ < query_depth)
1639  {
1640  //check if we don't have children
1641  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1642  {
1643  loadChildren (false);
1644  }
1645  //if we do have children
1646  if (num_children_ > 0)
1647  {
1648  //recursively add their valid points within the queried bounding box to the list v
1649  for (std::size_t i = 0; i < 8; i++)
1650  {
1651  if (children_[i])
1652  children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1653  }
1654  return;
1655  }
1656  }
1657  //otherwise we are at the max depth, so we add all our points or some of our points
1658  else
1659  {
1660  //if this node's bounding box falls completely within the queried bounding box
1661  if (inBoundingBox (min_bb, max_bb))
1662  {
1663  //add a random sample of all the points
1664  AlignedPointTVector payload_cache;
1665  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1666  dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1667  return;
1668  }
1669  //otherwise the queried bounding box only partially intersects with this node's bounding box
1670  //brute force selection of all valid points
1671  AlignedPointTVector payload_cache_within_region;
1672  {
1673  AlignedPointTVector payload_cache;
1674  payload_->readRange (0, payload_->size (), payload_cache);
1675  for (std::size_t i = 0; i < payload_->size (); i++)
1676  {
1677  const PointT& p = payload_cache[i];
1678  if (pointInBoundingBox (min_bb, max_bb, p))
1679  {
1680  payload_cache_within_region.push_back (p);
1681  }
1682  }
1683  }//force the payload cache to deconstruct here
1684 
1685  //use STL random_shuffle and push back a random selection of the points onto our list
1686  std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1687  std::size_t numpick = static_cast<std::size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));;
1688 
1689  for (std::size_t i = 0; i < numpick; i++)
1690  {
1691  dst.push_back (payload_cache_within_region[i]);
1692  }
1693  }
1694  }
1695  }
1696  ////////////////////////////////////////////////////////////////////////////////
1697 
1698 //dir is current level. we put this nodes files into it
1699  template<typename ContainerT, typename PointT>
1700  OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super)
1701  : m_tree_ ()
1702  , root_node_ ()
1703  , parent_ ()
1704  , depth_ ()
1705  , children_ (8, nullptr)
1706  , num_children_ ()
1707  , num_loaded_children_ (0)
1708  , payload_ ()
1709  , node_metadata_ (new OutofcoreOctreeNodeMetadata)
1710  {
1711  node_metadata_->setOutofcoreVersion (3);
1712 
1713  if (super == nullptr)
1714  {
1715  PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1716  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1717  }
1718 
1719  this->parent_ = super;
1720  root_node_ = super->root_node_;
1721  m_tree_ = super->root_node_->m_tree_;
1722  assert (m_tree_ != NULL);
1723 
1724  depth_ = super->depth_ + 1;
1725  num_children_ = 0;
1726 
1727  node_metadata_->setBoundingBox (bb_min, bb_max);
1728 
1729  std::string uuid_idx;
1730  std::string uuid_cont;
1733 
1734  std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension;
1735 
1736  std::string node_container_name;
1737  node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension;
1738 
1739  node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1740  node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
1741  node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name));
1742 
1743  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
1744 
1745  payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
1746  this->saveIdx (false);
1747  }
1748 
1749  ////////////////////////////////////////////////////////////////////////////////
1750 
1751  template<typename ContainerT, typename PointT> void
1753  {
1754  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1755  {
1756  loadChildren (false);
1757  }
1758 
1759  for (std::size_t i = 0; i < num_children_; i++)
1760  {
1761  children_[i]->copyAllCurrentAndChildPointsRec (v);
1762  }
1763 
1764  AlignedPointTVector payload_cache;
1765  payload_->readRange (0, payload_->size (), payload_cache);
1766 
1767  {
1768  v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1769  }
1770  }
1771 
1772  ////////////////////////////////////////////////////////////////////////////////
1773 
1774  template<typename ContainerT, typename PointT> void
1776  {
1777  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1778  {
1779  loadChildren (false);
1780  }
1781 
1782  for (std::size_t i = 0; i < 8; i++)
1783  {
1784  if (children_[i])
1785  children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1786  }
1787 
1788  std::vector<PointT> payload_cache;
1789  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1790 
1791  for (std::size_t i = 0; i < payload_cache.size (); i++)
1792  {
1793  v.push_back (payload_cache[i]);
1794  }
1795  }
1796 
1797  ////////////////////////////////////////////////////////////////////////////////
1798 
1799  template<typename ContainerT, typename PointT> inline bool
1800  OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1801  {
1802  Eigen::Vector3d min, max;
1803  node_metadata_->getBoundingBox (min, max);
1804 
1805  //Check whether any portion of min_bb, max_bb falls within min,max
1806  if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1807  {
1808  if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1809  {
1810  if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1811  {
1812  return (true);
1813  }
1814  }
1815  }
1816 
1817  return (false);
1818  }
1819  ////////////////////////////////////////////////////////////////////////////////
1820 
1821  template<typename ContainerT, typename PointT> inline bool
1822  OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1823  {
1824  Eigen::Vector3d min, max;
1825 
1826  node_metadata_->getBoundingBox (min, max);
1827 
1828  if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1829  {
1830  if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1831  {
1832  if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1833  {
1834  return (true);
1835  }
1836  }
1837  }
1838 
1839  return (false);
1840  }
1841  ////////////////////////////////////////////////////////////////////////////////
1842 
1843  template<typename ContainerT, typename PointT> inline bool
1844  OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb,
1845  const PointT& p)
1846  {
1847  //by convention, minimum boundary is included; maximum boundary is not
1848  if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1849  {
1850  if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1851  {
1852  if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1853  {
1854  return (true);
1855  }
1856  }
1857  }
1858  return (false);
1859  }
1860 
1861  ////////////////////////////////////////////////////////////////////////////////
1862 
1863  template<typename ContainerT, typename PointT> void
1865  {
1866  Eigen::Vector3d min;
1867  Eigen::Vector3d max;
1868  node_metadata_->getBoundingBox (min, max);
1869 
1870  double l = max[0] - min[0];
1871  double h = max[1] - min[1];
1872  double w = max[2] - min[2];
1873  file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h
1874  << ", width=" << w << " )\n";
1875 
1876  for (std::size_t i = 0; i < num_children_; i++)
1877  {
1878  children_[i]->writeVPythonVisual (file);
1879  }
1880  }
1881 
1882  ////////////////////////////////////////////////////////////////////////////////
1883 
1884  template<typename ContainerT, typename PointT> int
1886  {
1887  return (this->payload_->read (output_cloud));
1888  }
1889 
1890  ////////////////////////////////////////////////////////////////////////////////
1891 
1892  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
1894  {
1895  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1896  return (children_[index_arg]);
1897  }
1898 
1899  ////////////////////////////////////////////////////////////////////////////////
1900  template<typename ContainerT, typename PointT> std::uint64_t
1902  {
1903  return (this->payload_->getDataSize ());
1904  }
1905 
1906  ////////////////////////////////////////////////////////////////////////////////
1907 
1908  template<typename ContainerT, typename PointT> std::size_t
1910  {
1911  std::size_t loaded_children_count = 0;
1912 
1913  for (std::size_t i=0; i<8; i++)
1914  {
1915  if (children_[i] != nullptr)
1916  loaded_children_count++;
1917  }
1918 
1919  return (loaded_children_count);
1920  }
1921 
1922  ////////////////////////////////////////////////////////////////////////////////
1923 
1924  template<typename ContainerT, typename PointT> void
1926  {
1927  PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1928  node_metadata_->loadMetadataFromDisk (path);
1929 
1930  //this shouldn't be part of 'loadFromFile'
1931  this->parent_ = super;
1932 
1933  if (num_children_ > 0)
1934  recFreeChildren ();
1935 
1936  this->num_children_ = 0;
1937  this->payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
1938  }
1939 
1940  ////////////////////////////////////////////////////////////////////////////////
1941 
1942  template<typename ContainerT, typename PointT> void
1944  {
1945  std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz");
1946  boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1947  payload_->convertToXYZ (xyzfile);
1948 
1949  if (hasUnloadedChildren ())
1950  {
1951  loadChildren (false);
1952  }
1953 
1954  for (std::size_t i = 0; i < 8; i++)
1955  {
1956  if (children_[i])
1957  children_[i]->convertToXYZ ();
1958  }
1959  }
1960 
1961  ////////////////////////////////////////////////////////////////////////////////
1962 
1963  template<typename ContainerT, typename PointT> void
1965  {
1966  for (std::size_t i = 0; i < 8; i++)
1967  {
1968  if (children_[i])
1969  children_[i]->flushToDiskRecursive ();
1970  }
1971  }
1972 
1973  ////////////////////////////////////////////////////////////////////////////////
1974 
1975  template<typename ContainerT, typename PointT> void
1976  OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz)
1977  {
1978  if (indices.size () < 8)
1979  indices.resize (8);
1980 
1981  int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") );
1982  int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") );
1983  int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") );
1984 
1985  int x_offset = input_cloud->fields[x_idx].offset;
1986  int y_offset = input_cloud->fields[y_idx].offset;
1987  int z_offset = input_cloud->fields[z_idx].offset;
1988 
1989  for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1990  {
1991  PointT local_pt;
1992 
1993  local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset]));
1994  local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset]));
1995  local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset]));
1996 
1997  if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
1998  continue;
1999 
2000  if(!this->pointInBoundingBox (local_pt))
2001  {
2002  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);
2003  }
2004 
2005  assert (this->pointInBoundingBox (local_pt) == true);
2006 
2007  //compute the box we are in
2008  std::size_t box = 0;
2009  box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2010  assert (box < 8);
2011 
2012  //insert to the vector of indices
2013  indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
2014  }
2015  }
2016  ////////////////////////////////////////////////////////////////////////////////
2017 
2018 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2019  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
2020  makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super)
2021  {
2023 //octree_disk_node ();
2024 
2025  if (super == NULL)
2026  {
2027  thisnode->thisdir_ = path.parent_path ();
2028 
2029  if (!boost::filesystem::exists (thisnode->thisdir_))
2030  {
2031  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2032  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2033  }
2034 
2035  thisnode->thisnodeindex_ = path;
2036 
2037  thisnode->depth_ = 0;
2038  thisnode->root_node_ = thisnode;
2039  }
2040  else
2041  {
2042  thisnode->thisdir_ = path;
2043  thisnode->depth_ = super->depth_ + 1;
2044  thisnode->root_node_ = super->root_node_;
2045 
2046  if (thisnode->depth_ > thisnode->root->max_depth_)
2047  {
2048  thisnode->root->max_depth_ = thisnode->depth_;
2049  }
2050 
2051  boost::filesystem::directory_iterator diterend;
2052  bool loaded = false;
2053  for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2054  {
2055  const boost::filesystem::path& file = *diter;
2056  if (!boost::filesystem::is_directory (file))
2057  {
2058  if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
2059  {
2060  thisnode->thisnodeindex_ = file;
2061  loaded = true;
2062  break;
2063  }
2064  }
2065  }
2066 
2067  if (!loaded)
2068  {
2069  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2070  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2071  }
2072 
2073  }
2074  thisnode->max_depth_ = 0;
2075 
2076  {
2077  std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2078 
2079  f >> thisnode->min_[0];
2080  f >> thisnode->min_[1];
2081  f >> thisnode->min_[2];
2082  f >> thisnode->max_[0];
2083  f >> thisnode->max_[1];
2084  f >> thisnode->max_[2];
2085 
2086  std::string filename;
2087  f >> filename;
2088  thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2089 
2090  f.close ();
2091 
2092  thisnode->payload_.reset (new ContainerT (thisnode->thisnodestorage_));
2093  }
2094 
2095  thisnode->parent_ = super;
2096  children_.clear ();
2097  children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0));
2098  thisnode->num_children_ = 0;
2099 
2100  return (thisnode);
2101  }
2102 
2103  ////////////////////////////////////////////////////////////////////////////////
2104 
2105 //accelerate search
2106  template<typename ContainerT, typename PointT> void
2107  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)
2108  {
2109  OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2110  if (root == NULL)
2111  {
2112  std::cout << "test";
2113  }
2114  if (root->intersectsWithBoundingBox (min, max))
2115  {
2116  if (query_depth == root->max_depth_)
2117  {
2118  if (!root->payload_->empty ())
2119  {
2120  bin_name.push_back (root->thisnodestorage_.string ());
2121  }
2122  return;
2123  }
2124 
2125  for (int i = 0; i < 8; i++)
2126  {
2127  boost::filesystem::path child_dir = root->thisdir_
2128  / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2129  if (boost::filesystem::exists (child_dir))
2130  {
2131  root->children_[i] = makenode_norec (child_dir, root);
2132  root->num_children_++;
2133  queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name);
2134  }
2135  }
2136  }
2137  delete root;
2138  }
2139 
2140  ////////////////////////////////////////////////////////////////////////////////
2141 
2142  template<typename ContainerT, typename PointT> void
2143  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)
2144  {
2145  if (current->intersectsWithBoundingBox (min, max))
2146  {
2147  if (current->depth_ == query_depth)
2148  {
2149  if (!current->payload_->empty ())
2150  {
2151  bin_name.push_back (current->thisnodestorage_.string ());
2152  }
2153  }
2154  else
2155  {
2156  for (int i = 0; i < 8; i++)
2157  {
2158  boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2159  if (boost::filesystem::exists (child_dir))
2160  {
2161  current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2162  current->num_children_++;
2163  queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name);
2164  }
2165  }
2166  }
2167  }
2168  }
2169 #endif
2170  ////////////////////////////////////////////////////////////////////////////////
2171 
2172  }//namespace outofcore
2173 }//namespace pcl
2174 
2175 //#define PCL_INSTANTIATE....
2176 
2177 #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
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
Definition: conversions.h:168
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:428
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:64
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
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:61
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...
~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 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
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.hpp:53
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:413
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:121
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 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's vector; This co...
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.
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:282
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void saveIdx(bool recursive)
Save node'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::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
ExtractIndices extracts a set of indices from a point cloud.
void filter(std::vector< int > &indices)
Calls the filtering method and returns the filtered point cloud indices.
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
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 chilren by testing the children_ array; used to update num_loaded_chilren...