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