Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
octree_base.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#ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
41#define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
42
43
44#include <pcl/outofcore/octree_base.h>
45
46// JSON
47#include <pcl/outofcore/cJSON.h>
48
49#include <pcl/filters/random_sample.h>
50#include <pcl/filters/extract_indices.h>
51
52// C++
53#include <iostream>
54#include <fstream>
55#include <sstream>
56#include <string>
57#include <exception>
58
59namespace pcl
60{
61 namespace outofcore
62 {
63
64 ////////////////////////////////////////////////////////////////////////////////
65 //Static constants
66 ////////////////////////////////////////////////////////////////////////////////
67
68 template<typename ContainerT, typename PointT>
70
71 template<typename ContainerT, typename PointT>
73
74 ////////////////////////////////////////////////////////////////////////////////
75
76 template<typename ContainerT, typename PointT>
77 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::filesystem::path& root_name, const bool load_all)
78 : root_node_ ()
79 , read_write_mutex_ ()
80 , metadata_ (new OutofcoreOctreeBaseMetadata ())
81 , sample_percent_ (0.125)
82 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
83 {
84 //validate the root filename
85 if (!this->checkExtension (root_name))
86 {
87 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
88 }
89
90 // Create root_node_node
91 root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, nullptr, load_all);
92 // Set root_node_nodes tree to the newly created tree
93 root_node_->m_tree_ = this;
94
95 // Set the path to the outofcore octree metadata (unique to the root folder) ending in .octree
96 boost::filesystem::path treepath = root_name.parent_path () / (root_name.stem ().string () + TREE_EXTENSION_);
97
98 //Load the JSON metadata
99 metadata_->loadMetadataFromDisk (treepath);
100 }
101
102 ////////////////////////////////////////////////////////////////////////////////
103
104 template<typename ContainerT, typename PointT>
105 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
106 : root_node_()
107 , read_write_mutex_ ()
108 , metadata_ (new OutofcoreOctreeBaseMetadata ())
109 , sample_percent_ (0.125)
110 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
111 {
112 //Enlarge the bounding box to a cube so our voxels will be cubes
113 Eigen::Vector3d tmp_min = min;
114 Eigen::Vector3d tmp_max = max;
115 this->enlargeToCube (tmp_min, tmp_max);
116
117 //Compute the depth of the tree given the resolution
118 std::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
119
120 //Create a new outofcore tree
121 this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
122 }
123
124 ////////////////////////////////////////////////////////////////////////////////
125
126 template<typename ContainerT, typename PointT>
127 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const std::uint64_t max_depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
128 : root_node_()
129 , read_write_mutex_ ()
130 , metadata_ (new OutofcoreOctreeBaseMetadata ())
131 , sample_percent_ (0.125)
132 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
133 {
134 //Create a new outofcore tree
135 this->init (max_depth, min, max, root_node_name, coord_sys);
136 }
137
138 ////////////////////////////////////////////////////////////////////////////////
139 template<typename ContainerT, typename PointT> void
140 OutofcoreOctreeBase<ContainerT, PointT>::init (const std::uint64_t& depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_name, const std::string& coord_sys)
141 {
142 //Validate the extension of the pathname
143 if (!this->checkExtension (root_name))
144 {
145 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
146 }
147
148 //Check to make sure that we are not overwriting existing data
149 if (boost::filesystem::exists (root_name.parent_path ()))
150 {
151 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase] A dir named %s already exists. Overwriting an existing tree is not supported.\n", root_name.parent_path ().c_str () );
152 PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
153 }
154
155 // Get fullpath and recreate directories
156 boost::filesystem::path dir = root_name.parent_path ();
157
158 if (!boost::filesystem::exists (dir))
159 {
160 boost::filesystem::create_directory (dir);
161 }
162
163 Eigen::Vector3d tmp_min = min;
164 Eigen::Vector3d tmp_max = max;
165 this->enlargeToCube (tmp_min, tmp_max);
166
167 // Create root node
168 root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name);
169 root_node_->m_tree_ = this;
170
171 // Set root nodes file path
172 boost::filesystem::path treepath = dir / (root_name.stem ().string () + TREE_EXTENSION_);
173
174 //fill the fields of the metadata
175 metadata_->setCoordinateSystem (coord_sys);
176 metadata_->setDepth (depth);
177 metadata_->setLODPoints (depth+1);
178 metadata_->setMetadataFilename (treepath);
179 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
180 //metadata_->setPointType ( <point type string here> );
181
182 //save to disk
183 metadata_->serializeMetadataToDisk ();
184 }
185
186
187 ////////////////////////////////////////////////////////////////////////////////
188 template<typename ContainerT, typename PointT>
190 {
191 root_node_->flushToDiskRecursive ();
192
193 saveToFile ();
194 delete (root_node_);
195 }
196
197 ////////////////////////////////////////////////////////////////////////////////
198
199 template<typename ContainerT, typename PointT> void
201 {
202 this->metadata_->serializeMetadataToDisk ();
204
205 ////////////////////////////////////////////////////////////////////////////////
206
207 template<typename ContainerT, typename PointT> std::uint64_t
209 {
210 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
211
212 constexpr bool _FORCE_BB_CHECK = true;
213
214 std::uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
215
216 assert (p.size () == pt_added);
217
218 return (pt_added);
219 }
220
221 ////////////////////////////////////////////////////////////////////////////////
222
223 template<typename ContainerT, typename PointT> std::uint64_t
225 {
226 return (addDataToLeaf (point_cloud->points));
227 }
228
229 ////////////////////////////////////////////////////////////////////////////////
230
231 template<typename ContainerT, typename PointT> std::uint64_t
234 std::uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
235// assert (input_cloud->width*input_cloud->height == pt_added);
236 return (pt_added);
237 }
238
239
240 ////////////////////////////////////////////////////////////////////////////////
241
242 template<typename ContainerT, typename PointT> std::uint64_t
245 // Lock the tree while writing
246 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
247 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points, false);
248 return (pt_added);
249 }
250
251 ////////////////////////////////////////////////////////////////////////////////
252
253 template<typename ContainerT, typename PointT> std::uint64_t
255 {
256 // Lock the tree while writing
257 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
258 std::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
259
260 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
261
262 assert ( input_cloud->width*input_cloud->height == pt_added );
263
264 return (pt_added);
265 }
266
267 ////////////////////////////////////////////////////////////////////////////////
268
269 template<typename ContainerT, typename PointT> std::uint64_t
271 {
272 // Lock the tree while writing
273 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
274 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src, false);
275 return (pt_added);
276 }
277
278 ////////////////////////////////////////////////////////////////////////////////
279
280 template<typename Container, typename PointT> void
281 OutofcoreOctreeBase<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) const
283 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
284 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
285 }
286
287 ////////////////////////////////////////////////////////////////////////////////
289 template<typename Container, typename PointT> void
290 OutofcoreOctreeBase<Container, PointT>::queryFrustum(const double *planes, std::list<std::string>& file_names, const std::uint32_t query_depth) const
291 {
292 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
293 root_node_->queryFrustum (planes, file_names, query_depth);
294 }
296 ////////////////////////////////////////////////////////////////////////////////
297
298 template<typename Container, typename PointT> void
300 const double *planes,
301 const Eigen::Vector3d &eye,
302 const Eigen::Matrix4d &view_projection_matrix,
303 std::list<std::string>& file_names,
304 const std::uint32_t query_depth) const
305 {
306 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
307 root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
308 }
309
310 ////////////////////////////////////////////////////////////////////////////////
311
312 template<typename ContainerT, typename PointT> void
313 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, AlignedPointTVector& dst) const
314 {
315 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
316 dst.clear ();
317 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode] Querying Bounding Box %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf", min[0], min[1], min[2], max[0], max[1], max[2]);
318 root_node_->queryBBIncludes (min, max, query_depth, dst);
319 }
320
321 ////////////////////////////////////////////////////////////////////////////////
323 template<typename ContainerT, typename PointT> void
324 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) const
325 {
326 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
327
328 dst_blob->data.clear ();
329 dst_blob->width = 0;
330 dst_blob->height =1;
331
332 root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
333 }
334
335 ////////////////////////////////////////////////////////////////////////////////
337 template<typename ContainerT, typename PointT> void
338 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, const double percent, AlignedPointTVector& dst) const
339 {
340 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
341 dst.clear ();
342 root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
343 }
344
345 ////////////////////////////////////////////////////////////////////////////////
346 template<typename ContainerT, typename PointT> void
347 OutofcoreOctreeBase<ContainerT, PointT>::queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent)
348 {
349 if (percent==1.0)
350 {
351 root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
352 }
353 else
354 {
355 root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
356 }
357 }
359 ////////////////////////////////////////////////////////////////////////////////
360
361 template<typename ContainerT, typename PointT> bool
362 OutofcoreOctreeBase<ContainerT, PointT>::getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const
363 {
364 if (root_node_!= nullptr)
365 {
366 root_node_->getBoundingBox (min, max);
367 return true;
368 }
369 return false;
370 }
371
372 ////////////////////////////////////////////////////////////////////////////////
374 template<typename ContainerT, typename PointT> void
376 {
377 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
378 root_node_->printBoundingBox (query_depth);
379 }
380
381 ////////////////////////////////////////////////////////////////////////////////
382
383 template<typename ContainerT, typename PointT> void
385 {
386 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
387 if (query_depth > metadata_->getDepth ())
388 {
389 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
390 }
391 else
392 {
393 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
394 }
395 }
396
397 ////////////////////////////////////////////////////////////////////////////////
399 template<typename ContainerT, typename PointT> void
400 OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth) const
401 {
402 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
403 if (query_depth > metadata_->getDepth ())
404 {
405 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
406 }
407 else
408 {
409 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
410 }
411 }
412
413 ////////////////////////////////////////////////////////////////////////////////
414
415 template<typename ContainerT, typename PointT> void
416 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name) const
417 {
418 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
419 bin_name.clear ();
420#if defined _MSC_VER
421 #pragma warning(push)
422 #pragma warning(disable : 4267)
423#endif
424 root_node_->queryBBIntersects (min, max, query_depth, bin_name);
425#if defined _MSC_VER
426 #pragma warning(pop)
427#endif
428 }
429
430 ////////////////////////////////////////////////////////////////////////////////
431
432 template<typename ContainerT, typename PointT> void
433 OutofcoreOctreeBase<ContainerT, PointT>::writeVPythonVisual (const boost::filesystem::path& filename)
434 {
435 std::ofstream f (filename.c_str ());
436
437 f << "from visual import *\n\n";
438
439 root_node_->writeVPythonVisual (f);
440 }
441
442 ////////////////////////////////////////////////////////////////////////////////
443
444 template<typename ContainerT, typename PointT> void
446 {
447 root_node_->flushToDisk ();
449
450 ////////////////////////////////////////////////////////////////////////////////
451
452 template<typename ContainerT, typename PointT> void
454 {
455 root_node_->flushToDiskLazy ();
456 }
457
458 ////////////////////////////////////////////////////////////////////////////////
459
460 template<typename ContainerT, typename PointT> void
462 {
463 saveToFile ();
464 root_node_->convertToXYZ ();
465 }
466
467 ////////////////////////////////////////////////////////////////////////////////
468
469 template<typename ContainerT, typename PointT> void
471 {
472 DeAllocEmptyNodeCache (root_node_);
473 }
474
475 ////////////////////////////////////////////////////////////////////////////////
476
477 template<typename ContainerT, typename PointT> void
479 {
480 if (current == nullptr)
481 current = root_node_;
482
483 if (current->size () == 0)
484 {
485 current->flush_DeAlloc_this_only ();
486 }
487
488 for (int i = 0; i < current->numchildren (); i++)
489 {
490 DeAllocEmptyNodeCache (current->children[i]);
491 }
492 }
493
494 ////////////////////////////////////////////////////////////////////////////////
495 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
496 OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
497 {
498 return (branch_arg.getChildPtr (childIdx_arg));
499 }
500
501 ////////////////////////////////////////////////////////////////////////////////
502 template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
505 return (lod_filter_ptr_);
506 }
507
508 ////////////////////////////////////////////////////////////////////////////////
509
510 template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
512 {
513 return (lod_filter_ptr_);
514 }
515
516 ////////////////////////////////////////////////////////////////////////////////
517
518 template<typename ContainerT, typename PointT> void
520 {
521 lod_filter_ptr_ = std::static_pointer_cast<decltype(lod_filter_ptr_)>(filter_arg);
522 }
523
524 ////////////////////////////////////////////////////////////////////////////////
525
526 template<typename ContainerT, typename PointT> bool
528 {
529 if (root_node_== nullptr)
530 {
531 x = 0;
532 y = 0;
533 return (false);
535
536 Eigen::Vector3d min, max;
537 this->getBoundingBox (min, max);
538
539 double depth = static_cast<double> (metadata_->getDepth ());
540 Eigen::Vector3d diff = max-min;
541
542 y = diff[1] * pow (.5, depth);
543 x = diff[0] * pow (.5, depth);
544
545 return (true);
546 }
547
548 ////////////////////////////////////////////////////////////////////////////////
549
550 template<typename ContainerT, typename PointT> double
553 Eigen::Vector3d min, max;
554 this->getBoundingBox (min, max);
555 double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
556 return (result);
557 }
558
559 ////////////////////////////////////////////////////////////////////////////////
560
561 template<typename ContainerT, typename PointT> void
563 {
564 if (root_node_== nullptr)
565 {
566 PCL_ERROR ("Root node is null; aborting buildLOD.\n");
567 return;
568 }
569
570 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
572 constexpr int number_of_nodes = 1;
573
574 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(nullptr));
575 current_branch[0] = root_node_;
576 assert (current_branch.back () != nullptr);
577 this->buildLODRecursive (current_branch);
578 }
579
580 ////////////////////////////////////////////////////////////////////////////////
581
582 template<typename ContainerT, typename PointT> void
584 {
585 Eigen::Vector3d min, max;
586 node.getBoundingBox (min,max);
587 PCL_INFO ("[pcl::outofcore::OutofcoreOctreeBase::%s] min(%lf,%lf,%lf), max(%lf,%lf,%lf)\n", __FUNCTION__, min[0], min[1], min[2], max[0], max[1], max[2]);
588 }
589
590
591 ////////////////////////////////////////////////////////////////////////////////
592
593 template<typename ContainerT, typename PointT> void
594 OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
596 PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
597
598 if (!current_branch.back ())
600 return;
601 }
602
603 if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
605 assert (current_branch.back ()->getDepth () == this->getDepth ());
606
607 BranchNode* leaf = current_branch.back ();
608
609 pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
610 //read the data from the PCD file associated with the leaf; it is full resolution
611 leaf->read (leaf_input_cloud);
612 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
613
614 //go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution
615 for (auto level = static_cast<std::int64_t>(current_branch.size ()-1); level >= 1; level--)
616 {
617 BranchNode* target_parent = current_branch[level-1];
618 assert (target_parent != nullptr);
619 double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
620 double current_depth_sample_percent = pow (sample_percent_, exponent);
621
622 assert (current_depth_sample_percent > 0.0);
623 //------------------------------------------------------------
624 //subsample data:
625 // 1. Get indices from a random sample
626 // 2. Extract those indices with the extract indices class (in order to also get the complement)
627 //------------------------------------------------------------
628
629 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
630
631 //set sample size to 1/8 of total points (12.5%)
632 auto sample_size = static_cast<std::uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
633
634 if (sample_size == 0)
635 sample_size = 1;
636
637 lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
638
639 //create our destination
640 pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
641
642 //create destination for indices
643 pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ());
644 lod_filter_ptr_->filter (*downsampled_cloud_indices);
645
646 //extract the "random subset", size by setSampleSize
648 extractor.setInputCloud (leaf_input_cloud);
649 extractor.setIndices (downsampled_cloud_indices);
650 extractor.filter (*downsampled_cloud);
651
652 //write to the target
653 if (downsampled_cloud->width*downsampled_cloud->height > 0)
654 {
655 target_parent->payload_->insertRange (downsampled_cloud);
656 this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
657 }
658 }
659 }
660 else//not at leaf, keep going down
661 {
662 //clear this node while walking down the tree in case we are updating the LOD
663 current_branch.back ()->clearData ();
664
665 std::vector<BranchNode*> next_branch (current_branch);
666
667 if (current_branch.back ()->hasUnloadedChildren ())
668 {
669 current_branch.back ()->loadChildren (false);
670 }
671
672 for (std::size_t i = 0; i < 8; i++)
673 {
674 next_branch.push_back (current_branch.back ()->getChildPtr (i));
675 //skip that child if it doesn't exist
676 if (next_branch.back () != nullptr)
677 buildLODRecursive (next_branch);
678
679 next_branch.pop_back ();
680 }
681 }
682 }
683 ////////////////////////////////////////////////////////////////////////////////
684
685 template<typename ContainerT, typename PointT> void
686 OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (std::uint64_t depth, std::uint64_t new_point_count)
687 {
688 if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
689 {
690 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase::incrementPointsInLOD] Overflow error. Too many points in depth %d of outofcore octree with root at %s\n", depth, metadata_->getMetadataFilename().c_str());
691 PCL_THROW_EXCEPTION (PCLException, "Overflow error");
692 }
693
694 metadata_->setLODPoints (depth, new_point_count, true /*true->increment*/);
695 }
696
697 ////////////////////////////////////////////////////////////////////////////////
698
699 template<typename ContainerT, typename PointT> bool
700 OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
701 {
702 if (path_name.extension ().string () != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
703 {
704 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBase] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", path_name.extension ().string ().c_str (), OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension.c_str () );
705 return (false);
706 }
707
708 return (true);
709 }
710
711 ////////////////////////////////////////////////////////////////////////////////
712
713 template<typename ContainerT, typename PointT> void
714 OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
715 {
716 Eigen::Vector3d diff = bb_max - bb_min;
717 assert (diff[0] > 0);
718 assert (diff[1] > 0);
719 assert (diff[2] > 0);
720 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
721
722 double max_sidelength = std::max (std::max (std::abs (diff[0]), std::abs (diff[1])), std::abs (diff[2]));
723 assert (max_sidelength > 0);
724 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
725 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
726 }
727
728 ////////////////////////////////////////////////////////////////////////////////
729
730 template<typename ContainerT, typename PointT> std::uint64_t
731 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
732 {
733 //Assume cube
734 double side_length = max_bb[0] - min_bb[0];
735
736 if (side_length < leaf_resolution)
737 return (0);
738
739 auto res = static_cast<std::uint64_t> (std::ceil (std::log2 (side_length / leaf_resolution)));
740
741 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
742 return (res);
743 }
744 }//namespace outofcore
745}//namespace pcl
746
747#endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
ExtractIndices extracts a set of indices from a point cloud.
shared_ptr< Filter< PointT > > Ptr
Definition filter.h:83
shared_ptr< const Filter< PointT > > ConstPtr
Definition filter.h:84
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
RandomSample applies a random sampling with uniform probability.
This code defines the octree used for point storage at Urban Robotics.
void DeAllocEmptyNodeCache()
Flush empty nodes only.
OutofcoreOctreeBaseMetadata::Ptr metadata_
std::uint64_t addDataToLeaf(const AlignedPointTVector &p)
Recursively add points to the tree.
void incrementPointsInLOD(std::uint64_t depth, std::uint64_t inc)
Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in Outofcore...
void queryFrustum(const double *planes, std::list< std::string > &file_names) const
std::uint64_t addPointCloud_and_genLOD(pcl::PCLPointCloud2::Ptr &input_cloud)
Recursively add points to the tree.
void convertToXYZ()
Save each .bin file as an XYZ file.
OutofcoreOctreeBase(const boost::filesystem::path &root_node_name, const bool load_all)
Load an existing tree.
void getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, std::size_t query_depth) const
Returns the voxel centers of all existing voxels at query_depth.
void writeVPythonVisual(const boost::filesystem::path &filename)
Write a python script using the vpython module containing all the bounding boxes.
void buildLOD()
Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs belo...
void queryBBIncludes(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint64_t query_depth, AlignedPointTVector &dst) const
Get Points in BB, only points inside BB.
OutofcoreOctreeBaseNode< OutofcoreOctreeDiskContainer< pcl::PointXYZ >, pcl::PointXYZRGB > BranchNode
void queryBBIntersects(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const std::uint32_t query_depth, std::list< std::string > &bin_name) const
Get a list of file paths at query_depth that intersect with your bounding box specified by min and ma...
bool getBoundingBox(Eigen::Vector3d &min, Eigen::Vector3d &max) const
Get the overall bounding box of the outofcore octree; this is the same as the bounding box of the roo...
double getVoxelSideLength() const
Gets the smallest (assumed) cubic voxel side lengths.
void flushToDiskLazy()
Flush all non leaf nodes' cache.
virtual void queryBoundingBox(const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent=1.0)
Query all points falling within the input bounding box at query_depth and return a PCLPointCloud2 obj...
bool checkExtension(const boost::filesystem::path &path_name)
Auxiliary function to validate path_name extension is .octree.
std::uint64_t addDataToLeaf_and_genLOD(AlignedPointTVector &p)
Recursively add points to the tree subsampling LODs on the way.
bool getBinDimension(double &x, double &y) const
Computes the expected voxel dimensions at the leaves.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void flushToDisk()
Flush all nodes' cache.
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
void buildLODRecursive(const std::vector< BranchNode * > &current_branch)
recursive portion of lod builder
static const std::string TREE_EXTENSION_
defined as ".octree" to append to treepath files
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
friend class OutofcoreOctreeBaseNode< ContainerT, PointT >
void queryBBIncludes_subsample(const Eigen::Vector3d &min, const Eigen::Vector3d &max, std::uint64_t query_depth, const double percent, AlignedPointTVector &dst) const
Returns a random subsample of points within the given bounding box at query_depth.
std::uint64_t addPointCloud(PointCloudConstPtr point_cloud)
Copies the points from the point_cloud falling within the bounding box of the octree to the out-of-co...
void init(const std::uint64_t &depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_name, const std::string &coord_sys)
void saveToFile()
Write octree definition ".octree" (defined by octree_extension_) to disk.
typename PointCloud::ConstPtr PointCloudConstPtr
void setLODFilter(const pcl::Filter< pcl::PCLPointCloud2 >::Ptr &filter_arg)
Sets the filter to use when building the levels of depth.
OutofcoreNodeType * root_node_
Pointer to the root node of the octree data structure.
pcl::Filter< pcl::PCLPointCloud2 >::Ptr getLODFilter()
Encapsulated class to read JSON metadata into memory, and write the JSON metadata associated with the...
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
virtual void getBoundingBox(Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
gets the minimum and maximum corner of the bounding box represented by this node
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
std::uint64_t size() const
Number of points in the payload.
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
shared_ptr< ::pcl::PCLPointCloud2 > Ptr