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