Point Cloud Library (PCL) 1.15.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/pcl_config.h> // for HAVE_CJSON
61#if defined(HAVE_CJSON)
62#include <cjson/cJSON.h>
63#else
64#include <pcl/outofcore/cJSON.h>
65#endif
66
67namespace pcl
68{
69 namespace outofcore
70 {
71
72 template<typename ContainerT, typename PointT>
74
75 template<typename ContainerT, typename PointT>
77
78 template<typename ContainerT, typename PointT>
80
81 template<typename ContainerT, typename PointT>
83
84 template<typename ContainerT, typename PointT>
86
87 template<typename ContainerT, typename PointT>
89
90 template<typename ContainerT, typename PointT>
92
93 template<typename ContainerT, typename PointT>
95
96 template<typename ContainerT, typename PointT>
98 : m_tree_ ()
99 , root_node_ (NULL)
100 , parent_ (NULL)
101 , depth_ (0)
102 , children_ (8, nullptr)
103 , num_children_ (0)
104 , num_loaded_children_ (0)
105 , payload_ ()
106 , node_metadata_ (new OutofcoreOctreeNodeMetadata)
107 {
108 node_metadata_->setOutofcoreVersion (3);
109 }
110
111 ////////////////////////////////////////////////////////////////////////////////
112
113 template<typename ContainerT, typename PointT>
115 : m_tree_ ()
116 , root_node_ ()
117 , parent_ (super)
118 , depth_ ()
119 , children_ (8, nullptr)
120 , num_children_ (0)
121 , num_loaded_children_ (0)
122 , payload_ ()
123 , node_metadata_ (new OutofcoreOctreeNodeMetadata)
124 {
125 node_metadata_->setOutofcoreVersion (3);
126
127 //Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL)
128 if (super == nullptr)
129 {
130 node_metadata_->setDirectoryPathname (directory_path.parent_path ());
131 node_metadata_->setMetadataFilename (directory_path);
132 depth_ = 0;
133 root_node_ = this;
134
135 //Check if the specified directory to load currently exists; if not, don't continue
136 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
137 {
138 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ());
139 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
140 }
141 }
142 else
143 {
144 node_metadata_->setDirectoryPathname (directory_path);
145 depth_ = super->getDepth () + 1;
146 root_node_ = super->root_node_;
147
148 boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator
149
150 //flag to test if the desired metadata file was found
151 bool b_loaded = false;
152
153 for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
154 {
155 const boost::filesystem::path& file = *directory_it;
156
157 if (!boost::filesystem::is_directory (file))
158 {
159 if (file.extension ().string () == node_index_extension)
160 {
161 b_loaded = node_metadata_->loadMetadataFromDisk (file);
162 break;
163 }
164 }
165 }
166
167 if (!b_loaded)
168 {
169 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
170 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
171 }
172 }
173
174 //load the metadata
175 loadFromFile (node_metadata_->getMetadataFilename (), super);
176
177 //set the number of children in this node
179
180 if (load_all)
181 {
182 loadChildren (true);
183 }
184 }
185////////////////////////////////////////////////////////////////////////////////
186
187 template<typename ContainerT, typename PointT>
188 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)
189 : m_tree_ (tree)
190 , root_node_ ()
191 , parent_ ()
192 , depth_ ()
193 , children_ (8, nullptr)
194 , num_children_ (0)
195 , num_loaded_children_ (0)
196 , payload_ ()
197 , node_metadata_ (new OutofcoreOctreeNodeMetadata ())
198 {
199 assert (tree != nullptr);
200 node_metadata_->setOutofcoreVersion (3);
201 init_root_node (bb_min, bb_max, tree, root_name);
202 }
203
204 ////////////////////////////////////////////////////////////////////////////////
205
206 template<typename ContainerT, typename PointT> void
207 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)
208 {
209 assert (tree != nullptr);
210
211 parent_ = nullptr;
212 root_node_ = this;
213 m_tree_ = tree;
214 depth_ = 0;
215
216 //Mark the children as unallocated
217 num_children_ = 0;
218
219 Eigen::Vector3d tmp_max = bb_max;
220
221 // Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded
222 double epsilon = 1e-8;
223 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
224
225 node_metadata_->setBoundingBox (bb_min, tmp_max);
226 node_metadata_->setDirectoryPathname (root_name.parent_path ());
227 node_metadata_->setOutofcoreVersion (3);
228
229 // If the root directory doesn't exist create it
230 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
231 {
232 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
233 }
234 // If the root directory is a file, do not continue
235 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
236 {
237 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
238 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
239 }
240
241 // Create a unique id for node file name
242 std::string uuid;
243
245
246 std::string node_container_name;
247
248 node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension;
249
250 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
251 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
252
253 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
254 node_metadata_->serializeMetadataToDisk ();
255
256 // Create data container, ie octree_disk_container, octree_ram_container
257 payload_.reset (new ContainerT (node_metadata_->getPCDFilename ()));
258 }
259
260 ////////////////////////////////////////////////////////////////////////////////
261
262 template<typename ContainerT, typename PointT>
264 {
265 // Recursively delete all children and this nodes data
266 recFreeChildren ();
267 }
268
269 ////////////////////////////////////////////////////////////////////////////////
270
271 template<typename ContainerT, typename PointT> std::size_t
273 {
274 std::size_t child_count = 0;
275
276 for(std::size_t i=0; i<8; i++)
277 {
278 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
279 if (boost::filesystem::exists (child_path))
280 child_count++;
281 }
282 return (child_count);
283 }
284
285 ////////////////////////////////////////////////////////////////////////////////
286
287 template<typename ContainerT, typename PointT> void
289 {
290 node_metadata_->serializeMetadataToDisk ();
291
292 if (recursive)
293 {
294 for (std::size_t i = 0; i < 8; i++)
295 {
296 if (children_[i])
297 children_[i]->saveIdx (true);
298 }
299 }
300 }
301
302 ////////////////////////////////////////////////////////////////////////////////
303
304 template<typename ContainerT, typename PointT> bool
306 {
307 return (this->getNumLoadedChildren () < this->getNumChildren ());
308 }
309 ////////////////////////////////////////////////////////////////////////////////
310
311 template<typename ContainerT, typename PointT> void
313 {
314 //if we have fewer children loaded than exist on disk, load them
315 if (num_loaded_children_ < this->getNumChildren ())
316 {
317 //check all 8 possible child directories
318 for (int i = 0; i < 8; i++)
319 {
320 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
321 //if the directory exists and the child hasn't been created (set to 0 by this node's constructor)
322 if (boost::filesystem::exists (child_dir) && this->children_[i] == nullptr)
323 {
324 //load the child node
325 this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive);
326 //keep track of the children loaded
327 num_loaded_children_++;
328 }
329 }
330 }
331 assert (num_loaded_children_ == this->getNumChildren ());
332 }
333 ////////////////////////////////////////////////////////////////////////////////
334
335 template<typename ContainerT, typename PointT> void
337 {
338 if (num_children_ == 0)
339 {
340 return;
341 }
342
343 for (std::size_t i = 0; i < 8; i++)
344 {
345 delete static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(children_[i]);
346 }
347 children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (nullptr));
348 num_children_ = 0;
349 }
350 ////////////////////////////////////////////////////////////////////////////////
351
352 template<typename ContainerT, typename PointT> std::uint64_t
354 {
355 //quit if there are no points to add
356 if (p.empty ())
357 {
358 return (0);
359 }
360
361 //if this depth is the max depth of the tree, then add the points
362 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
363 return (addDataAtMaxDepth( p, skip_bb_check));
364
365 if (hasUnloadedChildren ())
366 loadChildren (false);
367
368 std::vector < std::vector<const PointT*> > c;
369 c.resize (8);
370 for (std::size_t i = 0; i < 8; i++)
371 {
372 c[i].reserve (p.size () / 8);
373 }
374
375 const std::size_t len = p.size ();
376 for (std::size_t i = 0; i < len; i++)
377 {
378 const PointT& pt = p[i];
379
380 if (!skip_bb_check)
381 {
382 if (!this->pointInBoundingBox (pt))
383 {
384 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
385 continue;
386 }
387 }
388
389 std::uint8_t box = 0;
390 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
391
392 box = static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
393 c[static_cast<std::size_t>(box)].push_back (&pt);
394 }
395
396 std::uint64_t points_added = 0;
397 for (std::size_t i = 0; i < 8; i++)
398 {
399 if (c[i].empty ())
400 continue;
401 if (!children_[i])
402 createChild (i);
403 points_added += children_[i]->addDataToLeaf (c[i], true);
404 c[i].clear ();
405 }
406 return (points_added);
407 }
408 ////////////////////////////////////////////////////////////////////////////////
409
410
411 template<typename ContainerT, typename PointT> std::uint64_t
412 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check)
413 {
414 if (p.empty ())
415 {
416 return (0);
417 }
418
419 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
420 {
421 //trust me, just add the points
422 if (skip_bb_check)
423 {
424 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
425
426 payload_->insertRange (p.data (), p.size ());
427
428 return (p.size ());
429 }
430 //check which points belong to this node, throw away the rest
431 std::vector<const PointT*> buff;
432 for (const PointT* pt : p)
433 {
434 if(pointInBoundingBox(*pt))
435 {
436 buff.push_back(pt);
437 }
438 }
439
440 if (!buff.empty ())
441 {
442 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
443 payload_->insertRange (buff.data (), buff.size ());
444// payload_->insertRange ( buff );
445
446 }
447 return (buff.size ());
448 }
449
450 if (this->hasUnloadedChildren ())
451 {
452 loadChildren (false);
453 }
454
455 std::vector < std::vector<const PointT*> > c;
456 c.resize (8);
457 for (std::size_t i = 0; i < 8; i++)
458 {
459 c[i].reserve (p.size () / 8);
460 }
461
462 const std::size_t len = p.size ();
463 for (std::size_t i = 0; i < len; i++)
464 {
465 //const PointT& pt = p[i];
466 if (!skip_bb_check)
467 {
468 if (!this->pointInBoundingBox (*p[i]))
469 {
470 // std::cerr << "failed to place point!!!" << std::endl;
471 continue;
472 }
473 }
474
475 std::uint8_t box = 00;
476 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
477 //hash each coordinate to the appropriate octant
478 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] )));
479 //3 bit, 8 octants
480 c[box].push_back (p[i]);
481 }
482
483 std::uint64_t points_added = 0;
484 for (std::size_t i = 0; i < 8; i++)
485 {
486 if (c[i].empty ())
487 continue;
488 if (!children_[i])
489 createChild (i);
490 points_added += children_[i]->addDataToLeaf (c[i], true);
491 c[i].clear ();
492 }
493 return (points_added);
494 }
495 ////////////////////////////////////////////////////////////////////////////////
496
497
498 template<typename ContainerT, typename PointT> std::uint64_t
499 OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check)
500 {
501 assert (this->root_node_->m_tree_ != nullptr);
502
503 if (input_cloud->height*input_cloud->width == 0)
504 return (0);
505
506 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
507 return (addDataAtMaxDepth (input_cloud, true));
508
509 if( num_children_ < 8 )
510 if(hasUnloadedChildren ())
511 loadChildren (false);
512
513 if( !skip_bb_check )
514 {
515
516 //indices to store the points for each bin
517 //these lists will be used to copy data to new point clouds and pass down recursively
518 std::vector < pcl::Indices > indices;
519 indices.resize (8);
520
521 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
522
523 for(std::size_t k=0; k<indices.size (); k++)
524 {
525 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
526 }
527
528 std::uint64_t points_added = 0;
529
530 for(std::size_t i=0; i<8; i++)
531 {
532 if ( indices[i].empty () )
533 continue;
534
535 if (children_[i] == nullptr)
536 {
537 createChild (i);
538 }
539
541
542 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
543
544 //copy the points from extracted indices from input cloud to destination cloud
545 pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ;
546
547 //recursively add the new cloud to the data
548 points_added += children_[i]->addPointCloud (dst_cloud, false);
549 indices[i].clear ();
550 }
551
552 return (points_added);
553 }
554
555 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
556
557 return 0;
558 }
559
560
561 ////////////////////////////////////////////////////////////////////////////////
562 template<typename ContainerT, typename PointT> void
564 {
565 assert (this->root_node_->m_tree_ != nullptr);
566
567 AlignedPointTVector sampleBuff;
568 if (!skip_bb_check)
569 {
570 for (const PointT& pt: p)
571 {
572 if (pointInBoundingBox(pt))
573 {
574 sampleBuff.push_back(pt);
575 }
576 }
577 }
578 else
579 {
580 sampleBuff = p;
581 }
582
583 // Derive percentage from specified sample_percent and tree depth
584 const double percent = pow(sample_percent_, static_cast<double>((this->root_node_->m_tree_->getDepth () - depth_)));
585 const auto samplesize = static_cast<std::uint64_t>(percent * static_cast<double>(sampleBuff.size()));
586 const std::uint64_t inputsize = sampleBuff.size();
587
588 if(samplesize > 0)
589 {
590 // Resize buffer to sample size
591 insertBuff.resize(samplesize);
592
593 // Create random number generator
594 std::lock_guard<std::mutex> lock(rng_mutex_);
595 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
596
597 // Randomly pick sampled points
598 for(std::uint64_t i = 0; i < samplesize; ++i)
599 {
600 std::uint64_t buffstart = buffdist(rng_);
601 insertBuff[i] = ( sampleBuff[buffstart] );
602 }
603 }
604 // Have to do it the slow way
605 else
606 {
607 std::lock_guard<std::mutex> lock(rng_mutex_);
608 std::bernoulli_distribution buffdist(percent);
609
610 for(std::uint64_t i = 0; i < inputsize; ++i)
611 if(buffdist(rng_))
612 insertBuff.push_back( p[i] );
613 }
614 }
615 ////////////////////////////////////////////////////////////////////////////////
616
617 template<typename ContainerT, typename PointT> std::uint64_t
619 {
620 assert (this->root_node_->m_tree_ != nullptr);
621
622 // Trust me, just add the points
623 if (skip_bb_check)
624 {
625 // Increment point count for node
626 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
627
628 // Insert point data
629 payload_->insertRange ( p );
630
631 return (p.size ());
632 }
633
634 // Add points found within the current node's bounding box
636 const std::size_t len = p.size ();
637
638 for (std::size_t i = 0; i < len; i++)
639 {
640 if (pointInBoundingBox (p[i]))
641 {
642 buff.push_back (p[i]);
643 }
644 }
645
646 if (!buff.empty ())
647 {
648 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
649 payload_->insertRange ( buff );
650 }
651 return (buff.size ());
652 }
653 ////////////////////////////////////////////////////////////////////////////////
654 template<typename ContainerT, typename PointT> std::uint64_t
656 {
657 //this assumes data is already in the correct bin
658 if(skip_bb_check)
659 {
660 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
661
662 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
663 payload_->insertRange (input_cloud);
664
665 return (input_cloud->width*input_cloud->height);
666 }
667 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
668 return (0);
669 }
670
671
672 ////////////////////////////////////////////////////////////////////////////////
673 template<typename ContainerT, typename PointT> void
674 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
675 {
676 // Reserve space for children nodes
677 c.resize(8);
678 for(std::size_t i = 0; i < 8; i++)
679 c[i].reserve(p.size() / 8);
680
681 const std::size_t len = p.size();
682 for(std::size_t i = 0; i < len; i++)
683 {
684 const PointT& pt = p[i];
685
686 if(!skip_bb_check)
687 if(!this->pointInBoundingBox(pt))
688 continue;
689
690 subdividePoint (pt, c);
691 }
692 }
693 ////////////////////////////////////////////////////////////////////////////////
694
695 template<typename ContainerT, typename PointT> void
696 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c)
697 {
698 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
699 std::size_t octant = 0;
700 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
701 c[octant].push_back (point);
702 }
703
704 ////////////////////////////////////////////////////////////////////////////////
705 template<typename ContainerT, typename PointT> std::uint64_t
707 {
708 std::uint64_t points_added = 0;
709
710 if ( input_cloud->width * input_cloud->height == 0 )
711 {
712 return (0);
713 }
714
715 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
716 {
717 std::uint64_t points_added = addDataAtMaxDepth (input_cloud, true);
718 assert (points_added > 0);
719 return (points_added);
720 }
721
722 if (num_children_ < 8 )
723 {
724 if ( hasUnloadedChildren () )
725 {
726 loadChildren (false);
727 }
728 }
729
730 //------------------------------------------------------------
731 //subsample data:
732 // 1. Get indices from a random sample
733 // 2. Extract those indices with the extract indices class (in order to also get the complement)
734 //------------------------------------------------------------
736 random_sampler.setInputCloud (input_cloud);
737
738 //set sample size to 1/8 of total points (12.5%)
739 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
740 random_sampler.setSample (static_cast<unsigned int> (sample_size));
741
742 //create our destination
743 pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () );
744
745 //create destination for indices
746 pcl::IndicesPtr downsampled_cloud_indices ( new pcl::Indices () );
747 random_sampler.filter (*downsampled_cloud_indices);
748
749 //extract the "random subset", size by setSampleSize
751 extractor.setInputCloud (input_cloud);
752 extractor.setIndices (downsampled_cloud_indices);
753 extractor.filter (*downsampled_cloud);
754
755 //extract the complement of those points (i.e. everything remaining)
756 pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () );
757 extractor.setNegative (true);
758 extractor.filter (*remaining_points);
759
760 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 );
761
762 //insert subsampled data to the node's disk container payload
763 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
764 {
765 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
766 payload_->insertRange (downsampled_cloud);
767 points_added += downsampled_cloud->width*downsampled_cloud->height ;
768 }
769
770 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
771
772 //subdivide remaining data by destination octant
773 std::vector<pcl::Indices> indices;
774 indices.resize (8);
775
776 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
777
778 //pass each set of points to the appropriate child octant
779 for(std::size_t i=0; i<8; i++)
780 {
781
782 if(indices[i].empty ())
783 continue;
784
785 if (children_[i] == nullptr)
786 {
787 assert (i < 8);
788 createChild (i);
789 }
790
791 //copy correct indices into a temporary cloud
792 pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ());
793 pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud);
794
795 //recursively add points and keep track of how many were successfully added to the tree
796 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
797 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);
798
799 }
800 assert (points_added == input_cloud->width*input_cloud->height);
801 return (points_added);
802 }
803 ////////////////////////////////////////////////////////////////////////////////
804
805 template<typename ContainerT, typename PointT> std::uint64_t
807 {
808 // If there are no points return
809 if (p.empty ())
810 return (0);
811
812 // when adding data and generating sampled LOD
813 // If the max depth has been reached
814 assert (this->root_node_->m_tree_ != nullptr );
815
816 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
817 {
818 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
819 return (addDataAtMaxDepth(p, false));
820 }
821
822 // Create child nodes of the current node but not grand children+
823 if (this->hasUnloadedChildren ())
824 loadChildren (false /*no recursive loading*/);
825
826 // Randomly sample data
827 AlignedPointTVector insertBuff;
828 randomSample(p, insertBuff, skip_bb_check);
829
830 if(!insertBuff.empty())
831 {
832 // Increment point count for node
833 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
834 // Insert sampled point data
835 payload_->insertRange (insertBuff);
836
837 }
838
839 //subdivide vec to pass data down lower
840 std::vector<AlignedPointTVector> c;
841 subdividePoints(p, c, skip_bb_check);
842
843 std::uint64_t points_added = 0;
844 for(std::size_t i = 0; i < 8; i++)
845 {
846 // If child doesn't have points
847 if(c[i].empty())
848 continue;
849
850 // If child doesn't exist
851 if(!children_[i])
852 createChild(i);
853
854 // Recursively build children
855 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true);
856 c[i].clear();
857 }
858
859 return (points_added);
860 }
861 ////////////////////////////////////////////////////////////////////////////////
862
863 template<typename ContainerT, typename PointT> void
865 {
866 assert (idx < 8);
867
868 //if already has 8 children, return
869 if (children_[idx] || (num_children_ == 8))
870 {
871 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
872 return;
873 }
874
875 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
876 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0);
877
878 Eigen::Vector3d childbb_min;
879 Eigen::Vector3d childbb_max;
880
881 int x, y, z;
882 if (idx > 3)
883 {
884 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
885 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
886 z = 1;
887 }
888 else
889 {
890 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
891 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
892 z = 0;
893 }
894
895 childbb_min[2] = start[2] + static_cast<double> (z) * step[2];
896 childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2];
897
898 childbb_min[1] = start[1] + static_cast<double> (y) * step[1];
899 childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1];
900
901 childbb_min[0] = start[0] + static_cast<double> (x) * step[0];
902 childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0];
903
904 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
905 children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this);
906
907 num_children_++;
908 }
909 ////////////////////////////////////////////////////////////////////////////////
910
911 template<typename ContainerT, typename PointT> bool
912 pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point)
913 {
914 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
915 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
916 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
917 {
918 return (true);
919
920 }
921 return (false);
922 }
923
924
925 ////////////////////////////////////////////////////////////////////////////////
926 template<typename ContainerT, typename PointT> bool
928 {
929 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
930 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
931
932 if (((min[0] <= p.x) && (p.x < max[0])) &&
933 ((min[1] <= p.y) && (p.y < max[1])) &&
934 ((min[2] <= p.z) && (p.z < max[2])))
935 {
936 return (true);
937
938 }
939 return (false);
940 }
941
942 ////////////////////////////////////////////////////////////////////////////////
943 template<typename ContainerT, typename PointT> void
945 {
946 Eigen::Vector3d min;
947 Eigen::Vector3d max;
948 node_metadata_->getBoundingBox (min, max);
949
950 if (this->depth_ < query_depth){
951 for (std::size_t i = 0; i < this->depth_; i++)
952 std::cout << " ";
953
954 std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - ";
955 std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - ";
956 std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1];
957 std::cout << ", " << max[2] - min[2] << "]" << std::endl;
958
959 if (num_children_ > 0)
960 {
961 for (std::size_t i = 0; i < 8; i++)
962 {
963 if (children_[i])
964 children_[i]->printBoundingBox (query_depth);
965 }
966 }
967 }
968 }
969
970 ////////////////////////////////////////////////////////////////////////////////
971 template<typename ContainerT, typename PointT> void
973 {
974 if (this->depth_ < query_depth){
975 if (num_children_ > 0)
976 {
977 for (std::size_t i = 0; i < 8; i++)
978 {
979 if (children_[i])
980 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
981 }
982 }
983 }
984 else
985 {
986 PointT voxel_center;
987 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
988 voxel_center.x = static_cast<float>(mid_xyz[0]);
989 voxel_center.y = static_cast<float>(mid_xyz[1]);
990 voxel_center.z = static_cast<float>(mid_xyz[2]);
991
992 voxel_centers.push_back(voxel_center);
993 }
994 }
995
996 ////////////////////////////////////////////////////////////////////////////////
997// Eigen::Vector3d cornerOffsets[] =
998// {
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// Eigen::Vector3d(-1.0, -1.0, 1.0), // + - -
1004// Eigen::Vector3d( 1.0, -1.0, 1.0), // + - +
1005// Eigen::Vector3d(-1.0, 1.0, 1.0), // + + -
1006// Eigen::Vector3d( 1.0, 1.0, 1.0) // + + +
1007// };
1008//
1009// // Note that the input vector must already be negated when using this code for halfplane tests
1010// int
1011// vectorToIndex(Eigen::Vector3d normal)
1012// {
1013// int index = 0;
1014//
1015// if (normal.z () >= 0) index |= 1;
1016// if (normal.y () >= 0) index |= 2;
1017// if (normal.x () >= 0) index |= 4;
1018//
1019// return index;
1020// }
1021//
1022// void
1023// get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb)
1024// {
1025//
1026// p_vertex = min_bb;
1027// n_vertex = max_bb;
1028//
1029// if (normal.x () >= 0)
1030// {
1031// n_vertex.x () = min_bb.x ();
1032// p_vertex.x () = max_bb.x ();
1033// }
1034//
1035// if (normal.y () >= 0)
1036// {
1037// n_vertex.y () = min_bb.y ();
1038// p_vertex.y () = max_bb.y ();
1039// }
1040//
1041// if (normal.z () >= 0)
1042// {
1043// p_vertex.z () = max_bb.z ();
1044// n_vertex.z () = min_bb.z ();
1045// }
1046// }
1047
1048 template<typename Container, typename PointT> void
1049 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names)
1050 {
1051 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1052 }
1053
1054 template<typename Container, typename PointT> void
1055 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)
1056 {
1057
1058 enum {INSIDE, INTERSECT, OUTSIDE};
1059
1060 int result = INSIDE;
1061
1062 if (this->depth_ > query_depth)
1063 {
1064 return;
1065 }
1066
1067// if (this->depth_ > query_depth)
1068// return;
1069
1070 if (!skip_vfc_check)
1071 {
1072 for(int i =0; i < 6; i++){
1073 double a = planes[(i*4)];
1074 double b = planes[(i*4)+1];
1075 double c = planes[(i*4)+2];
1076 double d = planes[(i*4)+3];
1077
1078 //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1079
1080 Eigen::Vector3d normal(a, b, c);
1081
1082 Eigen::Vector3d min_bb;
1083 Eigen::Vector3d max_bb;
1084 node_metadata_->getBoundingBox(min_bb, max_bb);
1085
1086 // Basic VFC algorithm
1087 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1088 Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1089 std::abs (static_cast<double> (max_bb.y () - center.y ())),
1090 std::abs (static_cast<double> (max_bb.z () - center.z ())));
1091
1092 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1093 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1094
1095 if (m + n < 0){
1096 result = OUTSIDE;
1097 break;
1098 }
1099
1100 if (m - n < 0) result = INTERSECT;
1101
1102 // // n-p implementation
1103 // Eigen::Vector3d p_vertex; //pos vertex
1104 // Eigen::Vector3d n_vertex; //neg vertex
1105 // get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb);
1106 //
1107 // std::cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << std::endl;
1108 // std::cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << std::endl;
1109
1110 // is the positive vertex outside?
1111 // if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0)
1112 // {
1113 // result = OUTSIDE;
1114 // }
1115 // // is the negative vertex outside?
1116 // else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0)
1117 // {
1118 // result = INTERSECT;
1119 // }
1120
1121 //
1122 //
1123 // // This should be the same as below
1124 // if (normal.dot(n_vertex) + d > 0)
1125 // {
1126 // result = OUTSIDE;
1127 // }
1128 //
1129 // if (normal.dot(p_vertex) + d >= 0)
1130 // {
1131 // result = INTERSECT;
1132 // }
1133
1134 // This should be the same as above
1135 // double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ());
1136 // std::cout << "m = " << m << std::endl;
1137 // if (m > -d)
1138 // {
1139 // result = OUTSIDE;
1140 // }
1141 //
1142 // double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ());
1143 // std::cout << "n = " << n << std::endl;
1144 // if (n > -d)
1145 // {
1146 // result = INTERSECT;
1147 // }
1148 }
1149 }
1150
1151 if (result == OUTSIDE)
1152 {
1153 return;
1154 }
1155
1156// switch(result){
1157// case OUTSIDE:
1158// //std::cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1159// return;
1160// case INTERSECT:
1161// //std::cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << std::endl;
1162// break;
1163// case INSIDE:
1164// //std::cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << std::endl;
1165// break;
1166// }
1167
1168 // Add files breadth first
1169 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1170 //if (payload_->getDataSize () > 0)
1171 {
1172 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1173 }
1174
1175 if (hasUnloadedChildren ())
1176 {
1177 loadChildren (false);
1178 }
1179
1180 if (this->getNumChildren () > 0)
1181 {
1182 for (std::size_t i = 0; i < 8; i++)
1183 {
1184 if (children_[i])
1185 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1186 }
1187 }
1188// else if (hasUnloadedChildren ())
1189// {
1190// loadChildren (false);
1191//
1192// for (std::size_t i = 0; i < 8; i++)
1193// {
1194// if (children_[i])
1195// children_[i]->queryFrustum (planes, file_names, query_depth);
1196// }
1197// }
1198 //}
1199 }
1200
1201////////////////////////////////////////////////////////////////////////////////
1202
1203 template<typename Container, typename PointT> void
1204 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)
1205 {
1206
1207 // If we're above our query depth
1208 if (this->depth_ > query_depth)
1209 {
1210 return;
1211 }
1212
1213 // Bounding Box
1214 Eigen::Vector3d min_bb;
1215 Eigen::Vector3d max_bb;
1216 node_metadata_->getBoundingBox(min_bb, max_bb);
1217
1218 // Frustum Culling
1219 enum {INSIDE, INTERSECT, OUTSIDE};
1220
1221 int result = INSIDE;
1222
1223 if (!skip_vfc_check)
1224 {
1225 for(int i =0; i < 6; i++){
1226 double a = planes[(i*4)];
1227 double b = planes[(i*4)+1];
1228 double c = planes[(i*4)+2];
1229 double d = planes[(i*4)+3];
1230
1231 //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl;
1232
1233 Eigen::Vector3d normal(a, b, c);
1234
1235 // Basic VFC algorithm
1236 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1237 Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())),
1238 std::abs (static_cast<double> (max_bb.y () - center.y ())),
1239 std::abs (static_cast<double> (max_bb.z () - center.z ())));
1240
1241 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1242 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1243
1244 if (m + n < 0){
1245 result = OUTSIDE;
1246 break;
1247 }
1248
1249 if (m - n < 0) result = INTERSECT;
1250
1251 }
1252 }
1253
1254 if (result == OUTSIDE)
1255 {
1256 return;
1257 }
1258
1259 // Bounding box projection
1260 // 3--------2
1261 // /| /| Y 0 = xmin, ymin, zmin
1262 // / | / | | 6 = xmax, ymax. zmax
1263 // 7--------6 | |
1264 // | | | | |
1265 // | 0-----|--1 +------X
1266 // | / | / /
1267 // |/ |/ /
1268 // 4--------5 Z
1269
1270// bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1271// bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0);
1272// bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1273// bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0);
1274// bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1275// bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0);
1276// bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1277// bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0);
1278
1279 int width = 500;
1280 int height = 500;
1281
1282 float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height);
1283 //float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix);
1284
1285// for (int i=0; i < this->depth_; i++) std::cout << " ";
1286// std::cout << this->depth_ << ": " << coverage << std::endl;
1287
1288 // Add files breadth first
1289 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1290 //if (payload_->getDataSize () > 0)
1291 {
1292 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1293 }
1294
1295 //if (coverage <= 0.075)
1296 if (coverage <= 10000)
1297 return;
1298
1299 if (hasUnloadedChildren ())
1300 {
1301 loadChildren (false);
1302 }
1303
1304 if (this->getNumChildren () > 0)
1305 {
1306 for (std::size_t i = 0; i < 8; i++)
1307 {
1308 if (children_[i])
1309 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/);
1310 }
1311 }
1312 }
1313
1314////////////////////////////////////////////////////////////////////////////////
1315 template<typename ContainerT, typename PointT> void
1316 OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth)
1317 {
1318 if (this->depth_ < query_depth){
1319 if (num_children_ > 0)
1320 {
1321 for (std::size_t i = 0; i < 8; i++)
1322 {
1323 if (children_[i])
1324 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1325 }
1326 }
1327 }
1328 else
1329 {
1330 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1331 voxel_centers.push_back(voxel_center);
1332 }
1333 }
1334
1335
1336 ////////////////////////////////////////////////////////////////////////////////
1337
1338 template<typename ContainerT, typename PointT> void
1339 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)
1340 {
1341 if (intersectsWithBoundingBox (min_bb, max_bb))
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 (min_bb, max_bb, 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 (min_bb, max_bb, 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 auto 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_ != nullptr);
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 = node_metadata_->getPCDFilename ().stem ().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 {
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:67
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.
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:281
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
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.