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;
217 double epsilon = 1e-8;
218 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
220 node_metadata_->setBoundingBox (bb_min, tmp_max);
221 node_metadata_->setDirectoryPathname (root_name.parent_path ());
222 node_metadata_->setOutofcoreVersion (3);
225 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
227 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
230 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
232 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
233 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
241 std::string node_container_name;
243 node_container_name = uuid + std::string (
"_") + node_container_basename + pcd_extension;
245 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
246 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
248 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
249 node_metadata_->serializeMetadataToDisk ();
252 payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
257 template<
typename ContainerT,
typename Po
intT>
266 template<
typename ContainerT,
typename Po
intT> std::size_t
269 std::size_t child_count = 0;
271 for(std::size_t i=0; i<8; i++)
273 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
274 if (boost::filesystem::exists (child_path))
277 return (child_count);
282 template<
typename ContainerT,
typename Po
intT>
void
285 node_metadata_->serializeMetadataToDisk ();
289 for (std::size_t i = 0; i < 8; i++)
292 children_[i]->saveIdx (
true);
299 template<
typename ContainerT,
typename Po
intT>
bool
302 return (this->getNumLoadedChildren () < this->getNumChildren ());
306 template<
typename ContainerT,
typename Po
intT>
void
310 if (num_loaded_children_ < this->getNumChildren ())
313 for (
int i = 0; i < 8; i++)
315 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
317 if (boost::filesystem::exists (child_dir) && this->children_[i] ==
nullptr)
322 num_loaded_children_++;
326 assert (num_loaded_children_ == this->getNumChildren ());
330 template<
typename ContainerT,
typename Po
intT>
void
333 if (num_children_ == 0)
338 for (std::size_t i = 0; i < 8; i++)
347 template<
typename ContainerT,
typename Po
intT> std::uint64_t
357 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
358 return (addDataAtMaxDepth( p, skip_bb_check));
360 if (hasUnloadedChildren ())
361 loadChildren (
false);
363 std::vector < std::vector<const PointT*> > c;
365 for (std::size_t i = 0; i < 8; i++)
367 c[i].reserve (p.size () / 8);
370 const std::size_t len = p.size ();
371 for (std::size_t i = 0; i < len; i++)
379 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
384 std::uint8_t box = 0;
385 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
387 box =
static_cast<std::uint8_t
>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
388 c[
static_cast<std::size_t
>(box)].push_back (&pt);
391 std::uint64_t points_added = 0;
392 for (std::size_t i = 0; i < 8; i++)
398 points_added += children_[i]->addDataToLeaf (c[i],
true);
401 return (points_added);
406 template<
typename ContainerT,
typename Po
intT> std::uint64_t
414 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
419 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
421 payload_->insertRange (p.data (), p.size ());
426 std::vector<const PointT*> buff;
427 for (
const PointT* pt : p)
437 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
438 payload_->insertRange (buff.data (), buff.size ());
442 return (buff.size ());
445 if (this->hasUnloadedChildren ())
447 loadChildren (
false);
450 std::vector < std::vector<const PointT*> > c;
452 for (std::size_t i = 0; i < 8; i++)
454 c[i].reserve (p.size () / 8);
457 const std::size_t len = p.size ();
458 for (std::size_t i = 0; i < len; i++)
470 std::uint8_t box = 00;
471 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
473 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] )));
475 c[box].push_back (p[i]);
478 std::uint64_t points_added = 0;
479 for (std::size_t i = 0; i < 8; i++)
485 points_added += children_[i]->addDataToLeaf (c[i],
true);
488 return (points_added);
493 template<
typename ContainerT,
typename Po
intT> std::uint64_t
496 assert (this->root_node_->m_tree_ !=
nullptr);
498 if (input_cloud->height*input_cloud->width == 0)
501 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
502 return (addDataAtMaxDepth (input_cloud,
true));
504 if( num_children_ < 8 )
505 if(hasUnloadedChildren ())
506 loadChildren (
false);
513 std::vector < pcl::Indices > indices;
516 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
518 for(std::size_t k=0; k<indices.size (); k++)
520 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
523 std::uint64_t points_added = 0;
525 for(std::size_t i=0; i<8; i++)
527 if ( indices[i].empty () )
530 if (children_[i] ==
nullptr)
537 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
543 points_added += children_[i]->addPointCloud (dst_cloud,
false);
547 return (points_added);
550 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
557 template<
typename ContainerT,
typename Po
intT>
void
560 assert (this->root_node_->m_tree_ !=
nullptr);
569 sampleBuff.push_back(pt);
579 const double percent = pow(sample_percent_,
static_cast<double>((this->root_node_->m_tree_->getDepth () - depth_)));
580 const auto samplesize =
static_cast<std::uint64_t
>(percent *
static_cast<double>(sampleBuff.size()));
581 const std::uint64_t inputsize = sampleBuff.size();
586 insertBuff.resize(samplesize);
589 std::lock_guard<std::mutex> lock(rng_mutex_);
590 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
593 for(std::uint64_t i = 0; i < samplesize; ++i)
595 std::uint64_t buffstart = buffdist(rng_);
596 insertBuff[i] = ( sampleBuff[buffstart] );
602 std::lock_guard<std::mutex> lock(rng_mutex_);
603 std::bernoulli_distribution buffdist(percent);
605 for(std::uint64_t i = 0; i < inputsize; ++i)
607 insertBuff.push_back( p[i] );
612 template<
typename ContainerT,
typename Po
intT> std::uint64_t
615 assert (this->root_node_->m_tree_ !=
nullptr);
621 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
624 payload_->insertRange ( p );
631 const std::size_t len = p.size ();
633 for (std::size_t i = 0; i < len; i++)
637 buff.push_back (p[i]);
643 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
644 payload_->insertRange ( buff );
646 return (buff.size ());
649 template<
typename ContainerT,
typename Po
intT> std::uint64_t
655 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
657 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
658 payload_->insertRange (input_cloud);
660 return (input_cloud->width*input_cloud->height);
662 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
668 template<
typename ContainerT,
typename Po
intT>
void
673 for(std::size_t i = 0; i < 8; i++)
674 c[i].reserve(p.size() / 8);
676 const std::size_t len = p.size();
677 for(std::size_t i = 0; i < len; i++)
685 subdividePoint (pt, c);
690 template<
typename ContainerT,
typename Po
intT>
void
693 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
694 std::size_t octant = 0;
695 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
696 c[octant].push_back (point);
700 template<
typename ContainerT,
typename Po
intT> std::uint64_t
703 std::uint64_t points_added = 0;
705 if ( input_cloud->width * input_cloud->height == 0 )
710 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
712 std::uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
713 assert (points_added > 0);
714 return (points_added);
717 if (num_children_ < 8 )
719 if ( hasUnloadedChildren () )
721 loadChildren (
false);
734 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
735 random_sampler.
setSample (
static_cast<unsigned int> (sample_size));
742 random_sampler.
filter (*downsampled_cloud_indices);
747 extractor.
setIndices (downsampled_cloud_indices);
748 extractor.
filter (*downsampled_cloud);
753 extractor.
filter (*remaining_points);
755 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 );
758 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
760 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
761 payload_->insertRange (downsampled_cloud);
762 points_added += downsampled_cloud->width*downsampled_cloud->height ;
765 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
768 std::vector<pcl::Indices> indices;
771 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
774 for(std::size_t i=0; i<8; i++)
777 if(indices[i].empty ())
780 if (children_[i] ==
nullptr)
791 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
792 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);
795 assert (points_added == input_cloud->width*input_cloud->height);
796 return (points_added);
800 template<
typename ContainerT,
typename Po
intT> std::uint64_t
809 assert (this->root_node_->m_tree_ !=
nullptr );
811 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
813 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
814 return (addDataAtMaxDepth(p,
false));
818 if (this->hasUnloadedChildren ())
819 loadChildren (
false );
823 randomSample(p, insertBuff, skip_bb_check);
825 if(!insertBuff.empty())
828 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
830 payload_->insertRange (insertBuff);
835 std::vector<AlignedPointTVector> c;
836 subdividePoints(p, c, skip_bb_check);
838 std::uint64_t points_added = 0;
839 for(std::size_t i = 0; i < 8; i++)
850 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
854 return (points_added);
858 template<
typename ContainerT,
typename Po
intT>
void
864 if (children_[idx] || (num_children_ == 8))
866 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
870 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
871 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
873 Eigen::Vector3d childbb_min;
874 Eigen::Vector3d childbb_max;
879 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
880 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
885 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
886 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
890 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
891 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
893 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
894 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
896 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
897 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
899 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
906 template<
typename ContainerT,
typename Po
intT>
bool
907 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
909 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
910 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
911 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
921 template<
typename ContainerT,
typename Po
intT>
bool
924 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
925 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
927 if (((min[0] <= p.x) && (p.x < max[0])) &&
928 ((min[1] <= p.y) && (p.y < max[1])) &&
929 ((min[2] <= p.z) && (p.z < max[2])))
938 template<
typename ContainerT,
typename Po
intT>
void
943 node_metadata_->getBoundingBox (min, max);
945 if (this->depth_ < query_depth){
946 for (std::size_t i = 0; i < this->depth_; i++)
949 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
950 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
951 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
952 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
954 if (num_children_ > 0)
956 for (std::size_t i = 0; i < 8; i++)
959 children_[i]->printBoundingBox (query_depth);
966 template<
typename ContainerT,
typename Po
intT>
void
969 if (this->depth_ < query_depth){
970 if (num_children_ > 0)
972 for (std::size_t i = 0; i < 8; i++)
975 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
982 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
983 voxel_center.x =
static_cast<float>(mid_xyz[0]);
984 voxel_center.y =
static_cast<float>(mid_xyz[1]);
985 voxel_center.z =
static_cast<float>(mid_xyz[2]);
987 voxel_centers.push_back(voxel_center);
1043 template<
typename Container,
typename Po
intT>
void
1046 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1049 template<
typename Container,
typename Po
intT>
void
1053 enum {INSIDE, INTERSECT, OUTSIDE};
1055 int result = INSIDE;
1057 if (this->depth_ > query_depth)
1065 if (!skip_vfc_check)
1067 for(
int i =0; i < 6; i++){
1068 double a = planes[(i*4)];
1069 double b = planes[(i*4)+1];
1070 double c = planes[(i*4)+2];
1071 double d = planes[(i*4)+3];
1075 Eigen::Vector3d normal(a, b, c);
1077 Eigen::Vector3d min_bb;
1078 Eigen::Vector3d max_bb;
1079 node_metadata_->getBoundingBox(min_bb, max_bb);
1082 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1083 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1084 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1085 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1087 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1088 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1095 if (m - n < 0) result = INTERSECT;
1146 if (result == OUTSIDE)
1164 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1167 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1170 if (hasUnloadedChildren ())
1172 loadChildren (
false);
1175 if (this->getNumChildren () > 0)
1177 for (std::size_t i = 0; i < 8; i++)
1180 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1198 template<
typename Container,
typename Po
intT>
void
1203 if (this->depth_ > query_depth)
1209 Eigen::Vector3d min_bb;
1210 Eigen::Vector3d max_bb;
1211 node_metadata_->getBoundingBox(min_bb, max_bb);
1214 enum {INSIDE, INTERSECT, OUTSIDE};
1216 int result = INSIDE;
1218 if (!skip_vfc_check)
1220 for(
int i =0; i < 6; i++){
1221 double a = planes[(i*4)];
1222 double b = planes[(i*4)+1];
1223 double c = planes[(i*4)+2];
1224 double d = planes[(i*4)+3];
1228 Eigen::Vector3d normal(a, b, c);
1231 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1232 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1233 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1234 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1236 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1237 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1244 if (m - n < 0) result = INTERSECT;
1249 if (result == OUTSIDE)
1284 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1287 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1291 if (coverage <= 10000)
1294 if (hasUnloadedChildren ())
1296 loadChildren (
false);
1299 if (this->getNumChildren () > 0)
1301 for (std::size_t i = 0; i < 8; i++)
1304 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1310 template<
typename ContainerT,
typename Po
intT>
void
1313 if (this->depth_ < query_depth){
1314 if (num_children_ > 0)
1316 for (std::size_t i = 0; i < 8; i++)
1319 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1325 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1326 voxel_centers.push_back(voxel_center);
1333 template<
typename ContainerT,
typename Po
intT>
void
1336 if (intersectsWithBoundingBox (min_bb, max_bb))
1338 if (this->depth_ < query_depth)
1340 if (this->getNumChildren () > 0)
1342 for (std::size_t i = 0; i < 8; i++)
1345 children_[i]->queryBBIntersects (min_bb, max_bb, query_depth, file_names);
1348 else if (hasUnloadedChildren ())
1350 loadChildren (
false);
1352 for (std::size_t i = 0; i < 8; i++)
1355 children_[i]->queryBBIntersects (min_bb, max_bb, query_depth, file_names);
1361 if (payload_->getDataSize () > 0)
1363 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1369 template<
typename ContainerT,
typename Po
intT>
void
1372 std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1373 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1376 if (intersectsWithBoundingBox (min_bb, max_bb))
1379 if (this->depth_ < query_depth)
1382 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1383 loadChildren (
false);
1386 if (num_children_ > 0)
1389 for (std::size_t i = 0; i < 8; i++)
1392 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1394 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1404 payload_->readRange (0, payload_->size (), tmp_blob);
1406 if( tmp_blob->width*tmp_blob->height == 0 )
1410 if (inBoundingBox (min_bb, max_bb))
1416 if( dst_blob->width*dst_blob->height != 0 )
1418 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1419 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1424 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1429 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1431 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1433 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1444 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1446 Eigen::Vector4f min_pt (
static_cast<float> ( min_bb[0] ),
static_cast<float> ( min_bb[1] ),
static_cast<float> ( min_bb[2] ), 1.0f);
1447 Eigen::Vector4f max_pt (
static_cast<float> ( max_bb[0] ),
static_cast<float> ( max_bb[1] ) ,
static_cast<float>( max_bb[2] ), 1.0f );
1452 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1453 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1455 if ( !indices.empty () )
1457 if( dst_blob->width*dst_blob->height > 0 )
1464 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1465 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1467 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1468 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1472 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1478 assert ( dst_blob->width*dst_blob->height == indices.size () );
1484 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1487 template<
typename ContainerT,
typename Po
intT>
void
1492 if (intersectsWithBoundingBox (min_bb, max_bb))
1495 if (this->depth_ < query_depth)
1498 if (this->hasUnloadedChildren ())
1500 this->loadChildren (
false);
1504 if (getNumChildren () > 0)
1506 if(hasUnloadedChildren ())
1507 loadChildren (
false);
1510 for (std::size_t i = 0; i < 8; i++)
1513 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1522 if (inBoundingBox (min_bb, max_bb))
1526 payload_->readRange (0, payload_->size (), payload_cache);
1527 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1535 payload_->readRange (0, payload_->size (), payload_cache);
1537 std::uint64_t len = payload_->size ();
1539 for (std::uint64_t i = 0; i < len; i++)
1541 const PointT& p = payload_cache[i];
1550 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]);
1558 template<
typename ContainerT,
typename Po
intT>
void
1561 if (intersectsWithBoundingBox (min_bb, max_bb))
1563 if (this->depth_ < query_depth)
1565 if (this->hasUnloadedChildren ())
1566 this->loadChildren (
false);
1568 if (this->getNumChildren () > 0)
1570 for (std::size_t i=0; i<8; i++)
1573 if (children_[i]!=
nullptr)
1574 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1583 if (inBoundingBox (min_bb, max_bb))
1586 this->payload_->read (tmp_blob);
1587 std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1589 double sample_points =
static_cast<double>(num_pts) * percent;
1593 sample_points = sample_points > 1 ? sample_points : 1;
1607 random_sampler.
setSample (
static_cast<unsigned int> (sample_points));
1613 random_sampler.
filter (*downsampled_cloud_indices);
1614 extractor.
setIndices (downsampled_cloud_indices);
1615 extractor.
filter (*downsampled_points);
1626 template<
typename ContainerT,
typename Po
intT>
void
1630 if (intersectsWithBoundingBox (min_bb, max_bb))
1633 if (this->depth_ < query_depth)
1636 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1638 loadChildren (
false);
1641 if (num_children_ > 0)
1644 for (std::size_t i = 0; i < 8; i++)
1647 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1656 if (inBoundingBox (min_bb, max_bb))
1660 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1661 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1669 payload_->readRange (0, payload_->size (), payload_cache);
1670 for (std::size_t i = 0; i < payload_->size (); i++)
1672 const PointT& p = payload_cache[i];
1675 payload_cache_within_region.push_back (p);
1681 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1682 auto numpick =
static_cast<std::size_t
> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1684 for (std::size_t i = 0; i < numpick; i++)
1686 dst.push_back (payload_cache_within_region[i]);
1694 template<
typename ContainerT,
typename Po
intT>
1700 , children_ (8, nullptr)
1702 , num_loaded_children_ (0)
1708 if (super ==
nullptr)
1710 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1711 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1724 std::string uuid_idx;
1725 std::string uuid_cont;
1731 std::string node_container_name;
1734 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1738 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1746 template<
typename ContainerT,
typename Po
intT>
void
1749 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1751 loadChildren (
false);
1754 for (std::size_t i = 0; i < num_children_; i++)
1756 children_[i]->copyAllCurrentAndChildPointsRec (v);
1760 payload_->readRange (0, payload_->size (), payload_cache);
1763 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1769 template<
typename ContainerT,
typename Po
intT>
void
1772 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1774 loadChildren (
false);
1777 for (std::size_t i = 0; i < 8; i++)
1780 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1783 std::vector<PointT> payload_cache;
1784 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1786 for (std::size_t i = 0; i < payload_cache.size (); i++)
1788 v.push_back (payload_cache[i]);
1794 template<
typename ContainerT,
typename Po
intT>
inline bool
1797 Eigen::Vector3d min, max;
1798 node_metadata_->getBoundingBox (min, max);
1801 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1803 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1805 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1816 template<
typename ContainerT,
typename Po
intT>
inline bool
1819 Eigen::Vector3d min, max;
1821 node_metadata_->getBoundingBox (min, max);
1823 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1825 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1827 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1838 template<
typename ContainerT,
typename Po
intT>
inline bool
1843 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1845 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1847 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1858 template<
typename ContainerT,
typename Po
intT>
void
1861 Eigen::Vector3d min;
1862 Eigen::Vector3d max;
1863 node_metadata_->getBoundingBox (min, max);
1865 double l = max[0] - min[0];
1866 double h = max[1] - min[1];
1867 double w = max[2] - min[2];
1868 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1869 <<
", width=" << w <<
" )\n";
1871 for (std::size_t i = 0; i < num_children_; i++)
1873 children_[i]->writeVPythonVisual (file);
1879 template<
typename ContainerT,
typename Po
intT>
int
1882 return (this->payload_->read (output_cloud));
1890 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1891 return (children_[index_arg]);
1895 template<
typename ContainerT,
typename Po
intT> std::uint64_t
1898 return (this->payload_->getDataSize ());
1903 template<
typename ContainerT,
typename Po
intT> std::size_t
1906 std::size_t loaded_children_count = 0;
1908 for (std::size_t i=0; i<8; i++)
1910 if (children_[i] !=
nullptr)
1911 loaded_children_count++;
1914 return (loaded_children_count);
1919 template<
typename ContainerT,
typename Po
intT>
void
1922 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1923 node_metadata_->loadMetadataFromDisk (path);
1926 this->parent_ = super;
1928 if (num_children_ > 0)
1931 this->num_children_ = 0;
1932 this->payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
1937 template<
typename ContainerT,
typename Po
intT>
void
1940 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1941 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1942 payload_->convertToXYZ (xyzfile);
1944 if (hasUnloadedChildren ())
1946 loadChildren (
false);
1949 for (std::size_t i = 0; i < 8; i++)
1952 children_[i]->convertToXYZ ();
1958 template<
typename ContainerT,
typename Po
intT>
void
1961 for (std::size_t i = 0; i < 8; i++)
1964 children_[i]->flushToDiskRecursive ();
1970 template<
typename ContainerT,
typename Po
intT>
void
1973 if (indices.size () < 8)
1980 int x_offset = input_cloud->fields[x_idx].offset;
1981 int y_offset = input_cloud->fields[y_idx].offset;
1982 int z_offset = input_cloud->fields[z_idx].offset;
1984 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1988 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
1989 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
1990 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
1992 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
1997 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);
2003 std::size_t box = 0;
2004 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2008 indices[box].push_back (
static_cast<int> (point_idx/input_cloud->point_step));
2022 thisnode->thisdir_ = path.parent_path ();
2024 if (!boost::filesystem::exists (thisnode->thisdir_))
2026 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2027 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2030 thisnode->thisnodeindex_ = path;
2037 thisnode->thisdir_ = path;
2041 if (thisnode->
depth_ > thisnode->root->max_depth_)
2043 thisnode->root->max_depth_ = thisnode->
depth_;
2046 boost::filesystem::directory_iterator diterend;
2047 bool loaded =
false;
2048 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2050 const boost::filesystem::path& file = *diter;
2051 if (!boost::filesystem::is_directory (file))
2055 thisnode->thisnodeindex_ = file;
2064 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2065 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2069 thisnode->max_depth_ = 0;
2072 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2074 f >> thisnode->min_[0];
2075 f >> thisnode->min_[1];
2076 f >> thisnode->min_[2];
2077 f >> thisnode->max_[0];
2078 f >> thisnode->max_[1];
2079 f >> thisnode->max_[2];
2081 std::string filename;
2083 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2087 thisnode->
payload_.reset (
new ContainerT (thisnode->thisnodestorage_));
2092 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2101 template<
typename ContainerT,
typename Po
intT>
void
2102 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)
2104 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2107 std::cout <<
"test";
2109 if (root->intersectsWithBoundingBox (min, max))
2111 if (query_depth == root->max_depth_)
2113 if (!root->payload_->empty ())
2115 bin_name.push_back (root->thisnodestorage_.string ());
2120 for (
int i = 0; i < 8; i++)
2122 boost::filesystem::path child_dir = root->thisdir_
2123 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2124 if (boost::filesystem::exists (child_dir))
2127 root->num_children_++;
2137 template<
typename ContainerT,
typename Po
intT>
void
2138 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)
2140 if (current->intersectsWithBoundingBox (min, max))
2142 if (current->depth_ == query_depth)
2144 if (!current->payload_->empty ())
2146 bin_name.push_back (current->thisnodestorage_.string ());
2151 for (
int i = 0; i < 8; i++)
2153 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2154 if (boost::filesystem::exists (child_dir))
2156 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2157 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 children by testing the children_ array; used to update num_loaded_childr...
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)
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map, const std::uint8_t *msg_data)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.