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