40 #ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
41 #define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
44 #include <pcl/outofcore/octree_base.h>
47 #include <pcl/outofcore/cJSON.h>
49 #include <pcl/filters/random_sample.h>
50 #include <pcl/filters/extract_indices.h>
68 template<
typename ContainerT,
typename Po
intT>
71 template<
typename ContainerT,
typename Po
intT>
76 template<
typename ContainerT,
typename Po
intT>
79 , read_write_mutex_ ()
81 , sample_percent_ (0.125)
85 if (!this->checkExtension (root_name))
87 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
93 root_node_->m_tree_ =
this;
96 boost::filesystem::path treepath = root_name.parent_path () / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
99 metadata_->loadMetadataFromDisk (treepath);
104 template<
typename ContainerT,
typename Po
intT>
107 , read_write_mutex_ ()
109 , sample_percent_ (0.125)
113 Eigen::Vector3d tmp_min = min;
114 Eigen::Vector3d tmp_max = max;
115 this->enlargeToCube (tmp_min, tmp_max);
118 std::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
121 this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
126 template<
typename ContainerT,
typename Po
intT>
129 , read_write_mutex_ ()
131 , sample_percent_ (0.125)
135 this->init (max_depth, min, max, root_node_name, coord_sys);
139 template<
typename ContainerT,
typename Po
intT>
void
143 if (!this->checkExtension (root_name))
145 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
149 if (boost::filesystem::exists (root_name.parent_path ()))
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");
156 boost::filesystem::path dir = root_name.parent_path ();
158 if (!boost::filesystem::exists (dir))
160 boost::filesystem::create_directory (dir);
163 Eigen::Vector3d tmp_min = min;
164 Eigen::Vector3d tmp_max = max;
165 this->enlargeToCube (tmp_min, tmp_max);
169 root_node_->m_tree_ =
this;
172 boost::filesystem::path treepath = dir / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
175 metadata_->setCoordinateSystem (coord_sys);
176 metadata_->setDepth (depth);
177 metadata_->setLODPoints (depth+1);
178 metadata_->setMetadataFilename (treepath);
179 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
183 metadata_->serializeMetadataToDisk ();
188 template<
typename ContainerT,
typename Po
intT>
191 root_node_->flushToDiskRecursive ();
199 template<
typename ContainerT,
typename Po
intT>
void
202 this->metadata_->serializeMetadataToDisk ();
207 template<
typename ContainerT,
typename Po
intT> std::uint64_t
210 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
212 const bool _FORCE_BB_CHECK =
true;
214 std::uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
216 assert (p.size () == pt_added);
223 template<
typename ContainerT,
typename Po
intT> std::uint64_t
226 return (addDataToLeaf (point_cloud->points));
231 template<
typename ContainerT,
typename Po
intT> std::uint64_t
234 std::uint64_t pt_added = this->root_node_->
addPointCloud (input_cloud, skip_bb_check) ;
242 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
253 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
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 );
262 assert ( input_cloud->width*input_cloud->height == pt_added );
269 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
280 template<
typename Container,
typename Po
intT>
void
283 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
284 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
289 template<
typename Container,
typename Po
intT>
void
292 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
293 root_node_->queryFrustum (planes, file_names, query_depth);
298 template<
typename Container,
typename Po
intT>
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
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);
312 template<
typename ContainerT,
typename Po
intT>
void
315 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
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);
323 template<
typename ContainerT,
typename Po
intT>
void
326 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
328 dst_blob->data.clear ();
332 root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
337 template<
typename ContainerT,
typename Po
intT>
void
340 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
342 root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
346 template<
typename ContainerT,
typename Po
intT>
void
355 root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
361 template<
typename ContainerT,
typename Po
intT>
bool
364 if (root_node_!=
nullptr)
374 template<
typename ContainerT,
typename Po
intT>
void
377 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
378 root_node_->printBoundingBox (query_depth);
383 template<
typename ContainerT,
typename Po
intT>
void
386 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
387 if (query_depth > metadata_->getDepth ())
389 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
393 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
399 template<
typename ContainerT,
typename Po
intT>
void
402 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
403 if (query_depth > metadata_->getDepth ())
405 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
409 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
415 template<
typename ContainerT,
typename Po
intT>
void
418 std::shared_lock < std::shared_timed_mutex > lock (read_write_mutex_);
421 #pragma warning(push)
422 #pragma warning(disable : 4267)
424 root_node_->queryBBIntersects (min, max, query_depth, bin_name);
432 template<
typename ContainerT,
typename Po
intT>
void
435 std::ofstream f (filename.c_str ());
437 f <<
"from visual import *\n\n";
439 root_node_->writeVPythonVisual (f);
444 template<
typename ContainerT,
typename Po
intT>
void
452 template<
typename ContainerT,
typename Po
intT>
void
460 template<
typename ContainerT,
typename Po
intT>
void
464 root_node_->convertToXYZ ();
469 template<
typename ContainerT,
typename Po
intT>
void
472 DeAllocEmptyNodeCache (root_node_);
477 template<
typename ContainerT,
typename Po
intT>
void
480 if (current ==
nullptr)
481 current = root_node_;
483 if (current->
size () == 0)
485 current->flush_DeAlloc_this_only ();
488 for (
int i = 0; i < current->numchildren (); i++)
490 DeAllocEmptyNodeCache (current->children[i]);
495 template<
typename ContainerT,
typename Po
intT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
505 return (lod_filter_ptr_);
513 return (lod_filter_ptr_);
518 template<
typename ContainerT,
typename Po
intT>
void
521 lod_filter_ptr_ = std::static_pointer_cast<decltype(lod_filter_ptr_)>(filter_arg);
526 template<
typename ContainerT,
typename Po
intT>
bool
529 if (root_node_==
nullptr)
536 Eigen::Vector3d min, max;
537 this->getBoundingBox (min, max);
539 double depth =
static_cast<double> (metadata_->getDepth ());
540 Eigen::Vector3d diff = max-min;
542 y = diff[1] * pow (.5, depth);
543 x = diff[0] * pow (.5, depth);
550 template<
typename ContainerT,
typename Po
intT>
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));
561 template<
typename ContainerT,
typename Po
intT>
void
564 if (root_node_==
nullptr)
566 PCL_ERROR (
"Root node is null; aborting buildLOD.\n");
570 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
572 const int number_of_nodes = 1;
574 std::vector<BranchNode*> current_branch (number_of_nodes,
static_cast<BranchNode*
>(
nullptr));
575 current_branch[0] = root_node_;
576 assert (current_branch.back () != 0);
577 this->buildLODRecursive (current_branch);
582 template<
typename ContainerT,
typename Po
intT>
void
585 Eigen::Vector3d 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]);
593 template<
typename ContainerT,
typename Po
intT>
void
596 PCL_DEBUG (
"%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
598 if (!current_branch.back ())
605 assert (current_branch.back ()->getDepth () == this->getDepth ());
611 leaf->
read (leaf_input_cloud);
612 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
615 for (std::int64_t level =
static_cast<std::int64_t
>(current_branch.size ()-1); level >= 1; level--)
617 BranchNode* target_parent = current_branch[level-1];
618 assert (target_parent != 0);
619 double exponent =
static_cast<double>(current_branch.size () - target_parent->
getDepth ());
620 double current_depth_sample_percent = pow (sample_percent_, exponent);
622 assert (current_depth_sample_percent > 0.0);
629 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
632 std::uint64_t sample_size =
static_cast<std::uint64_t
> (
static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
634 if (sample_size == 0)
637 lod_filter_ptr_->setSample (
static_cast<unsigned int>(sample_size));
644 lod_filter_ptr_->filter (*downsampled_cloud_indices);
649 extractor.
setIndices (downsampled_cloud_indices);
650 extractor.
filter (*downsampled_cloud);
653 if (downsampled_cloud->width*downsampled_cloud->height > 0)
655 target_parent->
payload_->insertRange (downsampled_cloud);
656 this->incrementPointsInLOD (target_parent->
getDepth (), downsampled_cloud->width*downsampled_cloud->height);
663 current_branch.back ()->clearData ();
665 std::vector<BranchNode*> next_branch (current_branch);
667 if (current_branch.back ()->hasUnloadedChildren ())
669 current_branch.back ()->loadChildren (
false);
672 for (std::size_t i = 0; i < 8; i++)
674 next_branch.push_back (current_branch.back ()->getChildPtr (i));
676 if (next_branch.back () !=
nullptr)
677 buildLODRecursive (next_branch);
679 next_branch.pop_back ();
685 template<
typename ContainerT,
typename Po
intT>
void
688 if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
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());
694 metadata_->setLODPoints (depth, new_point_count,
true );
699 template<
typename ContainerT,
typename Po
intT>
bool
713 template<
typename ContainerT,
typename Po
intT>
void
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;
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);
730 template<
typename ContainerT,
typename Po
intT> std::uint64_t
731 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const double leaf_resolution)
734 double side_length = max_bb[0] - min_bb[0];
736 if (side_length < leaf_resolution)
739 std::uint64_t res =
static_cast<std::uint64_t
> (std::ceil (std::log2 (side_length / leaf_resolution)));
741 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
747 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_