41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
51 #include <pcl/common/common.h>
52 #include <pcl/visualization/common/common.h>
53 #include <pcl/outofcore/octree_base_node.h>
54 #include <pcl/filters/random_sample.h>
55 #include <pcl/filters/extract_indices.h>
58 #include <pcl/outofcore/cJSON.h>
65 template<
typename ContainerT,
typename Po
intT>
66 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename =
"node";
68 template<
typename ContainerT,
typename Po
intT>
69 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename =
"node";
71 template<
typename ContainerT,
typename Po
intT>
72 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension =
".oct_idx";
74 template<
typename ContainerT,
typename Po
intT>
75 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension =
".oct_dat";
77 template<
typename ContainerT,
typename Po
intT>
78 boost::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_;
80 template<
typename ContainerT,
typename Po
intT>
81 boost::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rand_gen_;
83 template<
typename ContainerT,
typename Po
intT>
84 const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125;
86 template<
typename ContainerT,
typename Po
intT>
87 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension =
".pcd";
89 template<
typename ContainerT,
typename Po
intT>
97 , num_loaded_children_ (0)
107 template<
typename ContainerT,
typename Po
intT>
115 , num_loaded_children_ (0)
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;
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>
190 , num_loaded_children_ (0)
194 assert (tree != NULL);
201 template<
typename ContainerT,
typename Po
intT>
void
204 assert (tree != NULL);
214 Eigen::Vector3d tmp_max = bb_max;
215 Eigen::Vector3d tmp_min = bb_min;
218 double epsilon = 1e-8;
219 tmp_max = 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_ = boost::shared_ptr<ContainerT> (
new ContainerT (node_metadata_->getPCDFilename ()));
258 template<
typename ContainerT,
typename Po
intT>
267 template<
typename ContainerT,
typename Po
intT>
size_t
270 size_t child_count = 0;
272 for(
size_t i=0; i<8; i++)
274 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::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 (
size_t i = 0; i < 8; i++)
293 children_[i]->saveIdx (
true);
300 template<
typename ContainerT,
typename Po
intT>
bool
303 if (this->getNumLoadedChildren () < this->getNumChildren ())
310 template<
typename ContainerT,
typename Po
intT>
void
314 if (num_loaded_children_ < this->getNumChildren ())
317 for (
int i = 0; i < 8; i++)
319 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (i));
321 if (boost::filesystem::exists (child_dir) && this->children_[i] == 0)
326 num_loaded_children_++;
330 assert (num_loaded_children_ == this->getNumChildren ());
334 template<
typename ContainerT,
typename Po
intT>
void
337 if (num_children_ == 0)
342 for (
size_t i = 0; i < 8; i++)
355 template<
typename ContainerT,
typename Po
intT> uint64_t
365 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
366 return (addDataAtMaxDepth( p, skip_bb_check));
368 if (hasUnloadedChildren ())
369 loadChildren (
false);
371 std::vector < std::vector<const PointT*> > c;
373 for (
size_t i = 0; i < 8; i++)
375 c[i].reserve (p.size () / 8);
378 const size_t len = p.size ();
379 for (
size_t i = 0; i < len; i++)
387 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
393 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
395 box =
static_cast<uint8_t
>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
396 c[
static_cast<size_t>(box)].push_back (&pt);
399 boost::uint64_t points_added = 0;
400 for (
size_t i = 0; i < 8; i++)
406 points_added += children_[i]->addDataToLeaf (c[i],
true);
409 return (points_added);
414 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
422 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
427 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
429 payload_->insertRange (p.data (), p.size ());
435 std::vector<const PointT*> buff;
436 BOOST_FOREACH(
const PointT* pt, p)
446 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
447 payload_->insertRange (buff.data (), buff.size ());
451 return (buff.size ());
456 if (this->hasUnloadedChildren ())
458 loadChildren (
false);
461 std::vector < std::vector<const PointT*> > c;
463 for (
size_t i = 0; i < 8; i++)
465 c[i].reserve (p.size () / 8);
468 const size_t len = p.size ();
469 for (
size_t i = 0; i < len; i++)
482 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
484 box =
static_cast<uint8_t
> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] )));
486 c[box].push_back (p[i]);
489 boost::uint64_t points_added = 0;
490 for (
size_t i = 0; i < 8; i++)
496 points_added += children_[i]->addDataToLeaf (c[i],
true);
499 return (points_added);
507 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
510 assert (this->root_node_->m_tree_ != NULL);
512 if (input_cloud->height*input_cloud->width == 0)
515 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
516 return (addDataAtMaxDepth (input_cloud,
true));
518 if( num_children_ < 8 )
519 if(hasUnloadedChildren ())
520 loadChildren (
false);
522 if( skip_bb_check ==
false )
527 std::vector < std::vector<int> > indices;
530 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
532 for(
size_t k=0; k<indices.size (); k++)
534 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
537 boost::uint64_t points_added = 0;
539 for(
size_t i=0; i<8; i++)
541 if ( indices[i].empty () )
544 if ( children_[i] ==
false )
551 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
557 points_added += children_[i]->addPointCloud (dst_cloud,
false);
561 return (points_added);
564 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
571 template<
typename ContainerT,
typename Po
intT>
void
574 assert (this->root_node_->m_tree_ != NULL);
579 BOOST_FOREACH (
const PointT& pt, p)
581 sampleBuff.push_back(pt);
589 const double percent = pow(sample_percent_,
double((this->root_node_->m_tree_->getDepth () - depth_)));
590 const uint64_t samplesize =
static_cast<uint64_t
>(percent *
static_cast<double>(sampleBuff.size()));
591 const uint64_t inputsize = sampleBuff.size();
596 insertBuff.resize(samplesize);
599 boost::mutex::scoped_lock lock(rng_mutex_);
600 boost::uniform_int<boost::uint64_t> buffdist(0, inputsize-1);
601 boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie(
rand_gen_, buffdist);
604 for(boost::uint64_t i = 0; i < samplesize; ++i)
606 boost::uint64_t buffstart = buffdie();
607 insertBuff[i] = ( sampleBuff[buffstart] );
613 boost::mutex::scoped_lock lock(rng_mutex_);
614 boost::bernoulli_distribution<double> buffdist(percent);
615 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin(
rand_gen_, buffdist);
617 for(boost::uint64_t i = 0; i < inputsize; ++i)
619 insertBuff.push_back( p[i] );
624 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
627 assert (this->root_node_->m_tree_ != NULL);
633 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
636 payload_->insertRange ( p );
645 const size_t len = p.size ();
647 for (
size_t i = 0; i < len; i++)
651 buff.push_back (p[i]);
657 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
658 payload_->insertRange ( buff );
661 return (buff.size ());
665 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
669 if(skip_bb_check ==
true)
671 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
673 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
674 payload_->insertRange (input_cloud);
676 return (input_cloud->width*input_cloud->height);
680 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
687 template<
typename ContainerT,
typename Po
intT>
void
692 for(
size_t i = 0; i < 8; i++)
693 c[i].reserve(p.size() / 8);
695 const size_t len = p.size();
696 for(
size_t i = 0; i < len; i++)
704 subdividePoint (pt, c);
709 template<
typename ContainerT,
typename Po
intT>
void
712 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
714 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
715 c[octant].push_back (point);
719 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
722 boost::uint64_t points_added = 0;
724 if ( input_cloud->width * input_cloud->height == 0 )
729 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
731 uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
732 assert (points_added > 0);
733 return (points_added);
736 if (num_children_ < 8 )
738 if ( hasUnloadedChildren () )
740 loadChildren (
false);
753 uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
754 random_sampler.
setSample (static_cast<unsigned int> (sample_size));
760 pcl::IndicesPtr downsampled_cloud_indices (
new std::vector< int > () );
761 random_sampler.
filter (*downsampled_cloud_indices);
766 extractor.
setIndices (downsampled_cloud_indices);
767 extractor.
filter (*downsampled_cloud);
772 extractor.
filter (*remaining_points);
774 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 );
777 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
779 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
780 payload_->insertRange (downsampled_cloud);
781 points_added += downsampled_cloud->width*downsampled_cloud->height ;
784 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
787 std::vector<std::vector<int> > indices;
790 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
793 for(
size_t i=0; i<8; i++)
796 if(indices[i].empty ())
799 if( children_[i] ==
false )
810 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
811 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);
814 assert (points_added == input_cloud->width*input_cloud->height);
815 return (points_added);
819 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
828 assert (this->root_node_->m_tree_ != NULL );
830 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
832 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
833 return (addDataAtMaxDepth(p,
false));
837 if (this->hasUnloadedChildren ())
838 loadChildren (
false );
842 randomSample(p, insertBuff, skip_bb_check);
844 if(!insertBuff.empty())
847 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
849 payload_->insertRange (insertBuff);
854 std::vector<AlignedPointTVector> c;
855 subdividePoints(p, c, skip_bb_check);
857 boost::uint64_t points_added = 0;
858 for(
size_t i = 0; i < 8; i++)
869 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
873 return (points_added);
877 template<
typename ContainerT,
typename Po
intT>
void
883 if (children_[idx] || (num_children_ == 8))
885 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
889 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
890 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0);
892 Eigen::Vector3d childbb_min;
893 Eigen::Vector3d childbb_max;
898 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
899 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
904 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
905 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
909 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
910 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
912 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
913 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
915 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
916 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
918 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (idx));
925 template<
typename ContainerT,
typename Po
intT>
bool
926 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
928 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
929 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
930 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
940 template<
typename ContainerT,
typename Po
intT>
bool
943 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
944 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
946 if (((min[0] <= p.x) && (p.x < max[0])) &&
947 ((min[1] <= p.y) && (p.y < max[1])) &&
948 ((min[2] <= p.z) && (p.z < max[2])))
957 template<
typename ContainerT,
typename Po
intT>
void
962 node_metadata_->getBoundingBox (min, max);
964 if (this->depth_ < query_depth){
965 for (
size_t i = 0; i < this->depth_; i++)
968 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
969 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
970 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
971 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
973 if (num_children_ > 0)
975 for (
size_t i = 0; i < 8; i++)
978 children_[i]->printBoundingBox (query_depth);
985 template<
typename ContainerT,
typename Po
intT>
void
988 if (this->depth_ < query_depth){
989 if (num_children_ > 0)
991 for (
size_t i = 0; i < 8; i++)
994 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1001 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
1002 voxel_center.x =
static_cast<float>(mid_xyz[0]);
1003 voxel_center.y =
static_cast<float>(mid_xyz[1]);
1004 voxel_center.z =
static_cast<float>(mid_xyz[2]);
1006 voxel_centers.push_back(voxel_center);
1062 template<
typename Container,
typename Po
intT>
void
1065 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1068 template<
typename Container,
typename Po
intT>
void
1072 enum {INSIDE, INTERSECT, OUTSIDE};
1074 int result = INSIDE;
1076 if (this->depth_ > query_depth)
1084 if (!skip_vfc_check)
1086 for(
int i =0; i < 6; i++){
1087 double a = planes[(i*4)];
1088 double b = planes[(i*4)+1];
1089 double c = planes[(i*4)+2];
1090 double d = planes[(i*4)+3];
1094 Eigen::Vector3d normal(a, b, c);
1096 Eigen::Vector3d min_bb;
1097 Eigen::Vector3d max_bb;
1098 node_metadata_->getBoundingBox(min_bb, max_bb);
1101 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1102 Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
1103 fabs (static_cast<double> (max_bb.y () - center.y ())),
1104 fabs (static_cast<double> (max_bb.z () - center.z ())));
1106 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1107 double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
1114 if (m - n < 0) result = INTERSECT;
1165 if (result == OUTSIDE)
1183 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1186 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1189 if (hasUnloadedChildren ())
1191 loadChildren (
false);
1194 if (this->getNumChildren () > 0)
1196 for (
size_t i = 0; i < 8; i++)
1199 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1217 template<
typename Container,
typename Po
intT>
void
1222 if (this->depth_ > query_depth)
1228 Eigen::Vector3d min_bb;
1229 Eigen::Vector3d max_bb;
1230 node_metadata_->getBoundingBox(min_bb, max_bb);
1233 enum {INSIDE, INTERSECT, OUTSIDE};
1235 int result = INSIDE;
1237 if (!skip_vfc_check)
1239 for(
int i =0; i < 6; i++){
1240 double a = planes[(i*4)];
1241 double b = planes[(i*4)+1];
1242 double c = planes[(i*4)+2];
1243 double d = planes[(i*4)+3];
1247 Eigen::Vector3d normal(a, b, c);
1250 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1251 Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())),
1252 fabs (static_cast<double> (max_bb.y () - center.y ())),
1253 fabs (static_cast<double> (max_bb.z () - center.z ())));
1255 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1256 double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c));
1263 if (m - n < 0) result = INTERSECT;
1268 if (result == OUTSIDE)
1303 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1306 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1310 if (coverage <= 10000)
1313 if (hasUnloadedChildren ())
1315 loadChildren (
false);
1318 if (this->getNumChildren () > 0)
1320 for (
size_t i = 0; i < 8; i++)
1323 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1329 template<
typename ContainerT,
typename Po
intT>
void
1332 if (this->depth_ < query_depth){
1333 if (num_children_ > 0)
1335 for (
size_t i = 0; i < 8; i++)
1338 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1344 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1345 voxel_centers.push_back(voxel_center);
1352 template<
typename ContainerT,
typename Po
intT>
void
1356 Eigen::Vector3d my_min = min_bb;
1357 Eigen::Vector3d my_max = max_bb;
1359 if (intersectsWithBoundingBox (my_min, my_max))
1361 if (this->depth_ < query_depth)
1363 if (this->getNumChildren () > 0)
1365 for (
size_t i = 0; i < 8; i++)
1368 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1371 else if (hasUnloadedChildren ())
1373 loadChildren (
false);
1375 for (
size_t i = 0; i < 8; i++)
1378 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1384 if (payload_->getDataSize () > 0)
1386 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1392 template<
typename ContainerT,
typename Po
intT>
void
1395 uint64_t startingSize = dst_blob->width*dst_blob->height;
1396 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1399 if (intersectsWithBoundingBox (min_bb, max_bb))
1402 if (this->depth_ < query_depth)
1405 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1406 loadChildren (
false);
1409 if (num_children_ > 0)
1412 for (
size_t i = 0; i < 8; i++)
1415 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1417 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1427 payload_->readRange (0, payload_->size (), tmp_blob);
1429 if( tmp_blob->width*tmp_blob->height == 0 )
1433 if (inBoundingBox (min_bb, max_bb))
1439 if( dst_blob->width*dst_blob->height != 0 )
1441 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1442 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1447 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1452 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1454 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1456 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1471 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1473 Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f);
1474 Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f );
1476 std::vector<int> indices;
1479 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1480 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1482 if ( indices.size () > 0 )
1484 if( dst_blob->width*dst_blob->height > 0 )
1491 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1492 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1494 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1495 boost::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1496 (void)orig_points_in_destination;
1500 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1506 assert ( dst_blob->width*dst_blob->height == indices.size () );
1513 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1516 template<
typename ContainerT,
typename Po
intT>
void
1521 if (intersectsWithBoundingBox (min_bb, max_bb))
1524 if (this->depth_ < query_depth)
1527 if (this->hasUnloadedChildren ())
1529 this->loadChildren (
false);
1533 if (getNumChildren () > 0)
1535 if(hasUnloadedChildren ())
1536 loadChildren (
false);
1539 for (
size_t i = 0; i < 8; i++)
1542 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1551 if (inBoundingBox (min_bb, max_bb))
1555 payload_->readRange (0, payload_->size (), payload_cache);
1556 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1566 payload_->readRange (0, payload_->size (), payload_cache);
1568 uint64_t len = payload_->size ();
1570 for (uint64_t i = 0; i < len; i++)
1572 const PointT& p = payload_cache[i];
1581 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]);
1590 template<
typename ContainerT,
typename Po
intT>
void
1593 if (intersectsWithBoundingBox (min_bb, max_bb))
1595 if (this->depth_ < query_depth)
1597 if (this->hasUnloadedChildren ())
1598 this->loadChildren (
false);
1600 if (this->getNumChildren () > 0)
1602 for (
size_t i=0; i<8; i++)
1605 if (children_[i]!=0)
1606 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1615 if (inBoundingBox (min_bb, max_bb))
1618 this->payload_->read (tmp_blob);
1619 uint64_t num_pts = tmp_blob->width*tmp_blob->height;
1621 double sample_points =
static_cast<double>(num_pts) * percent;
1625 sample_points = sample_points > 0 ? sample_points : 1;
1639 random_sampler.
setSample (static_cast<unsigned int> (sample_points));
1644 random_sampler.
filter (*downsampled_cloud_indices);
1645 extractor.
setIndices (downsampled_cloud_indices);
1646 extractor.
filter (*downsampled_points);
1657 template<
typename ContainerT,
typename Po
intT>
void
1661 if (intersectsWithBoundingBox (min_bb, max_bb))
1664 if (this->depth_ < query_depth)
1667 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1669 loadChildren (
false);
1672 if (num_children_ > 0)
1675 for (
size_t i = 0; i < 8; i++)
1678 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1687 if (inBoundingBox (min_bb, max_bb))
1691 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1692 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1702 payload_->readRange (0, payload_->size (), payload_cache);
1703 for (
size_t i = 0; i < payload_->size (); i++)
1705 const PointT& p = payload_cache[i];
1708 payload_cache_within_region.push_back (p);
1714 std::random_shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end ());
1715 size_t numpick =
static_cast<size_t> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1717 for (
size_t i = 0; i < numpick; i++)
1719 dst.push_back (payload_cache_within_region[i]);
1728 template<
typename ContainerT,
typename Po
intT>
1736 , num_loaded_children_ (0)
1745 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1746 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1759 std::string uuid_idx;
1760 std::string uuid_cont;
1766 std::string node_container_name;
1769 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1773 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1781 template<
typename ContainerT,
typename Po
intT>
void
1784 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1786 loadChildren (
false);
1789 for (
size_t i = 0; i < num_children_; i++)
1791 children_[i]->copyAllCurrentAndChildPointsRec (v);
1795 payload_->readRange (0, payload_->size (), payload_cache);
1799 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1805 template<
typename ContainerT,
typename Po
intT>
void
1808 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1810 loadChildren (
false);
1813 for (
size_t i = 0; i < 8; i++)
1816 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1819 std::vector<PointT> payload_cache;
1820 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1822 for (
size_t i = 0; i < payload_cache.size (); i++)
1824 v.push_back (payload_cache[i]);
1830 template<
typename ContainerT,
typename Po
intT>
inline bool
1833 Eigen::Vector3d min, max;
1834 node_metadata_->getBoundingBox (min, max);
1837 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1839 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1841 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1852 template<
typename ContainerT,
typename Po
intT>
inline bool
1855 Eigen::Vector3d min, max;
1857 node_metadata_->getBoundingBox (min, max);
1859 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1861 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1863 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1874 template<
typename ContainerT,
typename Po
intT>
inline bool
1879 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1881 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1883 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1894 template<
typename ContainerT,
typename Po
intT>
void
1897 Eigen::Vector3d min;
1898 Eigen::Vector3d max;
1899 node_metadata_->getBoundingBox (min, max);
1901 double l = max[0] - min[0];
1902 double h = max[1] - min[1];
1903 double w = max[2] - min[2];
1904 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1905 <<
", width=" << w <<
" )\n";
1907 for (
size_t i = 0; i < num_children_; i++)
1909 children_[i]->writeVPythonVisual (file);
1915 template<
typename ContainerT,
typename Po
intT>
int
1918 return (this->payload_->read (output_cloud));
1926 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1927 return (children_[index_arg]);
1931 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
1934 return (this->payload_->getDataSize ());
1939 template<
typename ContainerT,
typename Po
intT>
size_t
1942 size_t loaded_children_count = 0;
1944 for (
size_t i=0; i<8; i++)
1946 if (children_[i] != 0)
1947 loaded_children_count++;
1950 return (loaded_children_count);
1955 template<
typename ContainerT,
typename Po
intT>
void
1958 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1959 node_metadata_->loadMetadataFromDisk (path);
1962 this->parent_ = super;
1964 if (num_children_ > 0)
1967 this->num_children_ = 0;
1968 this->payload_ = boost::shared_ptr<ContainerT> (
new ContainerT (node_metadata_->getPCDFilename ()));
1973 template<
typename ContainerT,
typename Po
intT>
void
1976 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1977 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1978 payload_->convertToXYZ (xyzfile);
1980 if (hasUnloadedChildren ())
1982 loadChildren (
false);
1985 for (
size_t i = 0; i < 8; i++)
1988 children_[i]->convertToXYZ ();
1994 template<
typename ContainerT,
typename Po
intT>
void
1997 for (
size_t i = 0; i < 8; i++)
2000 children_[i]->flushToDiskRecursive ();
2006 template<
typename ContainerT,
typename Po
intT>
void
2009 if (indices.size () < 8)
2016 int x_offset = input_cloud->fields[x_idx].offset;
2017 int y_offset = input_cloud->fields[y_idx].offset;
2018 int z_offset = input_cloud->fields[z_idx].offset;
2020 for (
size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
2024 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
2025 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
2026 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
2028 if (!pcl_isfinite (local_pt.x) || !pcl_isfinite (local_pt.y) || !pcl_isfinite (local_pt.z))
2033 PCL_ERROR (
"pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z);
2040 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2044 indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
2049 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2058 thisnode->thisdir_ = path.parent_path ();
2060 if (!boost::filesystem::exists (thisnode->thisdir_))
2062 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2063 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2066 thisnode->thisnodeindex_ = path;
2073 thisnode->thisdir_ = path;
2077 if (thisnode->
depth_ > thisnode->root->max_depth_)
2079 thisnode->root->max_depth_ = thisnode->
depth_;
2082 boost::filesystem::directory_iterator diterend;
2083 bool loaded =
false;
2084 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2086 const boost::filesystem::path& file = *diter;
2087 if (!boost::filesystem::is_directory (file))
2089 if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
2091 thisnode->thisnodeindex_ = file;
2100 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2101 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2105 thisnode->max_depth_ = 0;
2108 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2110 f >> thisnode->min_[0];
2111 f >> thisnode->min_[1];
2112 f >> thisnode->min_[2];
2113 f >> thisnode->max_[0];
2114 f >> thisnode->max_[1];
2115 f >> thisnode->max_[2];
2117 std::string filename;
2119 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2123 thisnode->
payload_ = boost::shared_ptr<ContainerT> (
new ContainerT (thisnode->thisnodestorage_));
2128 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2137 template<
typename ContainerT,
typename Po
intT>
void
2138 queryBBIntersects_noload (
const boost::filesystem::path& root_node,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const boost::uint32_t query_depth, std::list<std::string>& bin_name)
2140 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2143 std::cout <<
"test";
2145 if (root->intersectsWithBoundingBox (min, max))
2147 if (query_depth == root->max_depth_)
2149 if (!root->payload_->empty ())
2151 bin_name.push_back (root->thisnodestorage_.string ());
2156 for (
int i = 0; i < 8; i++)
2158 boost::filesystem::path child_dir = root->thisdir_
2159 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2160 if (boost::filesystem::exists (child_dir))
2163 root->num_children_++;
2173 template<
typename ContainerT,
typename Po
intT>
void
2174 queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const boost::uint32_t query_depth, std::list<std::string>& bin_name)
2176 if (current->intersectsWithBoundingBox (min, max))
2178 if (current->depth_ == query_depth)
2180 if (!current->payload_->empty ())
2182 bin_name.push_back (current->thisnodestorage_.string ());
2187 for (
int i = 0; i < 8; i++)
2189 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2190 if (boost::filesystem::exists (child_dir))
2192 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2193 current->num_children_++;
2208 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
uint64_t num_children_
Number of children on disk.
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, std::vector< int > &indices)
Get a set of points residing in a box given its bounds.
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...
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
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) ...
void flushToDiskRecursive()
A base class for all pcl exceptions which inherits from std::runtime_error.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
boost::shared_ptr< std::vector< int > > IndicesPtr
boost::shared_ptr< PointCloud< PointT > > Ptr
RandomSample applies a random sampling with uniform probability.
virtual void filter(PCLPointCloud2 &output)
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const boost::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 setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
uint32_t width
The point cloud width (if organized as an image-structure).
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
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...
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
virtual ~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
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.
virtual void queryBBIncludes_subsample(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::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 std::string node_index_basename
void recFreeChildren()
Method which recursively free children of this node.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const size_t query_depth)
Gets a vector of occupied voxel centers.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
boost::shared_ptr< ::pcl::PCLPointCloud2 > Ptr
virtual boost::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. ...
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
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.
virtual size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
void createChild(const std::size_t idx)
Creates child node idx.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
virtual OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
boost::shared_ptr< ContainerT > payload_
what holds the points.
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
virtual boost::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
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)
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector< int > > &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...
static const std::string node_index_extension
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.
virtual size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
static const std::string node_container_basename
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void queryBBIntersects_noload(const boost::filesystem::path &root_node, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::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 saveIdx(bool recursive)
Save node's metadata to file.
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the octree_disk_container class or octree_ram_container class, whichever it is templated against.
OutofcoreOctreeBaseNode * parent_
super-node
boost::uuids::random_generator OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
virtual boost::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...
PCL_EXPORTS bool concatenatePointCloud(const pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2, pcl::PCLPointCloud2 &cloud_out)
Concatenate two pcl::PCLPointCloud2.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
boost::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
virtual boost::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...
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
virtual size_t getDepth() const
A point structure representing Euclidean xyz coordinates, and the RGB color.
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...
This code defines the octree used for point storage at Urban Robotics.
void setSample(unsigned int sample)
Set number of indices to be sampled.
size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
virtual boost::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
uint32_t height
The point cloud height (if organized as an image-structure).