Point Cloud Library (PCL) 1.13.0
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 () / (boost::filesystem::basename (root_name) + 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 / (boost::filesystem::basename (root_name) + 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 const 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 const 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 (boost::filesystem::extension (path_name) != 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", boost::filesystem::extension (path_name).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.
Filter represents the base filter class.
Definition: filter.h:81
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
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.
Definition: random_sample.h:56
This code defines the octree used for point storage at Urban Robotics.
Definition: octree_base.h:150
void DeAllocEmptyNodeCache()
Flush empty nodes only.
OutofcoreOctreeBaseMetadata::Ptr metadata_
Definition: octree_base.h:631
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.
Definition: octree_base.hpp:77
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.
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.
Definition: octree_base.h:461
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
Definition: octree_base.h:188
void flushToDisk()
Flush all nodes' cache.
void printBoundingBox() const
Prints size of the bounding boxes to stdou.
Definition: octree_base.h:494
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
Definition: octree_base.h:636
OutofcoreNodeType * getBranchChildPtr(const BranchNode &branch_arg, unsigned char childIdx_arg) const
friend class OutofcoreOctreeBaseNode< ContainerT, PointT >
Definition: octree_base.h:151
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
Definition: octree_base.h:186
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.
Definition: octree_base.h:626
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::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
std::uint64_t size() const
Number of points in the payload.
virtual std::size_t getDepth() const
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