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