Point Cloud Library (PCL) 1.13.1
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 != 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_, static_cast<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 filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
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.
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 void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded children by testing the children_ array; used to update num_loaded_childr...
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:275
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
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.