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 #ifdef PCL_VISUALIZATION_AVAILABLE
55 #include <pcl/visualization/common/common.h>
57 #include <pcl/outofcore/octree_base_node.h>
58 #include <pcl/filters/random_sample.h>
59 #include <pcl/filters/extract_indices.h>
62 #include <pcl/pcl_config.h>
63 #if defined(HAVE_CJSON)
64 #include <cjson/cJSON.h>
66 #include <pcl/outofcore/cJSON.h>
74 template<
typename ContainerT,
typename Po
intT>
77 template<
typename ContainerT,
typename Po
intT>
80 template<
typename ContainerT,
typename Po
intT>
83 template<
typename ContainerT,
typename Po
intT>
86 template<
typename ContainerT,
typename Po
intT>
89 template<
typename ContainerT,
typename Po
intT>
92 template<
typename ContainerT,
typename Po
intT>
95 template<
typename ContainerT,
typename Po
intT>
98 template<
typename ContainerT,
typename Po
intT>
104 , children_ (8, nullptr)
106 , num_loaded_children_ (0)
115 template<
typename ContainerT,
typename Po
intT>
121 , children_ (8, nullptr)
123 , num_loaded_children_ (0)
130 if (super ==
nullptr)
132 node_metadata_->setDirectoryPathname (directory_path.parent_path ());
138 if (!boost::filesystem::exists (
node_metadata_->getDirectoryPathname ()))
140 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n",
node_metadata_->getDirectoryPathname ().c_str ());
141 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
150 boost::filesystem::directory_iterator directory_it_end;
153 bool b_loaded =
false;
155 for (boost::filesystem::directory_iterator directory_it (
node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
157 const boost::filesystem::path& file = *directory_it;
159 if (!boost::filesystem::is_directory (file))
171 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
172 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
189 template<
typename ContainerT,
typename Po
intT>
195 , children_ (8, nullptr)
197 , num_loaded_children_ (0)
201 assert (tree !=
nullptr);
208 template<
typename ContainerT,
typename Po
intT>
void
211 assert (tree !=
nullptr);
221 Eigen::Vector3d tmp_max = bb_max;
224 double epsilon = 1e-8;
225 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
227 node_metadata_->setBoundingBox (bb_min, tmp_max);
228 node_metadata_->setDirectoryPathname (root_name.parent_path ());
229 node_metadata_->setOutofcoreVersion (3);
232 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
234 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
237 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
239 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
240 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
248 std::string node_container_name;
250 node_container_name = uuid + std::string (
"_") + node_container_basename + pcd_extension;
252 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
253 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
255 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
256 node_metadata_->serializeMetadataToDisk ();
259 payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
264 template<
typename ContainerT,
typename Po
intT>
273 template<
typename ContainerT,
typename Po
intT> std::size_t
276 std::size_t child_count = 0;
278 for(std::size_t i=0; i<8; i++)
280 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
281 if (boost::filesystem::exists (child_path))
284 return (child_count);
289 template<
typename ContainerT,
typename Po
intT>
void
292 node_metadata_->serializeMetadataToDisk ();
296 for (std::size_t i = 0; i < 8; i++)
299 children_[i]->saveIdx (
true);
306 template<
typename ContainerT,
typename Po
intT>
bool
309 return (this->getNumLoadedChildren () < this->getNumChildren ());
313 template<
typename ContainerT,
typename Po
intT>
void
317 if (num_loaded_children_ < this->getNumChildren ())
320 for (
int i = 0; i < 8; i++)
322 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
324 if (boost::filesystem::exists (child_dir) && this->children_[i] ==
nullptr)
329 num_loaded_children_++;
333 assert (num_loaded_children_ == this->getNumChildren ());
337 template<
typename ContainerT,
typename Po
intT>
void
340 if (num_children_ == 0)
345 for (std::size_t i = 0; i < 8; i++)
354 template<
typename ContainerT,
typename Po
intT> std::uint64_t
364 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
365 return (addDataAtMaxDepth( p, skip_bb_check));
367 if (hasUnloadedChildren ())
368 loadChildren (
false);
370 std::vector < std::vector<const PointT*> > c;
372 for (std::size_t i = 0; i < 8; i++)
374 c[i].reserve (p.size () / 8);
377 const std::size_t len = p.size ();
378 for (std::size_t i = 0; i < len; i++)
386 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
391 std::uint8_t box = 0;
392 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
394 box =
static_cast<std::uint8_t
>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
395 c[
static_cast<std::size_t
>(box)].push_back (&pt);
398 std::uint64_t points_added = 0;
399 for (std::size_t i = 0; i < 8; i++)
405 points_added += children_[i]->addDataToLeaf (c[i],
true);
408 return (points_added);
413 template<
typename ContainerT,
typename Po
intT> std::uint64_t
421 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
426 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
428 payload_->insertRange (p.data (), p.size ());
433 std::vector<const PointT*> buff;
434 for (
const PointT* pt : p)
444 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
445 payload_->insertRange (buff.data (), buff.size ());
449 return (buff.size ());
452 if (this->hasUnloadedChildren ())
454 loadChildren (
false);
457 std::vector < std::vector<const PointT*> > c;
459 for (std::size_t i = 0; i < 8; i++)
461 c[i].reserve (p.size () / 8);
464 const std::size_t len = p.size ();
465 for (std::size_t i = 0; i < len; i++)
477 std::uint8_t box = 00;
478 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
480 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] )));
482 c[box].push_back (p[i]);
485 std::uint64_t points_added = 0;
486 for (std::size_t i = 0; i < 8; i++)
492 points_added += children_[i]->addDataToLeaf (c[i],
true);
495 return (points_added);
500 template<
typename ContainerT,
typename Po
intT> std::uint64_t
503 assert (this->root_node_->m_tree_ !=
nullptr);
505 if (input_cloud->height*input_cloud->width == 0)
508 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
509 return (addDataAtMaxDepth (input_cloud,
true));
511 if( num_children_ < 8 )
512 if(hasUnloadedChildren ())
513 loadChildren (
false);
520 std::vector < pcl::Indices > indices;
523 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
525 for(std::size_t k=0; k<indices.size (); k++)
527 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
530 std::uint64_t points_added = 0;
532 for(std::size_t i=0; i<8; i++)
534 if ( indices[i].empty () )
537 if (children_[i] ==
nullptr)
544 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
550 points_added += children_[i]->addPointCloud (dst_cloud,
false);
554 return (points_added);
557 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
564 template<
typename ContainerT,
typename Po
intT>
void
567 assert (this->root_node_->m_tree_ !=
nullptr);
576 sampleBuff.push_back(pt);
586 const double percent = pow(sample_percent_,
static_cast<double>((this->root_node_->m_tree_->getDepth () - depth_)));
587 const auto samplesize =
static_cast<std::uint64_t
>(percent *
static_cast<double>(sampleBuff.size()));
588 const std::uint64_t inputsize = sampleBuff.size();
593 insertBuff.resize(samplesize);
596 std::lock_guard<std::mutex> lock(rng_mutex_);
597 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
600 for(std::uint64_t i = 0; i < samplesize; ++i)
602 std::uint64_t buffstart = buffdist(rng_);
603 insertBuff[i] = ( sampleBuff[buffstart] );
609 std::lock_guard<std::mutex> lock(rng_mutex_);
610 std::bernoulli_distribution buffdist(percent);
612 for(std::uint64_t i = 0; i < inputsize; ++i)
614 insertBuff.push_back( p[i] );
619 template<
typename ContainerT,
typename Po
intT> std::uint64_t
622 assert (this->root_node_->m_tree_ !=
nullptr);
628 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
631 payload_->insertRange ( p );
638 const std::size_t len = p.size ();
640 for (std::size_t i = 0; i < len; i++)
644 buff.push_back (p[i]);
650 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
651 payload_->insertRange ( buff );
653 return (buff.size ());
656 template<
typename ContainerT,
typename Po
intT> std::uint64_t
662 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
664 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
665 payload_->insertRange (input_cloud);
667 return (input_cloud->width*input_cloud->height);
669 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
675 template<
typename ContainerT,
typename Po
intT>
void
680 for(std::size_t i = 0; i < 8; i++)
681 c[i].reserve(p.size() / 8);
683 const std::size_t len = p.size();
684 for(std::size_t i = 0; i < len; i++)
692 subdividePoint (pt, c);
697 template<
typename ContainerT,
typename Po
intT>
void
700 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
701 std::size_t octant = 0;
702 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
703 c[octant].push_back (point);
707 template<
typename ContainerT,
typename Po
intT> std::uint64_t
710 std::uint64_t points_added = 0;
712 if ( input_cloud->width * input_cloud->height == 0 )
717 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
719 std::uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
720 assert (points_added > 0);
721 return (points_added);
724 if (num_children_ < 8 )
726 if ( hasUnloadedChildren () )
728 loadChildren (
false);
741 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
742 random_sampler.
setSample (
static_cast<unsigned int> (sample_size));
749 random_sampler.
filter (*downsampled_cloud_indices);
754 extractor.
setIndices (downsampled_cloud_indices);
755 extractor.
filter (*downsampled_cloud);
760 extractor.
filter (*remaining_points);
762 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 );
765 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
767 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
768 payload_->insertRange (downsampled_cloud);
769 points_added += downsampled_cloud->width*downsampled_cloud->height ;
772 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
775 std::vector<pcl::Indices> indices;
778 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
781 for(std::size_t i=0; i<8; i++)
784 if(indices[i].empty ())
787 if (children_[i] ==
nullptr)
798 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
799 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);
802 assert (points_added == input_cloud->width*input_cloud->height);
803 return (points_added);
807 template<
typename ContainerT,
typename Po
intT> std::uint64_t
816 assert (this->root_node_->m_tree_ !=
nullptr );
818 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
820 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
821 return (addDataAtMaxDepth(p,
false));
825 if (this->hasUnloadedChildren ())
826 loadChildren (
false );
830 randomSample(p, insertBuff, skip_bb_check);
832 if(!insertBuff.empty())
835 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
837 payload_->insertRange (insertBuff);
842 std::vector<AlignedPointTVector> c;
843 subdividePoints(p, c, skip_bb_check);
845 std::uint64_t points_added = 0;
846 for(std::size_t i = 0; i < 8; i++)
857 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
861 return (points_added);
865 template<
typename ContainerT,
typename Po
intT>
void
871 if (children_[idx] || (num_children_ == 8))
873 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
877 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
878 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
880 Eigen::Vector3d childbb_min;
881 Eigen::Vector3d childbb_max;
886 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
887 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
892 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
893 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
897 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
898 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
900 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
901 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
903 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
904 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
906 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
913 template<
typename ContainerT,
typename Po
intT>
bool
914 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
916 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
917 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
918 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
928 template<
typename ContainerT,
typename Po
intT>
bool
931 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
932 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
934 if (((min[0] <= p.x) && (p.x < max[0])) &&
935 ((min[1] <= p.y) && (p.y < max[1])) &&
936 ((min[2] <= p.z) && (p.z < max[2])))
945 template<
typename ContainerT,
typename Po
intT>
void
950 node_metadata_->getBoundingBox (min, max);
952 if (this->depth_ < query_depth){
953 for (std::size_t i = 0; i < this->depth_; i++)
956 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
957 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
958 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
959 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
961 if (num_children_ > 0)
963 for (std::size_t i = 0; i < 8; i++)
966 children_[i]->printBoundingBox (query_depth);
973 template<
typename ContainerT,
typename Po
intT>
void
976 if (this->depth_ < query_depth){
977 if (num_children_ > 0)
979 for (std::size_t i = 0; i < 8; i++)
982 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
989 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
990 voxel_center.x =
static_cast<float>(mid_xyz[0]);
991 voxel_center.y =
static_cast<float>(mid_xyz[1]);
992 voxel_center.z =
static_cast<float>(mid_xyz[2]);
994 voxel_centers.push_back(voxel_center);
1050 template<
typename Container,
typename Po
intT>
void
1053 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1056 template<
typename Container,
typename Po
intT>
void
1060 enum {INSIDE, INTERSECT, OUTSIDE};
1062 int result = INSIDE;
1064 if (this->depth_ > query_depth)
1072 if (!skip_vfc_check)
1074 for(
int i =0; i < 6; i++){
1075 double a = planes[(i*4)];
1076 double b = planes[(i*4)+1];
1077 double c = planes[(i*4)+2];
1078 double d = planes[(i*4)+3];
1082 Eigen::Vector3d normal(a, b, c);
1084 Eigen::Vector3d min_bb;
1085 Eigen::Vector3d max_bb;
1086 node_metadata_->getBoundingBox(min_bb, max_bb);
1089 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1090 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1091 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1092 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1094 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1095 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1102 if (m - n < 0) result = INTERSECT;
1153 if (result == OUTSIDE)
1171 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1174 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1177 if (hasUnloadedChildren ())
1179 loadChildren (
false);
1182 if (this->getNumChildren () > 0)
1184 for (std::size_t i = 0; i < 8; i++)
1187 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1204 #ifdef PCL_VISUALIZATION_AVAILABLE
1205 template<
typename Container,
typename Po
intT>
void
1210 if (this->depth_ > query_depth)
1216 Eigen::Vector3d min_bb;
1217 Eigen::Vector3d max_bb;
1218 node_metadata_->getBoundingBox(min_bb, max_bb);
1221 enum {INSIDE, INTERSECT, OUTSIDE};
1223 int result = INSIDE;
1225 if (!skip_vfc_check)
1227 for(
int i =0; i < 6; i++){
1228 double a = planes[(i*4)];
1229 double b = planes[(i*4)+1];
1230 double c = planes[(i*4)+2];
1231 double d = planes[(i*4)+3];
1235 Eigen::Vector3d normal(a, b, c);
1238 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1239 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1240 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1241 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1243 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1244 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1251 if (m - n < 0) result = INTERSECT;
1256 if (result == OUTSIDE)
1291 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1294 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1298 if (coverage <= 10000)
1301 if (hasUnloadedChildren ())
1303 loadChildren (
false);
1306 if (this->getNumChildren () > 0)
1308 for (std::size_t i = 0; i < 8; i++)
1311 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1318 template<
typename ContainerT,
typename Po
intT>
void
1321 if (this->depth_ < query_depth){
1322 if (num_children_ > 0)
1324 for (std::size_t i = 0; i < 8; i++)
1327 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1333 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1334 voxel_centers.push_back(voxel_center);
1341 template<
typename ContainerT,
typename Po
intT>
void
1344 if (intersectsWithBoundingBox (min_bb, max_bb))
1346 if (this->depth_ < query_depth)
1348 if (this->getNumChildren () > 0)
1350 for (std::size_t i = 0; i < 8; i++)
1353 children_[i]->queryBBIntersects (min_bb, max_bb, query_depth, file_names);
1356 else if (hasUnloadedChildren ())
1358 loadChildren (
false);
1360 for (std::size_t i = 0; i < 8; i++)
1363 children_[i]->queryBBIntersects (min_bb, max_bb, query_depth, file_names);
1369 if (payload_->getDataSize () > 0)
1371 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1377 template<
typename ContainerT,
typename Po
intT>
void
1380 std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1381 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1384 if (intersectsWithBoundingBox (min_bb, max_bb))
1387 if (this->depth_ < query_depth)
1390 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1391 loadChildren (
false);
1394 if (num_children_ > 0)
1397 for (std::size_t i = 0; i < 8; i++)
1400 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1402 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1412 payload_->readRange (0, payload_->size (), tmp_blob);
1414 if( tmp_blob->width*tmp_blob->height == 0 )
1418 if (inBoundingBox (min_bb, max_bb))
1424 if( dst_blob->width*dst_blob->height != 0 )
1426 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1427 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1432 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1437 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1439 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1441 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1452 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1454 Eigen::Vector4f min_pt (
static_cast<float> ( min_bb[0] ),
static_cast<float> ( min_bb[1] ),
static_cast<float> ( min_bb[2] ), 1.0f);
1455 Eigen::Vector4f max_pt (
static_cast<float> ( max_bb[0] ),
static_cast<float> ( max_bb[1] ) ,
static_cast<float>( max_bb[2] ), 1.0f );
1460 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1461 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1463 if ( !indices.empty () )
1465 if( dst_blob->width*dst_blob->height > 0 )
1472 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1473 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1475 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1476 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1480 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1486 assert ( dst_blob->width*dst_blob->height == indices.size () );
1492 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1495 template<
typename ContainerT,
typename Po
intT>
void
1500 if (intersectsWithBoundingBox (min_bb, max_bb))
1503 if (this->depth_ < query_depth)
1506 if (this->hasUnloadedChildren ())
1508 this->loadChildren (
false);
1512 if (getNumChildren () > 0)
1514 if(hasUnloadedChildren ())
1515 loadChildren (
false);
1518 for (std::size_t i = 0; i < 8; i++)
1521 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1530 if (inBoundingBox (min_bb, max_bb))
1534 payload_->readRange (0, payload_->size (), payload_cache);
1535 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1543 payload_->readRange (0, payload_->size (), payload_cache);
1545 std::uint64_t len = payload_->size ();
1547 for (std::uint64_t i = 0; i < len; i++)
1549 const PointT& p = payload_cache[i];
1558 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]);
1566 template<
typename ContainerT,
typename Po
intT>
void
1569 if (intersectsWithBoundingBox (min_bb, max_bb))
1571 if (this->depth_ < query_depth)
1573 if (this->hasUnloadedChildren ())
1574 this->loadChildren (
false);
1576 if (this->getNumChildren () > 0)
1578 for (std::size_t i=0; i<8; i++)
1581 if (children_[i]!=
nullptr)
1582 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1591 if (inBoundingBox (min_bb, max_bb))
1594 this->payload_->read (tmp_blob);
1595 std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1597 double sample_points =
static_cast<double>(num_pts) * percent;
1601 sample_points = sample_points > 1 ? sample_points : 1;
1615 random_sampler.
setSample (
static_cast<unsigned int> (sample_points));
1621 random_sampler.
filter (*downsampled_cloud_indices);
1622 extractor.
setIndices (downsampled_cloud_indices);
1623 extractor.
filter (*downsampled_points);
1634 template<
typename ContainerT,
typename Po
intT>
void
1638 if (intersectsWithBoundingBox (min_bb, max_bb))
1641 if (this->depth_ < query_depth)
1644 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1646 loadChildren (
false);
1649 if (num_children_ > 0)
1652 for (std::size_t i = 0; i < 8; i++)
1655 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1664 if (inBoundingBox (min_bb, max_bb))
1668 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1669 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1677 payload_->readRange (0, payload_->size (), payload_cache);
1678 for (std::size_t i = 0; i < payload_->size (); i++)
1680 const PointT& p = payload_cache[i];
1683 payload_cache_within_region.push_back (p);
1689 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1690 auto numpick =
static_cast<std::size_t
> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1692 for (std::size_t i = 0; i < numpick; i++)
1694 dst.push_back (payload_cache_within_region[i]);
1702 template<
typename ContainerT,
typename Po
intT>
1708 , children_ (8, nullptr)
1710 , num_loaded_children_ (0)
1716 if (super ==
nullptr)
1718 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1719 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1732 std::string uuid_idx;
1733 std::string uuid_cont;
1739 std::string node_container_name;
1742 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1746 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1754 template<
typename ContainerT,
typename Po
intT>
void
1757 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1759 loadChildren (
false);
1762 for (std::size_t i = 0; i < num_children_; i++)
1764 children_[i]->copyAllCurrentAndChildPointsRec (v);
1768 payload_->readRange (0, payload_->size (), payload_cache);
1771 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1777 template<
typename ContainerT,
typename Po
intT>
void
1780 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1782 loadChildren (
false);
1785 for (std::size_t i = 0; i < 8; i++)
1788 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1791 std::vector<PointT> payload_cache;
1792 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1794 for (std::size_t i = 0; i < payload_cache.size (); i++)
1796 v.push_back (payload_cache[i]);
1802 template<
typename ContainerT,
typename Po
intT>
inline bool
1805 Eigen::Vector3d min, max;
1806 node_metadata_->getBoundingBox (min, max);
1809 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1811 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1813 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1824 template<
typename ContainerT,
typename Po
intT>
inline bool
1827 Eigen::Vector3d min, max;
1829 node_metadata_->getBoundingBox (min, max);
1831 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1833 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1835 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1846 template<
typename ContainerT,
typename Po
intT>
inline bool
1851 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1853 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1855 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1866 template<
typename ContainerT,
typename Po
intT>
void
1869 Eigen::Vector3d min;
1870 Eigen::Vector3d max;
1871 node_metadata_->getBoundingBox (min, max);
1873 double l = max[0] - min[0];
1874 double h = max[1] - min[1];
1875 double w = max[2] - min[2];
1876 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1877 <<
", width=" << w <<
" )\n";
1879 for (std::size_t i = 0; i < num_children_; i++)
1881 children_[i]->writeVPythonVisual (file);
1887 template<
typename ContainerT,
typename Po
intT>
int
1890 return (this->payload_->read (output_cloud));
1898 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1899 return (children_[index_arg]);
1903 template<
typename ContainerT,
typename Po
intT> std::uint64_t
1906 return (this->payload_->getDataSize ());
1911 template<
typename ContainerT,
typename Po
intT> std::size_t
1914 std::size_t loaded_children_count = 0;
1916 for (std::size_t i=0; i<8; i++)
1918 if (children_[i] !=
nullptr)
1919 loaded_children_count++;
1922 return (loaded_children_count);
1927 template<
typename ContainerT,
typename Po
intT>
void
1930 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1931 node_metadata_->loadMetadataFromDisk (path);
1934 this->parent_ = super;
1936 if (num_children_ > 0)
1939 this->num_children_ = 0;
1940 this->payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
1945 template<
typename ContainerT,
typename Po
intT>
void
1948 std::string fname = node_metadata_->getPCDFilename ().stem ().string () +
".dat.xyz";
1949 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1950 payload_->convertToXYZ (xyzfile);
1952 if (hasUnloadedChildren ())
1954 loadChildren (
false);
1957 for (std::size_t i = 0; i < 8; i++)
1960 children_[i]->convertToXYZ ();
1966 template<
typename ContainerT,
typename Po
intT>
void
1969 for (std::size_t i = 0; i < 8; i++)
1972 children_[i]->flushToDiskRecursive ();
1978 template<
typename ContainerT,
typename Po
intT>
void
1981 if (indices.size () < 8)
1988 int x_offset = input_cloud->fields[x_idx].offset;
1989 int y_offset = input_cloud->fields[y_idx].offset;
1990 int z_offset = input_cloud->fields[z_idx].offset;
1992 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1996 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
1997 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
1998 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
2000 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
2005 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);
2011 std::size_t box = 0;
2012 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2016 indices[box].push_back (
static_cast<int> (point_idx/input_cloud->point_step));
2030 thisnode->thisdir_ = path.parent_path ();
2032 if (!boost::filesystem::exists (thisnode->thisdir_))
2034 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2035 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2038 thisnode->thisnodeindex_ = path;
2045 thisnode->thisdir_ = path;
2049 if (thisnode->
depth_ > thisnode->root->max_depth_)
2051 thisnode->root->max_depth_ = thisnode->
depth_;
2054 boost::filesystem::directory_iterator diterend;
2055 bool loaded =
false;
2056 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2058 const boost::filesystem::path& file = *diter;
2059 if (!boost::filesystem::is_directory (file))
2063 thisnode->thisnodeindex_ = file;
2072 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2073 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2077 thisnode->max_depth_ = 0;
2080 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2082 f >> thisnode->min_[0];
2083 f >> thisnode->min_[1];
2084 f >> thisnode->min_[2];
2085 f >> thisnode->max_[0];
2086 f >> thisnode->max_[1];
2087 f >> thisnode->max_[2];
2089 std::string filename;
2091 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2095 thisnode->
payload_.reset (
new ContainerT (thisnode->thisnodestorage_));
2100 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2109 template<
typename ContainerT,
typename Po
intT>
void
2110 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)
2112 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2115 std::cout <<
"test";
2117 if (root->intersectsWithBoundingBox (min, max))
2119 if (query_depth == root->max_depth_)
2121 if (!root->payload_->empty ())
2123 bin_name.push_back (root->thisnodestorage_.string ());
2128 for (
int i = 0; i < 8; i++)
2130 boost::filesystem::path child_dir = root->thisdir_
2131 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2132 if (boost::filesystem::exists (child_dir))
2135 root->num_children_++;
2145 template<
typename ContainerT,
typename Po
intT>
void
2146 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)
2148 if (current->intersectsWithBoundingBox (min, max))
2150 if (current->depth_ == query_depth)
2152 if (!current->payload_->empty ())
2154 bin_name.push_back (current->thisnodestorage_.string ());
2159 for (
int i = 0; i < 8; i++)
2161 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2162 if (boost::filesystem::exists (child_dir))
2164 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2165 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.
bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
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.
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.