Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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] == 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 > 1 ? 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  extractor.setInputCloud (tmp_blob);
1643 
1644  pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ());
1645  random_sampler.filter (*downsampled_cloud_indices);
1646  extractor.setIndices (downsampled_cloud_indices);
1647  extractor.filter (*downsampled_points);
1648 
1649  //concatenate the result into the destination cloud
1650  pcl::concatenatePointCloud (*dst_blob, *downsampled_points, *dst_blob);
1651  }
1652  }
1653  }
1654  }
1655 
1656 
1657  ////////////////////////////////////////////////////////////////////////////////
1658  template<typename ContainerT, typename PointT> void
1659  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)
1660  {
1661  //check if the queried bounding box has any intersection with this node's bounding box
1662  if (intersectsWithBoundingBox (min_bb, max_bb))
1663  {
1664  //if we are not at the max depth for queried nodes
1665  if (this->depth_ < query_depth)
1666  {
1667  //check if we don't have children
1668  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1669  {
1670  loadChildren (false);
1671  }
1672  //if we do have children
1673  if (num_children_ > 0)
1674  {
1675  //recursively add their valid points within the queried bounding box to the list v
1676  for (size_t i = 0; i < 8; i++)
1677  {
1678  if (children_[i])
1679  children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1680  }
1681  return;
1682  }
1683  }
1684  //otherwise we are at the max depth, so we add all our points or some of our points
1685  else
1686  {
1687  //if this node's bounding box falls completely within the queried bounding box
1688  if (inBoundingBox (min_bb, max_bb))
1689  {
1690  //add a random sample of all the points
1691  AlignedPointTVector payload_cache;
1692  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1693  dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1694  return;
1695  }
1696  //otherwise the queried bounding box only partially intersects with this node's bounding box
1697  else
1698  {
1699  //brute force selection of all valid points
1700  AlignedPointTVector payload_cache_within_region;
1701  {
1702  AlignedPointTVector payload_cache;
1703  payload_->readRange (0, payload_->size (), payload_cache);
1704  for (size_t i = 0; i < payload_->size (); i++)
1705  {
1706  const PointT& p = payload_cache[i];
1707  if (pointInBoundingBox (min_bb, max_bb, p))
1708  {
1709  payload_cache_within_region.push_back (p);
1710  }
1711  }
1712  }//force the payload cache to deconstruct here
1713 
1714  //use STL random_shuffle and push back a random selection of the points onto our list
1715  std::random_shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end ());
1716  size_t numpick = static_cast<size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));;
1717 
1718  for (size_t i = 0; i < numpick; i++)
1719  {
1720  dst.push_back (payload_cache_within_region[i]);
1721  }
1722  }
1723  }
1724  }
1725  }
1726  ////////////////////////////////////////////////////////////////////////////////
1727 
1728 //dir is current level. we put this nodes files into it
1729  template<typename ContainerT, typename PointT>
1730  OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super)
1731  : m_tree_ ()
1732  , root_node_ ()
1733  , parent_ ()
1734  , depth_ ()
1735  , children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0)))
1736  , num_children_ ()
1737  , num_loaded_children_ (0)
1738  , payload_ ()
1739  , node_metadata_ ()
1740  {
1741  node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ());
1742  node_metadata_->setOutofcoreVersion (3);
1743 
1744  if (super == NULL)
1745  {
1746  PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1747  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1748  }
1749 
1750  this->parent_ = super;
1751  root_node_ = super->root_node_;
1752  m_tree_ = super->root_node_->m_tree_;
1753  assert (m_tree_ != NULL);
1754 
1755  depth_ = super->depth_ + 1;
1756  num_children_ = 0;
1757 
1758  node_metadata_->setBoundingBox (bb_min, bb_max);
1759 
1760  std::string uuid_idx;
1761  std::string uuid_cont;
1764 
1765  std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension;
1766 
1767  std::string node_container_name;
1768  node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension;
1769 
1770  node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1771  node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
1772  node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name));
1773 
1774  boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
1775 
1776  payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ()));
1777  this->saveIdx (false);
1778  }
1779 
1780  ////////////////////////////////////////////////////////////////////////////////
1781 
1782  template<typename ContainerT, typename PointT> void
1784  {
1785  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1786  {
1787  loadChildren (false);
1788  }
1789 
1790  for (size_t i = 0; i < num_children_; i++)
1791  {
1792  children_[i]->copyAllCurrentAndChildPointsRec (v);
1793  }
1794 
1795  AlignedPointTVector payload_cache;
1796  payload_->readRange (0, payload_->size (), payload_cache);
1797 
1798  {
1799  //boost::mutex::scoped_lock lock(queryBBIncludes_vector_mutex);
1800  v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1801  }
1802  }
1803 
1804  ////////////////////////////////////////////////////////////////////////////////
1805 
1806  template<typename ContainerT, typename PointT> void
1808  {
1809  if ((num_children_ == 0) && (hasUnloadedChildren ()))
1810  {
1811  loadChildren (false);
1812  }
1813 
1814  for (size_t i = 0; i < 8; i++)
1815  {
1816  if (children_[i])
1817  children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1818  }
1819 
1820  std::vector<PointT> payload_cache;
1821  payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1822 
1823  for (size_t i = 0; i < payload_cache.size (); i++)
1824  {
1825  v.push_back (payload_cache[i]);
1826  }
1827  }
1828 
1829  ////////////////////////////////////////////////////////////////////////////////
1830 
1831  template<typename ContainerT, typename PointT> inline bool
1832  OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1833  {
1834  Eigen::Vector3d min, max;
1835  node_metadata_->getBoundingBox (min, max);
1836 
1837  //Check whether any portion of min_bb, max_bb falls within min,max
1838  if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1839  {
1840  if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1841  {
1842  if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1843  {
1844  return (true);
1845  }
1846  }
1847  }
1848 
1849  return (false);
1850  }
1851  ////////////////////////////////////////////////////////////////////////////////
1852 
1853  template<typename ContainerT, typename PointT> inline bool
1854  OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const
1855  {
1856  Eigen::Vector3d min, max;
1857 
1858  node_metadata_->getBoundingBox (min, max);
1859 
1860  if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1861  {
1862  if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1863  {
1864  if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1865  {
1866  return (true);
1867  }
1868  }
1869  }
1870 
1871  return (false);
1872  }
1873  ////////////////////////////////////////////////////////////////////////////////
1874 
1875  template<typename ContainerT, typename PointT> inline bool
1876  OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb,
1877  const PointT& p)
1878  {
1879  //by convention, minimum boundary is included; maximum boundary is not
1880  if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1881  {
1882  if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1883  {
1884  if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1885  {
1886  return (true);
1887  }
1888  }
1889  }
1890  return (false);
1891  }
1892 
1893  ////////////////////////////////////////////////////////////////////////////////
1894 
1895  template<typename ContainerT, typename PointT> void
1897  {
1898  Eigen::Vector3d min;
1899  Eigen::Vector3d max;
1900  node_metadata_->getBoundingBox (min, max);
1901 
1902  double l = max[0] - min[0];
1903  double h = max[1] - min[1];
1904  double w = max[2] - min[2];
1905  file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h
1906  << ", width=" << w << " )\n";
1907 
1908  for (size_t i = 0; i < num_children_; i++)
1909  {
1910  children_[i]->writeVPythonVisual (file);
1911  }
1912  }
1913 
1914  ////////////////////////////////////////////////////////////////////////////////
1915 
1916  template<typename ContainerT, typename PointT> int
1918  {
1919  return (this->payload_->read (output_cloud));
1920  }
1921 
1922  ////////////////////////////////////////////////////////////////////////////////
1923 
1924  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
1926  {
1927  PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1928  return (children_[index_arg]);
1929  }
1930 
1931  ////////////////////////////////////////////////////////////////////////////////
1932  template<typename ContainerT, typename PointT> boost::uint64_t
1934  {
1935  return (this->payload_->getDataSize ());
1936  }
1937 
1938  ////////////////////////////////////////////////////////////////////////////////
1939 
1940  template<typename ContainerT, typename PointT> size_t
1942  {
1943  size_t loaded_children_count = 0;
1944 
1945  for (size_t i=0; i<8; i++)
1946  {
1947  if (children_[i] != 0)
1948  loaded_children_count++;
1949  }
1950 
1951  return (loaded_children_count);
1952  }
1953 
1954  ////////////////////////////////////////////////////////////////////////////////
1955 
1956  template<typename ContainerT, typename PointT> void
1958  {
1959  PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1960  node_metadata_->loadMetadataFromDisk (path);
1961 
1962  //this shouldn't be part of 'loadFromFile'
1963  this->parent_ = super;
1964 
1965  if (num_children_ > 0)
1966  recFreeChildren ();
1967 
1968  this->num_children_ = 0;
1969  this->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ()));
1970  }
1971 
1972  ////////////////////////////////////////////////////////////////////////////////
1973 
1974  template<typename ContainerT, typename PointT> void
1976  {
1977  std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz");
1978  boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1979  payload_->convertToXYZ (xyzfile);
1980 
1981  if (hasUnloadedChildren ())
1982  {
1983  loadChildren (false);
1984  }
1985 
1986  for (size_t i = 0; i < 8; i++)
1987  {
1988  if (children_[i])
1989  children_[i]->convertToXYZ ();
1990  }
1991  }
1992 
1993  ////////////////////////////////////////////////////////////////////////////////
1994 
1995  template<typename ContainerT, typename PointT> void
1997  {
1998  for (size_t i = 0; i < 8; i++)
1999  {
2000  if (children_[i])
2001  children_[i]->flushToDiskRecursive ();
2002  }
2003  }
2004 
2005  ////////////////////////////////////////////////////////////////////////////////
2006 
2007  template<typename ContainerT, typename PointT> void
2008  OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz)
2009  {
2010  if (indices.size () < 8)
2011  indices.resize (8);
2012 
2013  int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") );
2014  int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") );
2015  int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") );
2016 
2017  int x_offset = input_cloud->fields[x_idx].offset;
2018  int y_offset = input_cloud->fields[y_idx].offset;
2019  int z_offset = input_cloud->fields[z_idx].offset;
2020 
2021  for ( size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
2022  {
2023  PointT local_pt;
2024 
2025  local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset]));
2026  local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset]));
2027  local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset]));
2028 
2029  if (!pcl_isfinite (local_pt.x) || !pcl_isfinite (local_pt.y) || !pcl_isfinite (local_pt.z))
2030  continue;
2031 
2032  if(!this->pointInBoundingBox (local_pt))
2033  {
2034  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);
2035  }
2036 
2037  assert (this->pointInBoundingBox (local_pt) == true);
2038 
2039  //compute the box we are in
2040  size_t box = 0;
2041  box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2042  assert (box < 8);
2043 
2044  //insert to the vector of indices
2045  indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
2046  }
2047  }
2048  ////////////////////////////////////////////////////////////////////////////////
2049 
2050 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2051  template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
2052  makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super)
2053  {
2055 //octree_disk_node ();
2056 
2057  if (super == NULL)
2058  {
2059  thisnode->thisdir_ = path.parent_path ();
2060 
2061  if (!boost::filesystem::exists (thisnode->thisdir_))
2062  {
2063  PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2064  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2065  }
2066 
2067  thisnode->thisnodeindex_ = path;
2068 
2069  thisnode->depth_ = 0;
2070  thisnode->root_node_ = thisnode;
2071  }
2072  else
2073  {
2074  thisnode->thisdir_ = path;
2075  thisnode->depth_ = super->depth_ + 1;
2076  thisnode->root_node_ = super->root_node_;
2077 
2078  if (thisnode->depth_ > thisnode->root->max_depth_)
2079  {
2080  thisnode->root->max_depth_ = thisnode->depth_;
2081  }
2082 
2083  boost::filesystem::directory_iterator diterend;
2084  bool loaded = false;
2085  for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2086  {
2087  const boost::filesystem::path& file = *diter;
2088  if (!boost::filesystem::is_directory (file))
2089  {
2090  if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
2091  {
2092  thisnode->thisnodeindex_ = file;
2093  loaded = true;
2094  break;
2095  }
2096  }
2097  }
2098 
2099  if (!loaded)
2100  {
2101  PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2102  PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2103  }
2104 
2105  }
2106  thisnode->max_depth_ = 0;
2107 
2108  {
2109  std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2110 
2111  f >> thisnode->min_[0];
2112  f >> thisnode->min_[1];
2113  f >> thisnode->min_[2];
2114  f >> thisnode->max_[0];
2115  f >> thisnode->max_[1];
2116  f >> thisnode->max_[2];
2117 
2118  std::string filename;
2119  f >> filename;
2120  thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2121 
2122  f.close ();
2123 
2124  thisnode->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (thisnode->thisnodestorage_));
2125  }
2126 
2127  thisnode->parent_ = super;
2128  children_.clear ();
2129  children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0));
2130  thisnode->num_children_ = 0;
2131 
2132  return (thisnode);
2133  }
2134 
2135  ////////////////////////////////////////////////////////////////////////////////
2136 
2137 //accelerate search
2138  template<typename ContainerT, typename PointT> void
2139  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)
2140  {
2141  OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2142  if (root == NULL)
2143  {
2144  std::cout << "test";
2145  }
2146  if (root->intersectsWithBoundingBox (min, max))
2147  {
2148  if (query_depth == root->max_depth_)
2149  {
2150  if (!root->payload_->empty ())
2151  {
2152  bin_name.push_back (root->thisnodestorage_.string ());
2153  }
2154  return;
2155  }
2156 
2157  for (int i = 0; i < 8; i++)
2158  {
2159  boost::filesystem::path child_dir = root->thisdir_
2160  / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2161  if (boost::filesystem::exists (child_dir))
2162  {
2163  root->children_[i] = makenode_norec (child_dir, root);
2164  root->num_children_++;
2165  queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name);
2166  }
2167  }
2168  }
2169  delete root;
2170  }
2171 
2172  ////////////////////////////////////////////////////////////////////////////////
2173 
2174  template<typename ContainerT, typename PointT> void
2175  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)
2176  {
2177  if (current->intersectsWithBoundingBox (min, max))
2178  {
2179  if (current->depth_ == query_depth)
2180  {
2181  if (!current->payload_->empty ())
2182  {
2183  bin_name.push_back (current->thisnodestorage_.string ());
2184  }
2185  }
2186  else
2187  {
2188  for (int i = 0; i < 8; i++)
2189  {
2190  boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2191  if (boost::filesystem::exists (child_dir))
2192  {
2193  current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2194  current->num_children_++;
2195  queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name);
2196  }
2197  }
2198  }
2199  }
2200  }
2201 #endif
2202  ////////////////////////////////////////////////////////////////////////////////
2203 
2204  }//namespace outofcore
2205 }//namespace pcl
2206 
2207 //#define PCL_INSTANTIATE....
2208 
2209 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
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:101
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
Definition: conversions.h:169
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
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.
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
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
uint64_t num_children_
Number of children on disk.
static const std::string node_index_extension
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
virtual OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
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...
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
OutofcoreOctreeBaseNode * parent_
super-node
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
virtual size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
RandomSample applies a random sampling with uniform probability.
boost::shared_ptr< ContainerT > payload_
what holds the points.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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.
virtual void filter(PCLPointCloud2 &output)
virtual size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
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 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.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
static const std::string node_index_basename
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.
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node's bounding box.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
virtual boost::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
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...
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 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.
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.
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
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.
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 recFreeChildren()
Method which recursively free children of this node.
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list) ...
void createChild(const std::size_t idx)
Creates child node idx.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
PointCloud represents the base class in PCL for storing collections of 3D points. ...
boost::uuids::basic_random_generator< boost::mt19937 > OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
boost::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node's bounding box...
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
virtual ~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
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 setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const size_t query_depth)
Gets a vector of occupied voxel centers.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:149
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector< int > > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector; This co...
virtual void printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
void setSample(unsigned int sample)
Set number of indices to be sampled.
void saveIdx(bool recursive)
Save node's metadata to file.
Encapsulated class to read JSON metadata into memory, and write the JSON metadata for each node...
static const std::string node_container_basename
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.