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 !=
nullptr);
201 template<
typename ContainerT,
typename Po
intT>
void
204 assert (tree !=
nullptr);
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_ !=
nullptr);
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 < pcl::Indices > 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_ !=
nullptr);
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_ !=
nullptr);
622 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
625 payload_->insertRange ( p );
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));
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<pcl::Indices> 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_ !=
nullptr );
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 );
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))
1531 payload_->readRange (0, payload_->size (), payload_cache);
1532 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
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))
1665 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1666 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
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));
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_++;
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions.
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
RandomSample applies a random sampling with uniform probability.
void setSample(unsigned int sample)
Set number of indices to be sampled.
This code defines the octree used for point storage at Urban Robotics.
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
std::size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
bool intersectsWithBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box intersects with the current node's bounding box.
static const std::string node_index_basename
virtual std::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)
Add a single PCLPointCloud2 into the octree and build the subsampled LOD during construction; this me...
static const std::string node_index_extension
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
~OutofcoreOctreeBaseNode() override
Will recursively delete all children calling recFreeChildrein.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
virtual std::uint64_t addDataToLeaf_and_genLOD(const AlignedPointTVector &p, const bool skip_bb_check)
Recursively add points to the leaf and children subsampling LODs on the way down.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
static std::mutex rng_mutex_
Random number generator mutex.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector &v)
Recursively add points that fall into the queried bounding box up to the query_depth.
static const double sample_percent_
std::uint64_t num_children_
Number of children on disk.
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::uint32_t query_depth, std::list< std::string > &file_names)
Recursive acquires PCD paths to any node with which the queried bounding box intersects (at query_dep...
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
OutofcoreOctreeBaseNode * parent_
super-node
void init_root_node(const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase< ContainerT, PointT > *const tree, const boost::filesystem::path &rootname)
Create root node and directory.
std::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
Tests whether point falls within the input bounding box.
void saveIdx(bool recursive)
Save node's metadata to file.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz)
Sorts the indices based on x,y,z fields and pushes the index into the proper octant's vector; This co...
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
static const std::string node_container_basename
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
void subdividePoints(const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check)
Subdivide points to pass to child nodes.
void copyAllCurrentAndChildPointsRec(std::list< PointT > &v)
Copies points from this and all children into a single point container (std::list)
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
virtual std::size_t getDepth() const
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
void flushToDiskRecursive()
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
bool inBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const
Tests whether the input bounding box falls inclusively within this node's bounding box.
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
virtual std::uint64_t addDataToLeaf(const AlignedPointTVector &p, const bool skip_bb_check=true)
add point to this node if we are a leaf, or find the leaf below us that is supposed to take the point
static const std::string node_container_extension
void recFreeChildren()
Method which recursively free children of this node.
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
void createChild(const std::size_t idx)
Creates child node idx.
virtual std::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
Define standard C methods and C++ classes that are common to all methods.
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, Indices &indices)
Get a set of points residing in a box given its bounds.
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
void 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)
Non-class method which performs a bounding box query without loading any of the point cloud data from...
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
PCL_EXPORTS float viewScreenArea(const Eigen::Vector3d &eye, const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Matrix4d &view_projection_matrix, int width, int height)
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.