Point Cloud Library (PCL)  1.11.1
octree_base_node.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  * Copyright (c) 2012, Urban Robotics, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of Willow Garage, Inc. nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
43 
44 // C++
45 #include <iostream>
46 #include <fstream>
47 #include <random>
48 #include <sstream>
49 #include <string>
50 #include <exception>
51 
52 #include <pcl/common/common.h>
53 #include <pcl/common/utils.h> // pcl::utils::ignore
54 #include <pcl/visualization/common/common.h>
55 #include <pcl/outofcore/octree_base_node.h>
56 #include <pcl/filters/random_sample.h>
57 #include <pcl/filters/extract_indices.h>
58 
59 // JSON
60 #include <pcl/outofcore/cJSON.h>
61 
62 namespace pcl
63 {
64  namespace outofcore
65  {
66 
67  template<typename ContainerT, typename PointT>
69 
70  template<typename ContainerT, typename PointT>
72 
73  template<typename ContainerT, typename PointT>
75 
76  template<typename ContainerT, typename PointT>
78 
79  template<typename ContainerT, typename PointT>
81 
82  template<typename ContainerT, typename PointT>
84 
85  template<typename ContainerT, typename PointT>
87 
88  template<typename ContainerT, typename PointT>
90 
91  template<typename ContainerT, typename PointT>
93  : m_tree_ ()
94  , root_node_ (NULL)
95  , parent_ (NULL)
96  , depth_ (0)
97  , children_ (8, nullptr)
98  , num_children_ (0)
99  , num_loaded_children_ (0)
100  , payload_ ()
101  , node_metadata_ (new OutofcoreOctreeNodeMetadata)
102  {
103  node_metadata_->setOutofcoreVersion (3);
104  }
105 
106  ////////////////////////////////////////////////////////////////////////////////
107 
108  template<typename ContainerT, typename PointT>
110  : m_tree_ ()
111  , root_node_ ()
112  , parent_ (super)
113  , depth_ ()
114  , children_ (8, nullptr)
115  , num_children_ (0)
116  , num_loaded_children_ (0)
117  , payload_ ()
118  , node_metadata_ (new OutofcoreOctreeNodeMetadata)
119  {
120  node_metadata_->setOutofcoreVersion (3);
121 
122  //Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL)
123  if (super == nullptr)
124  {
125  node_metadata_->setDirectoryPathname (directory_path.parent_path ());
126  node_metadata_->setMetadataFilename (directory_path);
127  depth_ = 0;
128  root_node_ = this;
129 
130  //Check if the specified directory to load currently exists; if not, don't continue
131  if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
132  {
133  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ());
134  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
135  }
136  }
137  else
138  {
139  node_metadata_->setDirectoryPathname (directory_path);
140  depth_ = super->getDepth () + 1;
141  root_node_ = super->root_node_;
142 
143  boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator
144 
145  //flag to test if the desired metadata file was found
146  bool b_loaded = false;
147 
148  for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
149  {
150  const boost::filesystem::path& file = *directory_it;
151 
152  if (!boost::filesystem::is_directory (file))
153  {
154  if (boost::filesystem::extension (file) == node_index_extension)
155  {
156  b_loaded = node_metadata_->loadMetadataFromDisk (file);
157  break;
158  }
159  }
160  }
161 
162  if (!b_loaded)
163  {
164  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
165  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
166  }
167  }
168 
169  //load the metadata
170  loadFromFile (node_metadata_->getMetadataFilename (), super);
171 
172  //set the number of children in this node
173  num_children_ = this->countNumChildren ();
174 
175  if (load_all)
176  {
177  loadChildren (true);
178  }
179  }
180 ////////////////////////////////////////////////////////////////////////////////
181 
182  template<typename ContainerT, typename PointT>
183  OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
184  : m_tree_ (tree)
185  , root_node_ ()
186  , parent_ ()
187  , depth_ ()
188  , children_ (8, nullptr)
189  , num_children_ (0)
190  , num_loaded_children_ (0)
191  , payload_ ()
192  , node_metadata_ (new OutofcoreOctreeNodeMetadata ())
193  {
194  assert (tree != NULL);
195  node_metadata_->setOutofcoreVersion (3);
196  init_root_node (bb_min, bb_max, tree, root_name);
197  }
198 
199  ////////////////////////////////////////////////////////////////////////////////
200 
201  template<typename ContainerT, typename PointT> void
202  OutofcoreOctreeBaseNode<ContainerT, PointT>::init_root_node (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name)
203  {
204  assert (tree != NULL);
205 
206  parent_ = nullptr;
207  root_node_ = this;
208  m_tree_ = tree;
209  depth_ = 0;
210 
211  //Mark the children as unallocated
212  num_children_ = 0;
213 
214  Eigen::Vector3d tmp_max = bb_max;
215  Eigen::Vector3d tmp_min = bb_min;
216 
217  // Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded
218  double epsilon = 1e-8;
219  tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
220 
221  node_metadata_->setBoundingBox (tmp_min, tmp_max);
222  node_metadata_->setDirectoryPathname (root_name.parent_path ());
223  node_metadata_->setOutofcoreVersion (3);
224 
225  // If the root directory doesn't exist create it
226  if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
227  {
228  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
229  }
230  // If the root directory is a file, do not continue
231  else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
232  {
233  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
234  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
235  }
236 
237  // Create a unique id for node file name
238  std::string uuid;
239 
241 
242  std::string node_container_name;
243 
244  node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension;
245 
246  node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
247  node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
248 
249  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
250  node_metadata_->serializeMetadataToDisk ();
251 
252  // Create data container, ie octree_disk_container, octree_ram_container
253  payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
254  }
255 
256  ////////////////////////////////////////////////////////////////////////////////
257 
258  template<typename ContainerT, typename PointT>
260  {
261  // Recursively delete all children and this nodes data
262  recFreeChildren ();
263  }
264 
265  ////////////////////////////////////////////////////////////////////////////////
266 
267  template<typename ContainerT, typename PointT> std::size_t
269  {
270  std::size_t child_count = 0;
271 
272  for(std::size_t i=0; i<8; i++)
273  {
274  boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
275  if (boost::filesystem::exists (child_path))
276  child_count++;
277  }
278  return (child_count);
279  }
280 
281  ////////////////////////////////////////////////////////////////////////////////
282 
283  template<typename ContainerT, typename PointT> void
285  {
286  node_metadata_->serializeMetadataToDisk ();
287 
288  if (recursive)
289  {
290  for (std::size_t i = 0; i < 8; i++)
291  {
292  if (children_[i])
293  children_[i]->saveIdx (true);
294  }
295  }
296  }
297 
298  ////////////////////////////////////////////////////////////////////////////////
299 
300  template<typename ContainerT, typename PointT> bool
302  {
303  return (this->getNumLoadedChildren () < this->getNumChildren ());
304  }
305  ////////////////////////////////////////////////////////////////////////////////
306 
307  template<typename ContainerT, typename PointT> void
309  {
310  //if we have fewer children loaded than exist on disk, load them
311  if (num_loaded_children_ < this->getNumChildren ())
312  {
313  //check all 8 possible child directories
314  for (int i = 0; i < 8; i++)
315  {
316  boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
317  //if the directory exists and the child hasn't been created (set to 0 by this node's constructor)
318  if (boost::filesystem::exists (child_dir) && this->children_[i] == nullptr)
319  {
320  //load the child node
321  this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive);
322  //keep track of the children loaded
323  num_loaded_children_++;
324  }
325  }
326  }
327  assert (num_loaded_children_ == this->getNumChildren ());
328  }
329  ////////////////////////////////////////////////////////////////////////////////
330 
331  template<typename ContainerT, typename PointT> void
333  {
334  if (num_children_ == 0)
335  {
336  return;
337  }
338 
339  for (std::size_t i = 0; i < 8; i++)
340  {
341  delete static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(children_[i]);
342  }
343  children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (nullptr));
344  num_children_ = 0;
345  }
346  ////////////////////////////////////////////////////////////////////////////////
347 
348  template<typename ContainerT, typename PointT> std::uint64_t
350  {
351  //quit if there are no points to add
352  if (p.empty ())
353  {
354  return (0);
355  }
356 
357  //if this depth is the max depth of the tree, then add the points
358  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
359  return (addDataAtMaxDepth( p, skip_bb_check));
360 
361  if (hasUnloadedChildren ())
362  loadChildren (false);
363 
364  std::vector < std::vector<const PointT*> > c;
365  c.resize (8);
366  for (std::size_t i = 0; i < 8; i++)
367  {
368  c[i].reserve (p.size () / 8);
369  }
370 
371  const std::size_t len = p.size ();
372  for (std::size_t i = 0; i < len; i++)
373  {
374  const PointT& pt = p[i];
375 
376  if (!skip_bb_check)
377  {
378  if (!this->pointInBoundingBox (pt))
379  {
380  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
381  continue;
382  }
383  }
384 
385  std::uint8_t box = 0;
386  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
387 
388  box = static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
389  c[static_cast<std::size_t>(box)].push_back (&pt);
390  }
391 
392  std::uint64_t points_added = 0;
393  for (std::size_t i = 0; i < 8; i++)
394  {
395  if (c[i].empty ())
396  continue;
397  if (!children_[i])
398  createChild (i);
399  points_added += children_[i]->addDataToLeaf (c[i], true);
400  c[i].clear ();
401  }
402  return (points_added);
403  }
404  ////////////////////////////////////////////////////////////////////////////////
405 
406 
407  template<typename ContainerT, typename PointT> std::uint64_t
408  OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check)
409  {
410  if (p.empty ())
411  {
412  return (0);
413  }
414 
415  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
416  {
417  //trust me, just add the points
418  if (skip_bb_check)
419  {
420  root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
421 
422  payload_->insertRange (p.data (), p.size ());
423 
424  return (p.size ());
425  }
426  //check which points belong to this node, throw away the rest
427  std::vector<const PointT*> buff;
428  for (const PointT* pt : p)
429  {
430  if(pointInBoundingBox(*pt))
431  {
432  buff.push_back(pt);
433  }
434  }
435 
436  if (!buff.empty ())
437  {
438  root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
439  payload_->insertRange (buff.data (), buff.size ());
440 // payload_->insertRange ( buff );
441 
442  }
443  return (buff.size ());
444  }
445 
446  if (this->hasUnloadedChildren ())
447  {
448  loadChildren (false);
449  }
450 
451  std::vector < std::vector<const PointT*> > c;
452  c.resize (8);
453  for (std::size_t i = 0; i < 8; i++)
454  {
455  c[i].reserve (p.size () / 8);
456  }
457 
458  const std::size_t len = p.size ();
459  for (std::size_t i = 0; i < len; i++)
460  {
461  //const PointT& pt = p[i];
462  if (!skip_bb_check)
463  {
464  if (!this->pointInBoundingBox (*p[i]))
465  {
466  // std::cerr << "failed to place point!!!" << std::endl;
467  continue;
468  }
469  }
470 
471  std::uint8_t box = 00;
472  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
473  //hash each coordinate to the appropriate octant
474  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] )));
475  //3 bit, 8 octants
476  c[box].push_back (p[i]);
477  }
478 
479  std::uint64_t points_added = 0;
480  for (std::size_t i = 0; i < 8; i++)
481  {
482  if (c[i].empty ())
483  continue;
484  if (!children_[i])
485  createChild (i);
486  points_added += children_[i]->addDataToLeaf (c[i], true);
487  c[i].clear ();
488  }
489  return (points_added);
490  }
491  ////////////////////////////////////////////////////////////////////////////////
492 
493 
494  template<typename ContainerT, typename PointT> std::uint64_t
495  OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check)
496  {
497  assert (this->root_node_->m_tree_ != NULL);
498 
499  if (input_cloud->height*input_cloud->width == 0)
500  return (0);
501 
502  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
503  return (addDataAtMaxDepth (input_cloud, true));
504 
505  if( num_children_ < 8 )
506  if(hasUnloadedChildren ())
507  loadChildren (false);
508 
509  if( !skip_bb_check )
510  {
511 
512  //indices to store the points for each bin
513  //these lists will be used to copy data to new point clouds and pass down recursively
514  std::vector < std::vector<int> > indices;
515  indices.resize (8);
516 
517  this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
518 
519  for(std::size_t k=0; k<indices.size (); k++)
520  {
521  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
522  }
523 
524  std::uint64_t points_added = 0;
525 
526  for(std::size_t i=0; i<8; i++)
527  {
528  if ( indices[i].empty () )
529  continue;
530 
531  if (children_[i] == nullptr)
532  {
533  createChild (i);
534  }
535 
536  pcl::PCLPointCloud2::Ptr dst_cloud (new pcl::PCLPointCloud2 () );
537 
538  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
539 
540  //copy the points from extracted indices from input cloud to destination cloud
541  pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ;
542 
543  //recursively add the new cloud to the data
544  points_added += children_[i]->addPointCloud (dst_cloud, false);
545  indices[i].clear ();
546  }
547 
548  return (points_added);
549  }
550 
551  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
552 
553  return 0;
554  }
555 
556 
557  ////////////////////////////////////////////////////////////////////////////////
558  template<typename ContainerT, typename PointT> void
560  {
561  assert (this->root_node_->m_tree_ != NULL);
562 
563  AlignedPointTVector sampleBuff;
564  if (!skip_bb_check)
565  {
566  for (const PointT& pt: p)
567  {
568  if (pointInBoundingBox(pt))
569  {
570  sampleBuff.push_back(pt);
571  }
572  }
573  }
574  else
575  {
576  sampleBuff = p;
577  }
578 
579  // Derive percentage from specified sample_percent and tree depth
580  const double percent = pow(sample_percent_, double((this->root_node_->m_tree_->getDepth () - depth_)));
581  const std::uint64_t samplesize = static_cast<std::uint64_t>(percent * static_cast<double>(sampleBuff.size()));
582  const std::uint64_t inputsize = sampleBuff.size();
583 
584  if(samplesize > 0)
585  {
586  // Resize buffer to sample size
587  insertBuff.resize(samplesize);
588 
589  // Create random number generator
590  std::lock_guard<std::mutex> lock(rng_mutex_);
591  std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
592 
593  // Randomly pick sampled points
594  for(std::uint64_t i = 0; i < samplesize; ++i)
595  {
596  std::uint64_t buffstart = buffdist(rng_);
597  insertBuff[i] = ( sampleBuff[buffstart] );
598  }
599  }
600  // Have to do it the slow way
601  else
602  {
603  std::lock_guard<std::mutex> lock(rng_mutex_);
604  std::bernoulli_distribution buffdist(percent);
605 
606  for(std::uint64_t i = 0; i < inputsize; ++i)
607  if(buffdist(rng_))
608  insertBuff.push_back( p[i] );
609  }
610  }
611  ////////////////////////////////////////////////////////////////////////////////
612 
613  template<typename ContainerT, typename PointT> std::uint64_t
615  {
616  assert (this->root_node_->m_tree_ != NULL);
617 
618  // Trust me, just add the points
619  if (skip_bb_check)
620  {
621  // Increment point count for node
622  root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
623 
624  // Insert point data
625  payload_->insertRange ( p );
626 
627  return (p.size ());
628  }
629 
630  // Add points found within the current node's bounding box
631  AlignedPointTVector buff;
632  const std::size_t len = p.size ();
633 
634  for (std::size_t i = 0; i < len; i++)
635  {
636  if (pointInBoundingBox (p[i]))
637  {
638  buff.push_back (p[i]);
639  }
640  }
641 
642  if (!buff.empty ())
643  {
644  root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
645  payload_->insertRange ( buff );
646  }
647  return (buff.size ());
648  }
649  ////////////////////////////////////////////////////////////////////////////////
650  template<typename ContainerT, typename PointT> std::uint64_t
652  {
653  //this assumes data is already in the correct bin
654  if(skip_bb_check)
655  {
656  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
657 
658  this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
659  payload_->insertRange (input_cloud);
660 
661  return (input_cloud->width*input_cloud->height);
662  }
663  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
664  return (0);
665  }
666 
667 
668  ////////////////////////////////////////////////////////////////////////////////
669  template<typename ContainerT, typename PointT> void
670  OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
671  {
672  // Reserve space for children nodes
673  c.resize(8);
674  for(std::size_t i = 0; i < 8; i++)
675  c[i].reserve(p.size() / 8);
676 
677  const std::size_t len = p.size();
678  for(std::size_t i = 0; i < len; i++)
679  {
680  const PointT& pt = p[i];
681 
682  if(!skip_bb_check)
683  if(!this->pointInBoundingBox(pt))
684  continue;
685 
686  subdividePoint (pt, c);
687  }
688  }
689  ////////////////////////////////////////////////////////////////////////////////
690 
691  template<typename ContainerT, typename PointT> void
692  OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c)
693  {
694  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
695  std::size_t octant = 0;
696  octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
697  c[octant].push_back (point);
698  }
699 
700  ////////////////////////////////////////////////////////////////////////////////
701  template<typename ContainerT, typename PointT> std::uint64_t
703  {
704  std::uint64_t points_added = 0;
705 
706  if ( input_cloud->width * input_cloud->height == 0 )
707  {
708  return (0);
709  }
710 
711  if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
712  {
713  std::uint64_t points_added = addDataAtMaxDepth (input_cloud, true);
714  assert (points_added > 0);
715  return (points_added);
716  }
717 
718  if (num_children_ < 8 )
719  {
720  if ( hasUnloadedChildren () )
721  {
722  loadChildren (false);
723  }
724  }
725 
726  //------------------------------------------------------------
727  //subsample data:
728  // 1. Get indices from a random sample
729  // 2. Extract those indices with the extract indices class (in order to also get the complement)
730  //------------------------------------------------------------
732  random_sampler.setInputCloud (input_cloud);
733 
734  //set sample size to 1/8 of total points (12.5%)
735  std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
736  random_sampler.setSample (static_cast<unsigned int> (sample_size));
737 
738  //create our destination
739  pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () );
740 
741  //create destination for indices
742  pcl::IndicesPtr downsampled_cloud_indices ( new std::vector< int > () );
743  random_sampler.filter (*downsampled_cloud_indices);
744 
745  //extract the "random subset", size by setSampleSize
747  extractor.setInputCloud (input_cloud);
748  extractor.setIndices (downsampled_cloud_indices);
749  extractor.filter (*downsampled_cloud);
750 
751  //extract the complement of those points (i.e. everything remaining)
752  pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () );
753  extractor.setNegative (true);
754  extractor.filter (*remaining_points);
755 
756  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 );
757 
758  //insert subsampled data to the node's disk container payload
759  if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
760  {
761  root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
762  payload_->insertRange (downsampled_cloud);
763  points_added += downsampled_cloud->width*downsampled_cloud->height ;
764  }
765 
766  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
767 
768  //subdivide remaining data by destination octant
769  std::vector<std::vector<int> > indices;
770  indices.resize (8);
771 
772  this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
773 
774  //pass each set of points to the appropriate child octant
775  for(std::size_t i=0; i<8; i++)
776  {
777 
778  if(indices[i].empty ())
779  continue;
780 
781  if (children_[i] == nullptr)
782  {
783  assert (i < 8);
784  createChild (i);
785  }
786 
787  //copy correct indices into a temporary cloud
788  pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ());
789  pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud);
790 
791  //recursively add points and keep track of how many were successfully added to the tree
792  points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
793  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);
794 
795  }
796  assert (points_added == input_cloud->width*input_cloud->height);
797  return (points_added);
798  }
799  ////////////////////////////////////////////////////////////////////////////////
800 
801  template<typename ContainerT, typename PointT> std::uint64_t
803  {
804  // If there are no points return
805  if (p.empty ())
806  return (0);
807 
808  // when adding data and generating sampled LOD
809  // If the max depth has been reached
810  assert (this->root_node_->m_tree_ != NULL );
811 
812  if (this->depth_ == this->root_node_->m_tree_->getDepth ())
813  {
814  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
815  return (addDataAtMaxDepth(p, false));
816  }
817 
818  // Create child nodes of the current node but not grand children+
819  if (this->hasUnloadedChildren ())
820  loadChildren (false /*no recursive loading*/);
821 
822  // Randomly sample data
823  AlignedPointTVector insertBuff;
824  randomSample(p, insertBuff, skip_bb_check);
825 
826  if(!insertBuff.empty())
827  {
828  // Increment point count for node
829  root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
830  // Insert sampled point data
831  payload_->insertRange (insertBuff);
832 
833  }
834 
835  //subdivide vec to pass data down lower
836  std::vector<AlignedPointTVector> c;
837  subdividePoints(p, c, skip_bb_check);
838 
839  std::uint64_t points_added = 0;
840  for(std::size_t i = 0; i < 8; i++)
841  {
842  // If child doesn't have points
843  if(c[i].empty())
844  continue;
845 
846  // If child doesn't exist
847  if(!children_[i])
848  createChild(i);
849 
850  // Recursively build children
851  points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true);
852  c[i].clear();
853  }
854 
855  return (points_added);
856  }
857  ////////////////////////////////////////////////////////////////////////////////
858 
859  template<typename ContainerT, typename PointT> void
861  {
862  assert (idx < 8);
863 
864  //if already has 8 children, return
865  if (children_[idx] || (num_children_ == 8))
866  {
867  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
868  return;
869  }
870 
871  Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
872  Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0);
873 
874  Eigen::Vector3d childbb_min;
875  Eigen::Vector3d childbb_max;
876 
877  int x, y, z;
878  if (idx > 3)
879  {
880  x = ((idx == 5) || (idx == 7)) ? 1 : 0;
881  y = ((idx == 6) || (idx == 7)) ? 1 : 0;
882  z = 1;
883  }
884  else
885  {
886  x = ((idx == 1) || (idx == 3)) ? 1 : 0;
887  y = ((idx == 2) || (idx == 3)) ? 1 : 0;
888  z = 0;
889  }
890 
891  childbb_min[2] = start[2] + static_cast<double> (z) * step[2];
892  childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2];
893 
894  childbb_min[1] = start[1] + static_cast<double> (y) * step[1];
895  childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1];
896 
897  childbb_min[0] = start[0] + static_cast<double> (x) * step[0];
898  childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0];
899 
900  boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
901  children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this);
902 
903  num_children_++;
904  }
905  ////////////////////////////////////////////////////////////////////////////////
906 
907  template<typename ContainerT, typename PointT> bool
908  pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point)
909  {
910  if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
911  ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
912  ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
913  {
914  return (true);
915 
916  }
917  return (false);
918  }
919 
920 
921  ////////////////////////////////////////////////////////////////////////////////
922  template<typename ContainerT, typename PointT> bool
924  {
925  const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
926  const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
927 
928  if (((min[0] <= p.x) && (p.x < max[0])) &&
929  ((min[1] <= p.y) && (p.y < max[1])) &&
930  ((min[2] <= p.z) && (p.z < max[2])))
931  {
932  return (true);
933 
934  }
935  return (false);
936  }
937 
938  ////////////////////////////////////////////////////////////////////////////////
939  template<typename ContainerT, typename PointT> void
941  {
942  Eigen::Vector3d min;
943  Eigen::Vector3d max;
944  node_metadata_->getBoundingBox (min, max);
945 
946  if (this->depth_ < query_depth){
947  for (std::size_t i = 0; i < this->depth_; i++)
948  std::cout << " ";
949 
950  std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - ";
951  std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - ";
952  std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1];
953  std::cout << ", " << max[2] - min[2] << "]" << std::endl;
954 
955  if (num_children_ > 0)
956  {
957  for (std::size_t i = 0; i < 8; i++)
958  {
959  if (children_[i])
960  children_[i]->printBoundingBox (query_depth);
961  }
962  }
963  }
964  }
965 
966  ////////////////////////////////////////////////////////////////////////////////
967  template<typename ContainerT, typename PointT> void
969  {
970  if (this->depth_ < query_depth){
971  if (num_children_ > 0)
972  {
973  for (std::size_t i = 0; i < 8; i++)
974  {
975  if (children_[i])
976  children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
977  }
978  }
979  }
980  else
981  {
982  PointT voxel_center;
983  Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
984  voxel_center.x = static_cast<float>(mid_xyz[0]);
985  voxel_center.y = static_cast<float>(mid_xyz[1]);
986  voxel_center.z = static_cast<float>(mid_xyz[2]);
987 
988  voxel_centers.push_back(voxel_center);
989  }
990  }
991 
992  ////////////////////////////////////////////////////////////////////////////////
993 // Eigen::Vector3d cornerOffsets[] =
994 // {
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 // Eigen::Vector3d( 1.0, 1.0, 1.0) // + + +
1003 // };
1004 //
1005 // // Note that the input vector must already be negated when using this code for halfplane tests
1006 // int
1007 // vectorToIndex(Eigen::Vector3d normal)
1008 // {
1009 // int index = 0;
1010 //
1011 // if (normal.z () >= 0) index |= 1;
1012 // if (normal.y () >= 0) index |= 2;
1013 // if (normal.x () >= 0) index |= 4;
1014 //
1015 // return index;
1016 // }
1017 //
1018 // void
1019 // get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb)
1020 // {
1021 //
1022 // p_vertex = min_bb;
1023 // n_vertex = max_bb;
1024 //
1025 // if (normal.x () >= 0)
1026 // {
1027 // n_vertex.x () = min_bb.x ();
1028 // p_vertex.x () = max_bb.x ();
1029 // }
1030 //
1031 // if (normal.y () >= 0)
1032 // {
1033 // n_vertex.y () = min_bb.y ();
1034 // p_vertex.y () = max_bb.y ();
1035 // }
1036 //
1037 // if (normal.z () >= 0)
1038 // {
1039 // p_vertex.z () = max_bb.z ();
1040 // n_vertex.z () = min_bb.z ();
1041 // }
1042 // }
1043 
1044  template<typename Container, typename PointT> void
1045  OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names)
1046  {
1047  queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1048  }
1049 
1050  template<typename Container, typename PointT> void
1051  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)
1052  {
1053 
1054  enum {INSIDE, INTERSECT, OUTSIDE};
1055 
1056  int result = INSIDE;
1057 
1058  if (this->depth_ > query_depth)
1059  {
1060  return;
1061  }
1062 
1063 // if (this->depth_ > query_depth)
1064 // return;
1065 
1066  if (!skip_vfc_check)
1067  {
1068  for(int i =0; i < 6; i++){
1069  double a = planes[(i*4)];
1070  double b = planes[(i*4)+1];
1071  double c = planes[(i*4)+2];
1072  double d = planes[(i*4)+3];
1073 
1074  //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1075 
1076  Eigen::Vector3d normal(a, b, c);
1077 
1078  Eigen::Vector3d min_bb;
1079  Eigen::Vector3d max_bb;
1080  node_metadata_->getBoundingBox(min_bb, max_bb);
1081 
1082  // Basic VFC algorithm
1083  Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1084  Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1085  std::abs (static_cast<double> (max_bb.y () - center.y ())),
1086  std::abs (static_cast<double> (max_bb.z () - center.z ())));
1087 
1088  double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1089  double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1090 
1091  if (m + n < 0){
1092  result = OUTSIDE;
1093  break;
1094  }
1095 
1096  if (m - n < 0) result = INTERSECT;
1097 
1098  // // n-p implementation
1099  // Eigen::Vector3d p_vertex; //pos vertex
1100  // Eigen::Vector3d n_vertex; //neg vertex
1101  // get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb);
1102  //
1103  // std::cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << std::endl;
1104  // std::cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << std::endl;
1105 
1106  // is the positive vertex outside?
1107  // if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0)
1108  // {
1109  // result = OUTSIDE;
1110  // }
1111  // // is the negative vertex outside?
1112  // else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
1113  // {
1114  // result = INTERSECT;
1115  // }
1116 
1117  //
1118  //
1119  // // This should be the same as below
1120  // if (normal.dot(n_vertex) + d > 0)
1121  // {
1122  // result = OUTSIDE;
1123  // }
1124  //
1125  // if (normal.dot(p_vertex) + d >= 0)
1126  // {
1127  // result = INTERSECT;
1128  // }
1129 
1130  // This should be the same as above
1131  // double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ());
1132  // std::cout << "m = " << m << std::endl;
1133  // if (m > -d)
1134  // {
1135  // result = OUTSIDE;
1136  // }
1137  //
1138  // double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ());
1139  // std::cout << "n = " << n << std::endl;
1140  // if (n > -d)
1141  // {
1142  // result = INTERSECT;
1143  // }
1144  }
1145  }
1146 
1147  if (result == OUTSIDE)
1148  {
1149  return;
1150  }
1151 
1152 // switch(result){
1153 // case OUTSIDE:
1154 // //std::cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1155 // return;
1156 // case INTERSECT:
1157 // //std::cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << std::endl;
1158 // break;
1159 // case INSIDE:
1160 // //std::cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1161 // break;
1162 // }
1163 
1164  // Add files breadth first
1165  if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1166  //if (payload_->getDataSize () > 0)
1167  {
1168  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1169  }
1170 
1171  if (hasUnloadedChildren ())
1172  {
1173  loadChildren (false);
1174  }
1175 
1176  if (this->getNumChildren () > 0)
1177  {
1178  for (std::size_t i = 0; i < 8; i++)
1179  {
1180  if (children_[i])
1181  children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1182  }
1183  }
1184 // else if (hasUnloadedChildren ())
1185 // {
1186 // loadChildren (false);
1187 //
1188 // for (std::size_t i = 0; i < 8; i++)
1189 // {
1190 // if (children_[i])
1191 // children_[i]->queryFrustum (planes, file_names, query_depth);
1192 // }
1193 // }
1194  //}
1195  }
1196 
1197 ////////////////////////////////////////////////////////////////////////////////
1198 
1199  template<typename Container, typename PointT> void
1200  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)
1201  {
1202 
1203  // If we're above our query depth
1204  if (this->depth_ > query_depth)
1205  {
1206  return;
1207  }
1208 
1209  // Bounding Box
1210  Eigen::Vector3d min_bb;
1211  Eigen::Vector3d max_bb;
1212  node_metadata_->getBoundingBox(min_bb, max_bb);
1213 
1214  // Frustum Culling
1215  enum {INSIDE, INTERSECT, OUTSIDE};
1216 
1217  int result = INSIDE;
1218 
1219  if (!skip_vfc_check)
1220  {
1221  for(int i =0; i < 6; i++){
1222  double a = planes[(i*4)];
1223  double b = planes[(i*4)+1];
1224  double c = planes[(i*4)+2];
1225  double d = planes[(i*4)+3];
1226 
1227  //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1228 
1229  Eigen::Vector3d normal(a, b, c);
1230 
1231  // Basic VFC algorithm
1232  Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1233  Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1234  std::abs (static_cast<double> (max_bb.y () - center.y ())),
1235  std::abs (static_cast<double> (max_bb.z () - center.z ())));
1236 
1237  double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1238  double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1239 
1240  if (m + n < 0){
1241  result = OUTSIDE;
1242  break;
1243  }
1244 
1245  if (m - n < 0) result = INTERSECT;
1246 
1247  }
1248  }
1249 
1250  if (result == OUTSIDE)
1251  {
1252  return;
1253  }
1254 
1255  // Bounding box projection
1256  // 3--------2
1257  // /| /| Y 0 = xmin, ymin, zmin
1258  // / | / | | 6 = xmax, ymax. zmax
1259  // 7--------6 | |
1260  // | | | | |
1261  // | 0-----|--1 +------X
1262  // | / | / /
1263  // |/ |/ /
1264  // 4--------5 Z
1265 
1266 // bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1267 // bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1268 // bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1269 // bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1270 // bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1271 // bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1272 // bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1273 // bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1274 
1275  int width = 500;
1276  int height = 500;
1277 
1278  float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height);
1279  //float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix);
1280 
1281 // for (int i=0; i < this->depth_; i++) std::cout << " ";
1282 // std::cout << this->depth_ << ": " << coverage << std::endl;
1283 
1284  // Add files breadth first
1285  if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1286  //if (payload_->getDataSize () > 0)
1287  {
1288  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1289  }
1290 
1291  //if (coverage <= 0.075)
1292  if (coverage <= 10000)
1293  return;
1294 
1295  if (hasUnloadedChildren ())
1296  {
1297  loadChildren (false);
1298  }
1299 
1300  if (this->getNumChildren () > 0)
1301  {
1302  for (std::size_t i = 0; i < 8; i++)
1303  {
1304  if (children_[i])
1305  children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1306  }
1307  }
1308  }
1309 
1310 ////////////////////////////////////////////////////////////////////////////////
1311  template<typename ContainerT, typename PointT> void
1312  OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth)
1313  {
1314  if (this->depth_ < query_depth){
1315  if (num_children_ > 0)
1316  {
1317  for (std::size_t i = 0; i < 8; i++)
1318  {
1319  if (children_[i])
1320  children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1321  }
1322  }
1323  }
1324  else
1325  {
1326  Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1327  voxel_centers.push_back(voxel_center);
1328  }
1329  }
1330 
1331 
1332  ////////////////////////////////////////////////////////////////////////////////
1333 
1334  template<typename ContainerT, typename PointT> void
1335  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)
1336  {
1337 
1338  Eigen::Vector3d my_min = min_bb;
1339  Eigen::Vector3d my_max = max_bb;
1340 
1341  if (intersectsWithBoundingBox (my_min, my_max))
1342  {
1343  if (this->depth_ < query_depth)
1344  {
1345  if (this->getNumChildren () > 0)
1346  {
1347  for (std::size_t i = 0; i < 8; i++)
1348  {
1349  if (children_[i])
1350  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1351  }
1352  }
1353  else if (hasUnloadedChildren ())
1354  {
1355  loadChildren (false);
1356 
1357  for (std::size_t i = 0; i < 8; i++)
1358  {
1359  if (children_[i])
1360  children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1361  }
1362  }
1363  return;
1364  }
1365 
1366  if (payload_->getDataSize () > 0)
1367  {
1368  file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1369  }
1370  }
1371  }
1372  ////////////////////////////////////////////////////////////////////////////////
1373 
1374  template<typename ContainerT, typename PointT> void
1375  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)
1376  {
1377  std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1378  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1379 
1380  // If the queried bounding box has any intersection with this node's bounding box
1381  if (intersectsWithBoundingBox (min_bb, max_bb))
1382  {
1383  // If we aren't at the max desired depth
1384  if (this->depth_ < query_depth)
1385  {
1386  //if this node doesn't have any children, we are at the max depth for this query
1387  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1388  loadChildren (false);
1389 
1390  //if this node has children
1391  if (num_children_ > 0)
1392  {
1393  //recursively store any points that fall into the queried bounding box into v and return
1394  for (std::size_t i = 0; i < 8; i++)
1395  {
1396  if (children_[i])
1397  children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1398  }
1399  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1400  return;
1401  }
1402  }
1403  else //otherwise if we are at the max depth
1404  {
1405  //get all the points from the payload and return (easy with PCLPointCloud2)
1407  pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ());
1408  //load all the data in this node from disk
1409  payload_->readRange (0, payload_->size (), tmp_blob);
1410 
1411  if( tmp_blob->width*tmp_blob->height == 0 )
1412  return;
1413 
1414  //if this node's bounding box falls completely within the queried bounding box, keep all the points
1415  if (inBoundingBox (min_bb, max_bb))
1416  {
1417  //concatenate all of what was just read into the main dst_blob
1418  //(is it safe to do in place?)
1419 
1420  //if there is already something in the destination blob (remember this method is recursive)
1421  if( dst_blob->width*dst_blob->height != 0 )
1422  {
1423  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1424  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1425  int res = pcl::concatenate (*dst_blob, *tmp_blob, *dst_blob);
1426  pcl::utils::ignore(res);
1427  assert (res == 1);
1428 
1429  PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1430  }
1431  //otherwise, just copy the tmp_blob into the dst_blob
1432  else
1433  {
1434  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1435  pcl::copyPointCloud (*tmp_blob, *dst_blob);
1436  assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1437  }
1438  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1439  return;
1440  }
1441  //otherwise queried bounding box only partially intersects this
1442  //node's bounding box, so we have to check all the points in
1443  //this box for intersection with queried bounding box
1444 
1445 // 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] );
1446  //put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox)
1447  typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () );
1448  pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud );
1449  assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height );
1450 
1451  Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
1452  Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
1453 
1454  std::vector<int> indices;
1455 
1456  pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices );
1457  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1458  PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () );
1459 
1460  if ( !indices.empty () )
1461  {
1462  if( dst_blob->width*dst_blob->height > 0 )
1463  {
1464  //need a new tmp destination with extracted points within BB
1465  pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ());
1466 
1467  //copy just the points marked in indices
1468  pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb );
1469  assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1470  assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1471  //concatenate those points into the returned dst_blob
1472  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1473  std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1474  int res = pcl::concatenate (*dst_blob, *tmp_blob_within_bb, *dst_blob);
1475  pcl::utils::ignore(orig_points_in_destination, 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_
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map...
Definition: conversions.h:168
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:429
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&#39;s children are 1, ...
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
std::uint64_t uint64_t
Definition: types.h:60
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, Indices &indices)
Get a set of points residing in a box given its bounds.
Definition: common.hpp:102
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
std::uint32_t uint32_t
Definition: types.h:58
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
Tests whether point falls within the input bounding box.
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v)
Recursively add points that fall into the queried bounding box up to the query_depth.
static const std::string node_index_basename
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::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
void init_root_node(const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase< ContainerT, PointT > *const tree, const boost::filesystem::path &rootname)
Create root node and directory.
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
Define standard C methods and C++ classes that are common to all methods.
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition: utils.h:62
void createChild(const std::size_t idx)
Creates child node idx.
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.
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.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
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&#39;s vector; This co...
std::shared_ptr< ContainerT > payload_
what holds the points.
static const std::string node_index_extension
static const std::string node_container_basename
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:282
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
void saveIdx(bool recursive)
Save node&#39;s metadata to file.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
virtual std::size_t getDepth() const
virtual std::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)
Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this me...
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the pcl::outofcore::OutofcoreOctreeDiskContainer class or pcl::outofcore::OutofcoreOctreeRamContainer class, whichever it is templated against.
OutofcoreOctreeBaseNode * parent_
super-node
std::uint8_t uint8_t
Definition: types.h:54
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.
static const std::string node_container_extension
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator...
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
A point structure representing Euclidean xyz coordinates, and the RGB color.
static std::mutex rng_mutex_
Random number generator mutex.
std::uint64_t num_children_
Number of children on disk.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node&#39;s bounding box...
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:150
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node&#39;s bounding box.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
void setSample(unsigned int sample)
Set number of indices to be sampled.
Encapsulated class to read JSON metadata into memory, and write the JSON metadata for each node...
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::uint32_t query_depth, std::list< std::string > &file_names)
Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_dep...
virtual std::uint64_t addDataToLeaf(const AlignedPointTVector &p, const bool skip_bb_check=true)
add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point...
OutofcoreOctreeNodeMetadata::Ptr node_metadata_