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