41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
53 #include <pcl/common/utils.h>
54 #include <pcl/visualization/common/common.h>
55 #include <pcl/outofcore/octree_base_node.h>
56 #include <pcl/filters/random_sample.h>
57 #include <pcl/filters/extract_indices.h>
60 #include <pcl/outofcore/cJSON.h>
67 template<
typename ContainerT,
typename Po
intT>
70 template<
typename ContainerT,
typename Po
intT>
73 template<
typename ContainerT,
typename Po
intT>
76 template<
typename ContainerT,
typename Po
intT>
79 template<
typename ContainerT,
typename Po
intT>
82 template<
typename ContainerT,
typename Po
intT>
85 template<
typename ContainerT,
typename Po
intT>
88 template<
typename ContainerT,
typename Po
intT>
91 template<
typename ContainerT,
typename Po
intT>
97 , children_ (8, nullptr)
99 , num_loaded_children_ (0)
108 template<
typename ContainerT,
typename Po
intT>
114 , children_ (8, nullptr)
116 , num_loaded_children_ (0)
123 if (super ==
nullptr)
125 node_metadata_->setDirectoryPathname (directory_path.parent_path ());
131 if (!boost::filesystem::exists (
node_metadata_->getDirectoryPathname ()))
133 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n",
node_metadata_->getDirectoryPathname ().c_str ());
134 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
143 boost::filesystem::directory_iterator directory_it_end;
146 bool b_loaded =
false;
148 for (boost::filesystem::directory_iterator directory_it (
node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
150 const boost::filesystem::path& file = *directory_it;
152 if (!boost::filesystem::is_directory (file))
164 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
165 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
182 template<
typename ContainerT,
typename Po
intT>
188 , children_ (8, nullptr)
190 , num_loaded_children_ (0)
194 assert (tree != NULL);
201 template<
typename ContainerT,
typename Po
intT>
void
204 assert (tree != NULL);
214 Eigen::Vector3d tmp_max = bb_max;
215 Eigen::Vector3d tmp_min = bb_min;
218 double epsilon = 1e-8;
219 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
221 node_metadata_->setBoundingBox (tmp_min, tmp_max);
222 node_metadata_->setDirectoryPathname (root_name.parent_path ());
223 node_metadata_->setOutofcoreVersion (3);
226 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
228 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
231 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
233 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
234 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
242 std::string node_container_name;
244 node_container_name = uuid + std::string (
"_") + node_container_basename + pcd_extension;
246 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
247 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
249 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
250 node_metadata_->serializeMetadataToDisk ();
253 payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
258 template<
typename ContainerT,
typename Po
intT>
267 template<
typename ContainerT,
typename Po
intT> std::size_t
270 std::size_t child_count = 0;
272 for(std::size_t i=0; i<8; i++)
274 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
275 if (boost::filesystem::exists (child_path))
278 return (child_count);
283 template<
typename ContainerT,
typename Po
intT>
void
286 node_metadata_->serializeMetadataToDisk ();
290 for (std::size_t i = 0; i < 8; i++)
293 children_[i]->saveIdx (
true);
300 template<
typename ContainerT,
typename Po
intT>
bool
303 return (this->getNumLoadedChildren () < this->getNumChildren ());
307 template<
typename ContainerT,
typename Po
intT>
void
311 if (num_loaded_children_ < this->getNumChildren ())
314 for (
int i = 0; i < 8; i++)
316 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
318 if (boost::filesystem::exists (child_dir) && this->children_[i] ==
nullptr)
323 num_loaded_children_++;
327 assert (num_loaded_children_ == this->getNumChildren ());
331 template<
typename ContainerT,
typename Po
intT>
void
334 if (num_children_ == 0)
339 for (std::size_t i = 0; i < 8; i++)
348 template<
typename ContainerT,
typename Po
intT> std::uint64_t
358 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
359 return (addDataAtMaxDepth( p, skip_bb_check));
361 if (hasUnloadedChildren ())
362 loadChildren (
false);
364 std::vector < std::vector<const PointT*> > c;
366 for (std::size_t i = 0; i < 8; i++)
368 c[i].reserve (p.size () / 8);
371 const std::size_t len = p.size ();
372 for (std::size_t i = 0; i < len; i++)
380 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
385 std::uint8_t box = 0;
386 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
388 box =
static_cast<std::uint8_t
>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
389 c[
static_cast<std::size_t
>(box)].push_back (&pt);
392 std::uint64_t points_added = 0;
393 for (std::size_t i = 0; i < 8; i++)
399 points_added += children_[i]->addDataToLeaf (c[i],
true);
402 return (points_added);
407 template<
typename ContainerT,
typename Po
intT> std::uint64_t
415 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
420 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
422 payload_->insertRange (p.data (), p.size ());
427 std::vector<const PointT*> buff;
428 for (
const PointT* pt : p)
438 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
439 payload_->insertRange (buff.data (), buff.size ());
443 return (buff.size ());
446 if (this->hasUnloadedChildren ())
448 loadChildren (
false);
451 std::vector < std::vector<const PointT*> > c;
453 for (std::size_t i = 0; i < 8; i++)
455 c[i].reserve (p.size () / 8);
458 const std::size_t len = p.size ();
459 for (std::size_t i = 0; i < len; i++)
471 std::uint8_t box = 00;
472 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
474 box =
static_cast<std::uint8_t
> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] )));
476 c[box].push_back (p[i]);
479 std::uint64_t points_added = 0;
480 for (std::size_t i = 0; i < 8; i++)
486 points_added += children_[i]->addDataToLeaf (c[i],
true);
489 return (points_added);
494 template<
typename ContainerT,
typename Po
intT> std::uint64_t
497 assert (this->root_node_->m_tree_ != NULL);
499 if (input_cloud->height*input_cloud->width == 0)
502 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
503 return (addDataAtMaxDepth (input_cloud,
true));
505 if( num_children_ < 8 )
506 if(hasUnloadedChildren ())
507 loadChildren (
false);
514 std::vector < std::vector<int> > indices;
517 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
519 for(std::size_t k=0; k<indices.size (); k++)
521 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
524 std::uint64_t points_added = 0;
526 for(std::size_t i=0; i<8; i++)
528 if ( indices[i].empty () )
531 if (children_[i] ==
nullptr)
538 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
544 points_added += children_[i]->addPointCloud (dst_cloud,
false);
548 return (points_added);
551 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
558 template<
typename ContainerT,
typename Po
intT>
void
561 assert (this->root_node_->m_tree_ != NULL);
570 sampleBuff.push_back(pt);
580 const double percent = pow(sample_percent_,
double((this->root_node_->m_tree_->getDepth () - depth_)));
581 const std::uint64_t samplesize =
static_cast<std::uint64_t
>(percent *
static_cast<double>(sampleBuff.size()));
582 const std::uint64_t inputsize = sampleBuff.size();
587 insertBuff.resize(samplesize);
590 std::lock_guard<std::mutex> lock(rng_mutex_);
591 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
594 for(std::uint64_t i = 0; i < samplesize; ++i)
596 std::uint64_t buffstart = buffdist(rng_);
597 insertBuff[i] = ( sampleBuff[buffstart] );
603 std::lock_guard<std::mutex> lock(rng_mutex_);
604 std::bernoulli_distribution buffdist(percent);
606 for(std::uint64_t i = 0; i < inputsize; ++i)
608 insertBuff.push_back( p[i] );
613 template<
typename ContainerT,
typename Po
intT> std::uint64_t
616 assert (this->root_node_->m_tree_ != NULL);
622 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
625 payload_->insertRange ( p );
631 AlignedPointTVector buff;
632 const std::size_t len = p.size ();
634 for (std::size_t i = 0; i < len; i++)
638 buff.push_back (p[i]);
644 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
645 payload_->insertRange ( buff );
647 return (buff.size ());
650 template<
typename ContainerT,
typename Po
intT> std::uint64_t
656 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
658 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
659 payload_->insertRange (input_cloud);
661 return (input_cloud->width*input_cloud->height);
663 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
669 template<
typename ContainerT,
typename Po
intT>
void
674 for(std::size_t i = 0; i < 8; i++)
675 c[i].reserve(p.size() / 8);
677 const std::size_t len = p.size();
678 for(std::size_t i = 0; i < len; i++)
686 subdividePoint (pt, c);
691 template<
typename ContainerT,
typename Po
intT>
void
694 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
695 std::size_t octant = 0;
696 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
697 c[octant].push_back (point);
701 template<
typename ContainerT,
typename Po
intT> std::uint64_t
704 std::uint64_t points_added = 0;
706 if ( input_cloud->width * input_cloud->height == 0 )
711 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
713 std::uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
714 assert (points_added > 0);
715 return (points_added);
718 if (num_children_ < 8 )
720 if ( hasUnloadedChildren () )
722 loadChildren (
false);
735 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
736 random_sampler.
setSample (
static_cast<unsigned int> (sample_size));
742 pcl::IndicesPtr downsampled_cloud_indices (
new std::vector< int > () );
743 random_sampler.
filter (*downsampled_cloud_indices);
748 extractor.
setIndices (downsampled_cloud_indices);
749 extractor.
filter (*downsampled_cloud);
754 extractor.
filter (*remaining_points);
756 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Random sampled: %lu of %lu\n", __FUNCTION__, downsampled_cloud->width * downsampled_cloud->height, input_cloud->width * input_cloud->height );
759 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
761 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
762 payload_->insertRange (downsampled_cloud);
763 points_added += downsampled_cloud->width*downsampled_cloud->height ;
766 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
769 std::vector<std::vector<int> > indices;
772 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
775 for(std::size_t i=0; i<8; i++)
778 if(indices[i].empty ())
781 if (children_[i] ==
nullptr)
792 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
793 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] points_added: %lu, indices[i].size: %lu, tmp_local_point_cloud size: %lu\n", __FUNCTION__, points_added, indices[i].size (), tmp_local_point_cloud->width*tmp_local_point_cloud->height);
796 assert (points_added == input_cloud->width*input_cloud->height);
797 return (points_added);
801 template<
typename ContainerT,
typename Po
intT> std::uint64_t
810 assert (this->root_node_->m_tree_ != NULL );
812 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
814 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
815 return (addDataAtMaxDepth(p,
false));
819 if (this->hasUnloadedChildren ())
820 loadChildren (
false );
824 randomSample(p, insertBuff, skip_bb_check);
826 if(!insertBuff.empty())
829 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
831 payload_->insertRange (insertBuff);
836 std::vector<AlignedPointTVector> c;
837 subdividePoints(p, c, skip_bb_check);
839 std::uint64_t points_added = 0;
840 for(std::size_t i = 0; i < 8; i++)
851 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
855 return (points_added);
859 template<
typename ContainerT,
typename Po
intT>
void
865 if (children_[idx] || (num_children_ == 8))
867 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
871 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
872 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
874 Eigen::Vector3d childbb_min;
875 Eigen::Vector3d childbb_max;
880 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
881 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
886 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
887 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
891 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
892 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
894 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
895 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
897 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
898 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
900 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
907 template<
typename ContainerT,
typename Po
intT>
bool
908 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
910 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
911 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
912 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
922 template<
typename ContainerT,
typename Po
intT>
bool
925 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
926 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
928 if (((min[0] <= p.x) && (p.x < max[0])) &&
929 ((min[1] <= p.y) && (p.y < max[1])) &&
930 ((min[2] <= p.z) && (p.z < max[2])))
939 template<
typename ContainerT,
typename Po
intT>
void
944 node_metadata_->getBoundingBox (min, max);
946 if (this->depth_ < query_depth){
947 for (std::size_t i = 0; i < this->depth_; i++)
950 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
951 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
952 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
953 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
955 if (num_children_ > 0)
957 for (std::size_t i = 0; i < 8; i++)
960 children_[i]->printBoundingBox (query_depth);
967 template<
typename ContainerT,
typename Po
intT>
void
970 if (this->depth_ < query_depth){
971 if (num_children_ > 0)
973 for (std::size_t i = 0; i < 8; i++)
976 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
983 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
984 voxel_center.x =
static_cast<float>(mid_xyz[0]);
985 voxel_center.y =
static_cast<float>(mid_xyz[1]);
986 voxel_center.z =
static_cast<float>(mid_xyz[2]);
988 voxel_centers.push_back(voxel_center);
1044 template<
typename Container,
typename Po
intT>
void
1047 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1050 template<
typename Container,
typename Po
intT>
void
1054 enum {INSIDE, INTERSECT, OUTSIDE};
1056 int result = INSIDE;
1058 if (this->depth_ > query_depth)
1066 if (!skip_vfc_check)
1068 for(
int i =0; i < 6; i++){
1069 double a = planes[(i*4)];
1070 double b = planes[(i*4)+1];
1071 double c = planes[(i*4)+2];
1072 double d = planes[(i*4)+3];
1076 Eigen::Vector3d normal(a, b, c);
1078 Eigen::Vector3d min_bb;
1079 Eigen::Vector3d max_bb;
1080 node_metadata_->getBoundingBox(min_bb, max_bb);
1083 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1084 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1085 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1086 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1088 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1089 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1096 if (m - n < 0) result = INTERSECT;
1147 if (result == OUTSIDE)
1165 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1168 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1171 if (hasUnloadedChildren ())
1173 loadChildren (
false);
1176 if (this->getNumChildren () > 0)
1178 for (std::size_t i = 0; i < 8; i++)
1181 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1199 template<
typename Container,
typename Po
intT>
void
1204 if (this->depth_ > query_depth)
1210 Eigen::Vector3d min_bb;
1211 Eigen::Vector3d max_bb;
1212 node_metadata_->getBoundingBox(min_bb, max_bb);
1215 enum {INSIDE, INTERSECT, OUTSIDE};
1217 int result = INSIDE;
1219 if (!skip_vfc_check)
1221 for(
int i =0; i < 6; i++){
1222 double a = planes[(i*4)];
1223 double b = planes[(i*4)+1];
1224 double c = planes[(i*4)+2];
1225 double d = planes[(i*4)+3];
1229 Eigen::Vector3d normal(a, b, c);
1232 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1233 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1234 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1235 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1237 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1238 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1245 if (m - n < 0) result = INTERSECT;
1250 if (result == OUTSIDE)
1285 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1288 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1292 if (coverage <= 10000)
1295 if (hasUnloadedChildren ())
1297 loadChildren (
false);
1300 if (this->getNumChildren () > 0)
1302 for (std::size_t i = 0; i < 8; i++)
1305 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1311 template<
typename ContainerT,
typename Po
intT>
void
1314 if (this->depth_ < query_depth){
1315 if (num_children_ > 0)
1317 for (std::size_t i = 0; i < 8; i++)
1320 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1326 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1327 voxel_centers.push_back(voxel_center);
1334 template<
typename ContainerT,
typename Po
intT>
void
1338 Eigen::Vector3d my_min = min_bb;
1339 Eigen::Vector3d my_max = max_bb;
1341 if (intersectsWithBoundingBox (my_min, my_max))
1343 if (this->depth_ < query_depth)
1345 if (this->getNumChildren () > 0)
1347 for (std::size_t i = 0; i < 8; i++)
1350 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1353 else if (hasUnloadedChildren ())
1355 loadChildren (
false);
1357 for (std::size_t i = 0; i < 8; i++)
1360 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1366 if (payload_->getDataSize () > 0)
1368 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1374 template<
typename ContainerT,
typename Po
intT>
void
1377 std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1378 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1381 if (intersectsWithBoundingBox (min_bb, max_bb))
1384 if (this->depth_ < query_depth)
1387 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1388 loadChildren (
false);
1391 if (num_children_ > 0)
1394 for (std::size_t i = 0; i < 8; i++)
1397 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1399 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1409 payload_->readRange (0, payload_->size (), tmp_blob);
1411 if( tmp_blob->width*tmp_blob->height == 0 )
1415 if (inBoundingBox (min_bb, max_bb))
1421 if( dst_blob->width*dst_blob->height != 0 )
1423 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1424 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1429 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1434 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1436 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1438 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1449 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1451 Eigen::Vector4f min_pt (
static_cast<float> ( min_bb[0] ),
static_cast<float> ( min_bb[1] ),
static_cast<float> ( min_bb[2] ), 1.0f);
1452 Eigen::Vector4f max_pt (
static_cast<float> ( max_bb[0] ),
static_cast<float> ( max_bb[1] ) ,
static_cast<float>( max_bb[2] ), 1.0f );
1454 std::vector<int> indices;
1457 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1458 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1460 if ( !indices.empty () )
1462 if( dst_blob->width*dst_blob->height > 0 )
1469 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1470 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1472 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1473 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1477 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1483 assert ( dst_blob->width*dst_blob->height == indices.size () );
1489 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1492 template<
typename ContainerT,
typename Po
intT>
void
1497 if (intersectsWithBoundingBox (min_bb, max_bb))
1500 if (this->depth_ < query_depth)
1503 if (this->hasUnloadedChildren ())
1505 this->loadChildren (
false);
1509 if (getNumChildren () > 0)
1511 if(hasUnloadedChildren ())
1512 loadChildren (
false);
1515 for (std::size_t i = 0; i < 8; i++)
1518 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1527 if (inBoundingBox (min_bb, max_bb))
1530 AlignedPointTVector payload_cache;
1531 payload_->readRange (0, payload_->size (), payload_cache);
1532 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1539 AlignedPointTVector payload_cache;
1540 payload_->readRange (0, payload_->size (), payload_cache);
1542 std::uint64_t len = payload_->size ();
1544 for (std::uint64_t i = 0; i < len; i++)
1546 const PointT& p = payload_cache[i];
1555 PCL_DEBUG (
"[pcl::outofcore::queryBBIncludes] Point %.2lf %.2lf %.2lf not in bounding box %.2lf %.2lf %.2lf", p.x, p.y, p.z, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2]);
1563 template<
typename ContainerT,
typename Po
intT>
void
1566 if (intersectsWithBoundingBox (min_bb, max_bb))
1568 if (this->depth_ < query_depth)
1570 if (this->hasUnloadedChildren ())
1571 this->loadChildren (
false);
1573 if (this->getNumChildren () > 0)
1575 for (std::size_t i=0; i<8; i++)
1578 if (children_[i]!=
nullptr)
1579 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1588 if (inBoundingBox (min_bb, max_bb))
1591 this->payload_->read (tmp_blob);
1592 std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1594 double sample_points =
static_cast<double>(num_pts) * percent;
1598 sample_points = sample_points > 1 ? sample_points : 1;
1612 random_sampler.
setSample (
static_cast<unsigned int> (sample_points));
1618 random_sampler.
filter (*downsampled_cloud_indices);
1619 extractor.
setIndices (downsampled_cloud_indices);
1620 extractor.
filter (*downsampled_points);
1631 template<
typename ContainerT,
typename Po
intT>
void
1635 if (intersectsWithBoundingBox (min_bb, max_bb))
1638 if (this->depth_ < query_depth)
1641 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1643 loadChildren (
false);
1646 if (num_children_ > 0)
1649 for (std::size_t i = 0; i < 8; i++)
1652 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1661 if (inBoundingBox (min_bb, max_bb))
1664 AlignedPointTVector payload_cache;
1665 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1666 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1671 AlignedPointTVector payload_cache_within_region;
1673 AlignedPointTVector payload_cache;
1674 payload_->readRange (0, payload_->size (), payload_cache);
1675 for (std::size_t i = 0; i < payload_->size (); i++)
1677 const PointT& p = payload_cache[i];
1680 payload_cache_within_region.push_back (p);
1686 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1687 std::size_t numpick =
static_cast<std::size_t
> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1689 for (std::size_t i = 0; i < numpick; i++)
1691 dst.push_back (payload_cache_within_region[i]);
1699 template<
typename ContainerT,
typename Po
intT>
1705 , children_ (8, nullptr)
1707 , num_loaded_children_ (0)
1713 if (super ==
nullptr)
1715 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1716 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1729 std::string uuid_idx;
1730 std::string uuid_cont;
1736 std::string node_container_name;
1739 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1743 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1751 template<
typename ContainerT,
typename Po
intT>
void
1754 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1756 loadChildren (
false);
1759 for (std::size_t i = 0; i < num_children_; i++)
1761 children_[i]->copyAllCurrentAndChildPointsRec (v);
1765 payload_->readRange (0, payload_->size (), payload_cache);
1768 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1774 template<
typename ContainerT,
typename Po
intT>
void
1777 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1779 loadChildren (
false);
1782 for (std::size_t i = 0; i < 8; i++)
1785 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1788 std::vector<PointT> payload_cache;
1789 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1791 for (std::size_t i = 0; i < payload_cache.size (); i++)
1793 v.push_back (payload_cache[i]);
1799 template<
typename ContainerT,
typename Po
intT>
inline bool
1802 Eigen::Vector3d min, max;
1803 node_metadata_->getBoundingBox (min, max);
1806 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1808 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1810 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1821 template<
typename ContainerT,
typename Po
intT>
inline bool
1824 Eigen::Vector3d min, max;
1826 node_metadata_->getBoundingBox (min, max);
1828 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1830 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1832 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1843 template<
typename ContainerT,
typename Po
intT>
inline bool
1848 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1850 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1852 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1863 template<
typename ContainerT,
typename Po
intT>
void
1866 Eigen::Vector3d min;
1867 Eigen::Vector3d max;
1868 node_metadata_->getBoundingBox (min, max);
1870 double l = max[0] - min[0];
1871 double h = max[1] - min[1];
1872 double w = max[2] - min[2];
1873 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1874 <<
", width=" << w <<
" )\n";
1876 for (std::size_t i = 0; i < num_children_; i++)
1878 children_[i]->writeVPythonVisual (file);
1884 template<
typename ContainerT,
typename Po
intT>
int
1887 return (this->payload_->read (output_cloud));
1895 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1896 return (children_[index_arg]);
1900 template<
typename ContainerT,
typename Po
intT> std::uint64_t
1903 return (this->payload_->getDataSize ());
1908 template<
typename ContainerT,
typename Po
intT> std::size_t
1911 std::size_t loaded_children_count = 0;
1913 for (std::size_t i=0; i<8; i++)
1915 if (children_[i] !=
nullptr)
1916 loaded_children_count++;
1919 return (loaded_children_count);
1924 template<
typename ContainerT,
typename Po
intT>
void
1927 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1928 node_metadata_->loadMetadataFromDisk (path);
1931 this->parent_ = super;
1933 if (num_children_ > 0)
1936 this->num_children_ = 0;
1937 this->payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
1942 template<
typename ContainerT,
typename Po
intT>
void
1945 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1946 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1947 payload_->convertToXYZ (xyzfile);
1949 if (hasUnloadedChildren ())
1951 loadChildren (
false);
1954 for (std::size_t i = 0; i < 8; i++)
1957 children_[i]->convertToXYZ ();
1963 template<
typename ContainerT,
typename Po
intT>
void
1966 for (std::size_t i = 0; i < 8; i++)
1969 children_[i]->flushToDiskRecursive ();
1975 template<
typename ContainerT,
typename Po
intT>
void
1978 if (indices.size () < 8)
1985 int x_offset = input_cloud->fields[x_idx].offset;
1986 int y_offset = input_cloud->fields[y_idx].offset;
1987 int z_offset = input_cloud->fields[z_idx].offset;
1989 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1993 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
1994 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
1995 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
1997 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
2002 PCL_ERROR (
"pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box\n", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
2008 std::size_t box = 0;
2009 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2013 indices[box].push_back (
static_cast<int> (point_idx/input_cloud->point_step));
2018 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2027 thisnode->thisdir_ = path.parent_path ();
2029 if (!boost::filesystem::exists (thisnode->thisdir_))
2031 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2032 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2035 thisnode->thisnodeindex_ = path;
2042 thisnode->thisdir_ = path;
2046 if (thisnode->
depth_ > thisnode->root->max_depth_)
2048 thisnode->root->max_depth_ = thisnode->
depth_;
2051 boost::filesystem::directory_iterator diterend;
2052 bool loaded =
false;
2053 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2055 const boost::filesystem::path& file = *diter;
2056 if (!boost::filesystem::is_directory (file))
2060 thisnode->thisnodeindex_ = file;
2069 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2070 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2074 thisnode->max_depth_ = 0;
2077 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2079 f >> thisnode->min_[0];
2080 f >> thisnode->min_[1];
2081 f >> thisnode->min_[2];
2082 f >> thisnode->max_[0];
2083 f >> thisnode->max_[1];
2084 f >> thisnode->max_[2];
2086 std::string filename;
2088 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2092 thisnode->
payload_.reset (
new ContainerT (thisnode->thisnodestorage_));
2097 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2106 template<
typename ContainerT,
typename Po
intT>
void
2107 queryBBIntersects_noload (
const boost::filesystem::path& root_node,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const std::uint32_t query_depth, std::list<std::string>& bin_name)
2109 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2112 std::cout <<
"test";
2114 if (root->intersectsWithBoundingBox (min, max))
2116 if (query_depth == root->max_depth_)
2118 if (!root->payload_->empty ())
2120 bin_name.push_back (root->thisnodestorage_.string ());
2125 for (
int i = 0; i < 8; i++)
2127 boost::filesystem::path child_dir = root->thisdir_
2128 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2129 if (boost::filesystem::exists (child_dir))
2132 root->num_children_++;
2142 template<
typename ContainerT,
typename Po
intT>
void
2143 queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const std::uint32_t query_depth, std::list<std::string>& bin_name)
2145 if (current->intersectsWithBoundingBox (min, max))
2147 if (current->depth_ == query_depth)
2149 if (!current->payload_->empty ())
2151 bin_name.push_back (current->thisnodestorage_.string ());
2156 for (
int i = 0; i < 8; i++)
2158 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2159 if (boost::filesystem::exists (child_dir))
2161 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2162 current->num_children_++;
2177 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_