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 ();
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);
226 return (addDataToLeaf (point_cloud->points));
234 std::uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
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);
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 );
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,
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";
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->
size () == 0)
482 current->flush_DeAlloc_this_only ();
485 for (
int i = 0; i < current->numchildren (); i++)
487 DeAllocEmptyNodeCache (current->children[i]);
493 template<
typename ContainerT,
typename Po
intT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
503 return (lod_filter_ptr_);
511 return (lod_filter_ptr_);
516 template<
typename ContainerT,
typename Po
intT>
void
519 lod_filter_ptr_ = filter_arg;
524 template<
typename ContainerT,
typename Po
intT>
bool
527 if (root_node_==
nullptr)
534 Eigen::Vector3d min, max;
535 this->getBoundingBox (min, max);
537 double depth = static_cast<double> (metadata_->getDepth ());
538 Eigen::Vector3d diff = max-min;
540 y = diff[1] * pow (.5, depth);
541 x = diff[0] * pow (.5, depth);
548 template<
typename ContainerT,
typename Po
intT>
double
551 Eigen::Vector3d min, max;
552 this->getBoundingBox (min, max);
553 double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
559 template<
typename ContainerT,
typename Po
intT>
void
562 if (root_node_==
nullptr)
564 PCL_ERROR (
"Root node is null; aborting buildLOD.\n");
568 std::unique_lock < std::shared_timed_mutex > lock (read_write_mutex_);
570 const int number_of_nodes = 1;
572 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(
nullptr));
573 current_branch[0] = root_node_;
574 assert (current_branch.back () != 0);
575 this->buildLODRecursive (current_branch);
580 template<
typename ContainerT,
typename Po
intT>
void
583 Eigen::Vector3d min, max;
585 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]);
591 template<
typename ContainerT,
typename Po
intT>
void
594 PCL_DEBUG (
"%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
596 if (!current_branch.back ())
603 assert (current_branch.back ()->getDepth () == this->getDepth ());
609 leaf->read (leaf_input_cloud);
610 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
613 for (
std::int64_t level = static_cast<std::int64_t>(current_branch.size ()-1); level >= 1; level--)
615 BranchNode* target_parent = current_branch[level-1];
616 assert (target_parent != 0);
617 double exponent = static_cast<double>(current_branch.size () - target_parent->
getDepth ());
618 double current_depth_sample_percent = pow (sample_percent_, exponent);
620 assert (current_depth_sample_percent > 0.0);
627 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
630 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);
632 if (sample_size == 0)
635 lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
642 lod_filter_ptr_->filter (*downsampled_cloud_indices);
647 extractor.
setIndices (downsampled_cloud_indices);
648 extractor.
filter (*downsampled_cloud);
651 if (downsampled_cloud->width*downsampled_cloud->height > 0)
653 target_parent->
payload_->insertRange (downsampled_cloud);
654 this->incrementPointsInLOD (target_parent->
getDepth (), downsampled_cloud->width*downsampled_cloud->height);
661 current_branch.back ()->clearData ();
663 std::vector<BranchNode*> next_branch (current_branch);
665 if (current_branch.back ()->hasUnloadedChildren ())
667 current_branch.back ()->loadChildren (
false);
670 for (std::size_t i = 0; i < 8; i++)
672 next_branch.push_back (current_branch.back ()->getChildPtr (i));
674 if (next_branch.back () !=
nullptr)
675 buildLODRecursive (next_branch);
677 next_branch.pop_back ();
683 template<
typename ContainerT,
typename Po
intT>
void
686 if (std::numeric_limits<std::uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
688 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());
692 metadata_->setLODPoints (depth, new_point_count,
true );
697 template<
typename ContainerT,
typename Po
intT>
bool
711 template<
typename ContainerT,
typename Po
intT>
void
714 Eigen::Vector3d diff = bb_max - bb_min;
715 assert (diff[0] > 0);
716 assert (diff[1] > 0);
717 assert (diff[2] > 0);
718 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
720 double max_sidelength = std::max (std::max (std::abs (diff[0]), std::abs (diff[1])), std::abs (diff[2]));
721 assert (max_sidelength > 0);
722 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
723 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
729 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const double leaf_resolution)
732 double side_length = max_bb[0] - min_bb[0];
734 if (side_length < leaf_resolution)
737 std::uint64_t res = static_cast<std::uint64_t> (std::ceil (std::log2 (side_length / leaf_resolution)));
739 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
745 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_