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