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