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