Point Cloud Library (PCL)  1.7.0
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>
66  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename = "node";
67 
68  template<typename ContainerT, typename PointT>
69  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename = "node";
70 
71  template<typename ContainerT, typename PointT>
72  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension = ".oct_idx";
73 
74  template<typename ContainerT, typename PointT>
75  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension = ".oct_dat";
76 
77  template<typename ContainerT, typename PointT>
78  boost::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_;
79 
80  template<typename ContainerT, typename PointT>
81  boost::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rand_gen_;
82 
83  template<typename ContainerT, typename PointT>
84  const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125;
85 
86  template<typename ContainerT, typename PointT>
87  const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension = ".pcd";
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] == false )
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] == false )
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_