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