Point Cloud Library (PCL) 1.15.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/pcl_config.h> // for HAVE_CJSON
48#if defined(HAVE_CJSON)
49#include <cjson/cJSON.h>
50#else
51#include <pcl/outofcore/cJSON.h>
52#endif
53
54#include <pcl/filters/random_sample.h>
55#include <pcl/filters/extract_indices.h>
56
57// C++
58#include <iostream>
59#include <fstream>
60#include <sstream>
61#include <string>
62#include <exception>
63
64namespace pcl
65{
66 namespace outofcore
67 {
68
69 ////////////////////////////////////////////////////////////////////////////////
70 //Static constants
71 ////////////////////////////////////////////////////////////////////////////////
72
73 template<typename ContainerT, typename PointT>
75
76 template<typename ContainerT, typename PointT>
78
79 ////////////////////////////////////////////////////////////////////////////////
80
81 template<typename ContainerT, typename PointT>
82 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::filesystem::path& root_name, const bool load_all)
83 : root_node_ ()
84 , read_write_mutex_ ()
85 , metadata_ (new OutofcoreOctreeBaseMetadata ())
86 , sample_percent_ (0.125)
87 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
88 {
89 //validate the root filename
90 if (!this->checkExtension (root_name))
91 {
92 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
93 }
94
95 // Create root_node_node
96 root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, nullptr, load_all);
97 // Set root_node_nodes tree to the newly created tree
98 root_node_->m_tree_ = this;
99
100 // Set the path to the outofcore octree metadata (unique to the root folder) ending in .octree
101 boost::filesystem::path treepath = root_name.parent_path () / (root_name.stem ().string () + TREE_EXTENSION_);
102
103 //Load the JSON metadata
104 metadata_->loadMetadataFromDisk (treepath);
105 }
106
107 ////////////////////////////////////////////////////////////////////////////////
108
109 template<typename ContainerT, typename PointT>
110 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)
111 : root_node_()
112 , read_write_mutex_ ()
113 , metadata_ (new OutofcoreOctreeBaseMetadata ())
114 , sample_percent_ (0.125)
115 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
116 {
117 //Enlarge the bounding box to a cube so our voxels will be cubes
118 Eigen::Vector3d tmp_min = min;
119 Eigen::Vector3d tmp_max = max;
120 this->enlargeToCube (tmp_min, tmp_max);
121
122 //Compute the depth of the tree given the resolution
123 std::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
124
125 //Create a new outofcore tree
126 this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
127 }
128
129 ////////////////////////////////////////////////////////////////////////////////
130
131 template<typename ContainerT, typename PointT>
132 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)
133 : root_node_()
134 , read_write_mutex_ ()
135 , metadata_ (new OutofcoreOctreeBaseMetadata ())
136 , sample_percent_ (0.125)
137 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
138 {
139 //Create a new outofcore tree
140 this->init (max_depth, min, max, root_node_name, coord_sys);
141 }
142
143 ////////////////////////////////////////////////////////////////////////////////
144 template<typename ContainerT, typename PointT> void
145 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)
146 {
147 //Validate the extension of the pathname
148 if (!this->checkExtension (root_name))
149 {
150 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
151 }
152
153 //Check to make sure that we are not overwriting existing data
154 if (boost::filesystem::exists (root_name.parent_path ()))
155 {
156 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 () );
157 PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
158 }
159
160 // Get fullpath and recreate directories
161 boost::filesystem::path dir = root_name.parent_path ();
162
163 if (!boost::filesystem::exists (dir))
164 {
165 boost::filesystem::create_directory (dir);
166 }
167
168 Eigen::Vector3d tmp_min = min;
169 Eigen::Vector3d tmp_max = max;
170 this->enlargeToCube (tmp_min, tmp_max);
171
172 // Create root node
173 root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name);
174 root_node_->m_tree_ = this;
175
176 // Set root nodes file path
177 boost::filesystem::path treepath = dir / (root_name.stem ().string () + TREE_EXTENSION_);
178
179 //fill the fields of the metadata
180 metadata_->setCoordinateSystem (coord_sys);
181 metadata_->setDepth (depth);
182 metadata_->setLODPoints (depth+1);
183 metadata_->setMetadataFilename (treepath);
184 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
185 //metadata_->setPointType ( <point type string here> );
186
187 //save to disk
188 metadata_->serializeMetadataToDisk ();
189 }
190
191
192 ////////////////////////////////////////////////////////////////////////////////
193 template<typename ContainerT, typename PointT>
195 {
196 root_node_->flushToDiskRecursive ();
197
198 saveToFile ();
199 delete (root_node_);
200 }
201
202 ////////////////////////////////////////////////////////////////////////////////
203
204 template<typename ContainerT, typename PointT> void
206 {
207 this->metadata_->serializeMetadataToDisk ();
208 }
209
210 ////////////////////////////////////////////////////////////////////////////////
211
212 template<typename ContainerT, typename PointT> std::uint64_t
214 {
215 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
216
217 constexpr bool _FORCE_BB_CHECK = true;
218
219 std::uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
220
221 assert (p.size () == pt_added);
222
223 return (pt_added);
224 }
225
226 ////////////////////////////////////////////////////////////////////////////////
227
228 template<typename ContainerT, typename PointT> std::uint64_t
230 {
231 return (addDataToLeaf (point_cloud->points));
232 }
233
234 ////////////////////////////////////////////////////////////////////////////////
235
236 template<typename ContainerT, typename PointT> std::uint64_t
238 {
239 std::uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
240// assert (input_cloud->width*input_cloud->height == pt_added);
241 return (pt_added);
242 }
243
244
245 ////////////////////////////////////////////////////////////////////////////////
246
247 template<typename ContainerT, typename PointT> std::uint64_t
249 {
250 // Lock the tree while writing
251 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
252 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points, false);
253 return (pt_added);
255
256 ////////////////////////////////////////////////////////////////////////////////
257
258 template<typename ContainerT, typename PointT> std::uint64_t
260 {
261 // Lock the tree while writing
262 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
263 std::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
264
265 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
266
267 assert ( input_cloud->width*input_cloud->height == pt_added );
268
269 return (pt_added);
270 }
271
272 ////////////////////////////////////////////////////////////////////////////////
273
274 template<typename ContainerT, typename PointT> std::uint64_t
276 {
277 // Lock the tree while writing
278 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
279 std::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src, false);
280 return (pt_added);
281 }
282
283 ////////////////////////////////////////////////////////////////////////////////
284
285 template<typename Container, typename PointT> void
286 OutofcoreOctreeBase<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) const
287 {
288 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
289 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
290 }
291
292 ////////////////////////////////////////////////////////////////////////////////
293
294 template<typename Container, typename PointT> void
295 OutofcoreOctreeBase<Container, PointT>::queryFrustum(const double *planes, std::list<std::string>& file_names, const std::uint32_t query_depth) const
297 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
298 root_node_->queryFrustum (planes, file_names, query_depth);
299 }
300
301 ////////////////////////////////////////////////////////////////////////////////
302
303 template<typename Container, typename PointT> void
305 const double *planes,
306 const Eigen::Vector3d &eye,
307 const Eigen::Matrix4d &view_projection_matrix,
308 std::list<std::string>& file_names,
309 const std::uint32_t query_depth) const
310 {
311 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
312 root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
313 }
314
315 ////////////////////////////////////////////////////////////////////////////////
316
317 template<typename ContainerT, typename PointT> void
318 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint64_t query_depth, AlignedPointTVector& dst) const
319 {
320 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
321 dst.clear ();
322 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]);
323 root_node_->queryBBIncludes (min, max, query_depth, dst);
324 }
325
326 ////////////////////////////////////////////////////////////////////////////////
327
328 template<typename ContainerT, typename PointT> void
329 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
330 {
331 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
332
333 dst_blob->data.clear ();
334 dst_blob->width = 0;
335 dst_blob->height =1;
336
337 root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
338 }
339
340 ////////////////////////////////////////////////////////////////////////////////
341
342 template<typename ContainerT, typename PointT> void
343 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
344 {
345 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
346 dst.clear ();
347 root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
348 }
349
350 ////////////////////////////////////////////////////////////////////////////////
351 template<typename ContainerT, typename PointT> void
352 OutofcoreOctreeBase<ContainerT, PointT>::queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent)
353 {
354 if (percent==1.0)
355 {
356 root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
357 }
358 else
360 root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
361 }
362 }
363
364 ////////////////////////////////////////////////////////////////////////////////
365
366 template<typename ContainerT, typename PointT> bool
367 OutofcoreOctreeBase<ContainerT, PointT>::getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const
368 {
369 if (root_node_!= nullptr)
370 {
371 root_node_->getBoundingBox (min, max);
372 return true;
373 }
374 return false;
375 }
376
377 ////////////////////////////////////////////////////////////////////////////////
378
379 template<typename ContainerT, typename PointT> void
381 {
382 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
383 root_node_->printBoundingBox (query_depth);
384 }
385
386 ////////////////////////////////////////////////////////////////////////////////
387
388 template<typename ContainerT, typename PointT> void
390 {
391 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
392 if (query_depth > metadata_->getDepth ())
393 {
394 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
395 }
396 else
397 {
398 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
400 }
401
402 ////////////////////////////////////////////////////////////////////////////////
403
404 template<typename ContainerT, typename PointT> void
405 OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth) const
406 {
407 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
408 if (query_depth > metadata_->getDepth ())
409 {
410 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
411 }
412 else
413 {
414 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
415 }
416 }
417
418 ////////////////////////////////////////////////////////////////////////////////
419
420 template<typename ContainerT, typename PointT> void
421 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
422 {
423 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
424 bin_name.clear ();
425#if defined _MSC_VER
426 #pragma warning(push)
427 #pragma warning(disable : 4267)
428#endif
429 root_node_->queryBBIntersects (min, max, query_depth, bin_name);
430#if defined _MSC_VER
431 #pragma warning(pop)
432#endif
433 }
434
435 ////////////////////////////////////////////////////////////////////////////////
436
437 template<typename ContainerT, typename PointT> void
438 OutofcoreOctreeBase<ContainerT, PointT>::writeVPythonVisual (const boost::filesystem::path& filename)
439 {
440 std::ofstream f (filename.c_str ());
441
442 f << "from visual import *\n\n";
443
444 root_node_->writeVPythonVisual (f);
445 }
446
447 ////////////////////////////////////////////////////////////////////////////////
448
449 template<typename ContainerT, typename PointT> void
451 {
452 root_node_->flushToDisk ();
453 }
454
455 ////////////////////////////////////////////////////////////////////////////////
457 template<typename ContainerT, typename PointT> void
459 {
460 root_node_->flushToDiskLazy ();
461 }
462
463 ////////////////////////////////////////////////////////////////////////////////
464
465 template<typename ContainerT, typename PointT> void
467 {
468 saveToFile ();
469 root_node_->convertToXYZ ();
470 }
471
472 ////////////////////////////////////////////////////////////////////////////////
473
474 template<typename ContainerT, typename PointT> void
476 {
477 DeAllocEmptyNodeCache (root_node_);
478 }
479
480 ////////////////////////////////////////////////////////////////////////////////
482 template<typename ContainerT, typename PointT> void
484 {
485 if (current == nullptr)
486 current = root_node_;
487
488 if (current->size () == 0)
489 {
490 current->flush_DeAlloc_this_only ();
491 }
492
493 for (int i = 0; i < current->numchildren (); i++)
494 {
495 DeAllocEmptyNodeCache (current->children[i]);
496 }
497 }
498
499 ////////////////////////////////////////////////////////////////////////////////
500 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
501 OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
502 {
503 return (branch_arg.getChildPtr (childIdx_arg));
504 }
506 ////////////////////////////////////////////////////////////////////////////////
507 template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
509 {
510 return (lod_filter_ptr_);
511 }
513 ////////////////////////////////////////////////////////////////////////////////
514
515 template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
517 {
518 return (lod_filter_ptr_);
519 }
520
521 ////////////////////////////////////////////////////////////////////////////////
522
523 template<typename ContainerT, typename PointT> void
525 {
526 lod_filter_ptr_ = std::static_pointer_cast<decltype(lod_filter_ptr_)>(filter_arg);
527 }
528
529 ////////////////////////////////////////////////////////////////////////////////
530
531 template<typename ContainerT, typename PointT> bool
533 {
534 if (root_node_== nullptr)
536 x = 0;
537 y = 0;
538 return (false);
539 }
541 Eigen::Vector3d min, max;
542 this->getBoundingBox (min, max);
544 double depth = static_cast<double> (metadata_->getDepth ());
545 Eigen::Vector3d diff = max-min;
547 y = diff[1] * pow (.5, depth);
548 x = diff[0] * pow (.5, depth);
550 return (true);
551 }
552
553 ////////////////////////////////////////////////////////////////////////////////
554
555 template<typename ContainerT, typename PointT> double
557 {
558 Eigen::Vector3d min, max;
559 this->getBoundingBox (min, max);
560 double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
561 return (result);
562 }
563
564 ////////////////////////////////////////////////////////////////////////////////
565
566 template<typename ContainerT, typename PointT> void
568 {
569 if (root_node_== nullptr)
570 {
571 PCL_ERROR ("Root node is null; aborting buildLOD.\n");
572 return;
573 }
574
575 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
576
577 constexpr int number_of_nodes = 1;
578
579 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(nullptr));
580 current_branch[0] = root_node_;
581 assert (current_branch.back () != nullptr);
582 this->buildLODRecursive (current_branch);
583 }
584
585 ////////////////////////////////////////////////////////////////////////////////
586
587 template<typename ContainerT, typename PointT> void
588 OutofcoreOctreeBase<ContainerT, PointT>::printBoundingBox (OutofcoreOctreeBaseNode<ContainerT, PointT>& node) const
589 {
590 Eigen::Vector3d min, max;
591 node.getBoundingBox (min,max);
592 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]);
593 }
594
595
596 ////////////////////////////////////////////////////////////////////////////////
597
598 template<typename ContainerT, typename PointT> void
599 OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
601 PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
602
603 if (!current_branch.back ())
604 {
605 return;
606 }
607
608 if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
609 {
610 assert (current_branch.back ()->getDepth () == this->getDepth ());
611
612 BranchNode* leaf = current_branch.back ();
613
614 pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
615 //read the data from the PCD file associated with the leaf; it is full resolution
616 leaf->read (leaf_input_cloud);
617 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
618
619 //go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution
620 for (auto level = static_cast<std::int64_t>(current_branch.size ()-1); level >= 1; level--)
621 {
622 BranchNode* target_parent = current_branch[level-1];
623 assert (target_parent != nullptr);
624 double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
625 double current_depth_sample_percent = pow (sample_percent_, exponent);
626
627 assert (current_depth_sample_percent > 0.0);
628 //------------------------------------------------------------
629 //subsample data:
630 // 1. Get indices from a random sample
631 // 2. Extract those indices with the extract indices class (in order to also get the complement)
632 //------------------------------------------------------------
633
634 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
635
636 //set sample size to 1/8 of total points (12.5%)
637 auto sample_size = static_cast<std::uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
638
639 if (sample_size == 0)
640 sample_size = 1;
641
642 lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
643
644 //create our destination
645 pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
646
647 //create destination for indices
648 pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ());
649 lod_filter_ptr_->filter (*downsampled_cloud_indices);
650
651 //extract the "random subset", size by setSampleSize
653 extractor.setInputCloud (leaf_input_cloud);
654 extractor.setIndices (downsampled_cloud_indices);
655 extractor.filter (*downsampled_cloud);
656
657 //write to the target
658 if (downsampled_cloud->width*downsampled_cloud->height > 0)
659 {
660 target_parent->payload_->insertRange (downsampled_cloud);
661 this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
662 }
663 }
664 }
665 else//not at leaf, keep going down
666 {
667 //clear this node while walking down the tree in case we are updating the LOD
668 current_branch.back ()->clearData ();
669
670 std::vector<BranchNode*> next_branch (current_branch);
671
672 if (current_branch.back ()->hasUnloadedChildren ())
673 {
674 current_branch.back ()->loadChildren (false);
675 }
676
677 for (std::size_t i = 0; i < 8; i++)
678 {
679 next_branch.push_back (current_branch.back ()->getChildPtr (i));
680 //skip that child if it doesn't exist
681 if (next_branch.back () != nullptr)
682 buildLODRecursive (next_branch);
683
684 next_branch.pop_back ();
685 }
686 }
687 }
688 ////////////////////////////////////////////////////////////////////////////////
689
690 template<typename ContainerT, typename PointT> void
691 OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (std::uint64_t depth, std::uint64_t new_point_count)
692 {
693 if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
694 {
695 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());
696 PCL_THROW_EXCEPTION (PCLException, "Overflow error");
697 }
698
699 metadata_->setLODPoints (depth, new_point_count, true /*true->increment*/);
700 }
701
702 ////////////////////////////////////////////////////////////////////////////////
703
704 template<typename ContainerT, typename PointT> bool
705 OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
706 {
707 if (path_name.extension ().string () != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
708 {
709 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 () );
710 return (false);
711 }
712
713 return (true);
714 }
715
716 ////////////////////////////////////////////////////////////////////////////////
717
718 template<typename ContainerT, typename PointT> void
719 OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
720 {
721 Eigen::Vector3d diff = bb_max - bb_min;
722 assert (diff[0] > 0);
723 assert (diff[1] > 0);
724 assert (diff[2] > 0);
725 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
726
727 double max_sidelength = std::max (std::max (std::abs (diff[0]), std::abs (diff[1])), std::abs (diff[2]));
728 assert (max_sidelength > 0);
729 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
730 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
731 }
732
733 ////////////////////////////////////////////////////////////////////////////////
734
735 template<typename ContainerT, typename PointT> std::uint64_t
736 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
737 {
738 //Assume cube
739 double side_length = max_bb[0] - min_bb[0];
740
741 if (side_length < leaf_resolution)
742 return (0);
743
744 auto res = static_cast<std::uint64_t> (std::ceil (std::log2 (side_length / leaf_resolution)));
745
746 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
747 return (res);
748 }
749 }//namespace outofcore
750}//namespace pcl
751
752#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:67
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,...
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