41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_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>
68 template<
typename ContainerT,
typename Po
intT>
71 template<
typename ContainerT,
typename Po
intT>
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>
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] == 0)
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] == 0)
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 > 1 ? sample_points : 1;
1639 random_sampler.
setSample (static_cast<unsigned int> (sample_points));
1645 random_sampler.
filter (*downsampled_cloud_indices);
1646 extractor.
setIndices (downsampled_cloud_indices);
1647 extractor.
filter (*downsampled_points);
1658 template<
typename ContainerT,
typename Po
intT>
void 1662 if (intersectsWithBoundingBox (min_bb, max_bb))
1665 if (this->depth_ < query_depth)
1668 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1670 loadChildren (
false);
1673 if (num_children_ > 0)
1676 for (
size_t i = 0; i < 8; i++)
1679 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1688 if (inBoundingBox (min_bb, max_bb))
1692 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1693 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1703 payload_->readRange (0, payload_->size (), payload_cache);
1704 for (
size_t i = 0; i < payload_->size (); i++)
1706 const PointT& p = payload_cache[i];
1709 payload_cache_within_region.push_back (p);
1715 std::random_shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end ());
1716 size_t numpick = static_cast<size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));;
1718 for (
size_t i = 0; i < numpick; i++)
1720 dst.push_back (payload_cache_within_region[i]);
1729 template<
typename ContainerT,
typename Po
intT>
1737 , num_loaded_children_ (0)
1746 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1747 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1760 std::string uuid_idx;
1761 std::string uuid_cont;
1767 std::string node_container_name;
1770 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1774 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1782 template<
typename ContainerT,
typename Po
intT>
void 1785 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1787 loadChildren (
false);
1790 for (
size_t i = 0; i < num_children_; i++)
1792 children_[i]->copyAllCurrentAndChildPointsRec (v);
1796 payload_->readRange (0, payload_->size (), payload_cache);
1800 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1806 template<
typename ContainerT,
typename Po
intT>
void 1809 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1811 loadChildren (
false);
1814 for (
size_t i = 0; i < 8; i++)
1817 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1820 std::vector<PointT> payload_cache;
1821 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1823 for (
size_t i = 0; i < payload_cache.size (); i++)
1825 v.push_back (payload_cache[i]);
1831 template<
typename ContainerT,
typename Po
intT>
inline bool 1834 Eigen::Vector3d min, max;
1835 node_metadata_->getBoundingBox (min, max);
1838 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1840 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1842 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1853 template<
typename ContainerT,
typename Po
intT>
inline bool 1856 Eigen::Vector3d min, max;
1858 node_metadata_->getBoundingBox (min, max);
1860 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1862 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1864 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1875 template<
typename ContainerT,
typename Po
intT>
inline bool 1880 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1882 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1884 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1895 template<
typename ContainerT,
typename Po
intT>
void 1898 Eigen::Vector3d min;
1899 Eigen::Vector3d max;
1900 node_metadata_->getBoundingBox (min, max);
1902 double l = max[0] - min[0];
1903 double h = max[1] - min[1];
1904 double w = max[2] - min[2];
1905 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1906 <<
", width=" << w <<
" )\n";
1908 for (
size_t i = 0; i < num_children_; i++)
1910 children_[i]->writeVPythonVisual (file);
1916 template<
typename ContainerT,
typename Po
intT>
int 1919 return (this->payload_->read (output_cloud));
1927 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1928 return (children_[index_arg]);
1932 template<
typename ContainerT,
typename Po
intT> boost::uint64_t
1935 return (this->payload_->getDataSize ());
1940 template<
typename ContainerT,
typename Po
intT>
size_t 1943 size_t loaded_children_count = 0;
1945 for (
size_t i=0; i<8; i++)
1947 if (children_[i] != 0)
1948 loaded_children_count++;
1951 return (loaded_children_count);
1956 template<
typename ContainerT,
typename Po
intT>
void 1959 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1960 node_metadata_->loadMetadataFromDisk (path);
1963 this->parent_ = super;
1965 if (num_children_ > 0)
1968 this->num_children_ = 0;
1969 this->payload_ = boost::shared_ptr<ContainerT> (
new ContainerT (node_metadata_->getPCDFilename ()));
1974 template<
typename ContainerT,
typename Po
intT>
void 1977 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1978 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1979 payload_->convertToXYZ (xyzfile);
1981 if (hasUnloadedChildren ())
1983 loadChildren (
false);
1986 for (
size_t i = 0; i < 8; i++)
1989 children_[i]->convertToXYZ ();
1995 template<
typename ContainerT,
typename Po
intT>
void 1998 for (
size_t i = 0; i < 8; i++)
2001 children_[i]->flushToDiskRecursive ();
2007 template<
typename ContainerT,
typename Po
intT>
void 2010 if (indices.size () < 8)
2017 int x_offset = input_cloud->fields[x_idx].offset;
2018 int y_offset = input_cloud->fields[y_idx].offset;
2019 int z_offset = input_cloud->fields[z_idx].offset;
2021 for (
size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
2025 local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset]));
2026 local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset]));
2027 local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset]));
2029 if (!pcl_isfinite (local_pt.x) || !pcl_isfinite (local_pt.y) || !pcl_isfinite (local_pt.z))
2034 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);
2041 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2045 indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step));
2050 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated 2059 thisnode->thisdir_ = path.parent_path ();
2061 if (!boost::filesystem::exists (thisnode->thisdir_))
2063 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2064 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2067 thisnode->thisnodeindex_ = path;
2074 thisnode->thisdir_ = path;
2078 if (thisnode->
depth_ > thisnode->root->max_depth_)
2080 thisnode->root->max_depth_ = thisnode->
depth_;
2083 boost::filesystem::directory_iterator diterend;
2084 bool loaded =
false;
2085 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2087 const boost::filesystem::path& file = *diter;
2088 if (!boost::filesystem::is_directory (file))
2092 thisnode->thisnodeindex_ = file;
2101 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2102 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2106 thisnode->max_depth_ = 0;
2109 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2111 f >> thisnode->min_[0];
2112 f >> thisnode->min_[1];
2113 f >> thisnode->min_[2];
2114 f >> thisnode->max_[0];
2115 f >> thisnode->max_[1];
2116 f >> thisnode->max_[2];
2118 std::string filename;
2120 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2124 thisnode->
payload_ = boost::shared_ptr<ContainerT> (
new ContainerT (thisnode->thisnodestorage_));
2129 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2138 template<
typename ContainerT,
typename Po
intT>
void 2139 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)
2141 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2144 std::cout <<
"test";
2146 if (root->intersectsWithBoundingBox (min, max))
2148 if (query_depth == root->max_depth_)
2150 if (!root->payload_->empty ())
2152 bin_name.push_back (root->thisnodestorage_.string ());
2157 for (
int i = 0; i < 8; i++)
2159 boost::filesystem::path child_dir = root->thisdir_
2160 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2161 if (boost::filesystem::exists (child_dir))
2164 root->num_children_++;
2174 template<
typename ContainerT,
typename Po
intT>
void 2175 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)
2177 if (current->intersectsWithBoundingBox (min, max))
2179 if (current->depth_ == query_depth)
2181 if (!current->payload_->empty ())
2183 bin_name.push_back (current->thisnodestorage_.string ());
2188 for (
int i = 0; i < 8; i++)
2190 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2191 if (boost::filesystem::exists (child_dir))
2193 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2194 current->num_children_++;
2209 #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.
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.
boost::shared_ptr< std::vector< int > > IndicesPtr
RandomSample applies a random sampling with uniform probability.
This file defines compatibility wrappers for low level I/O functions.
virtual size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
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.
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.
static const double sample_percent_
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
uint32_t height
The point cloud height (if organized as an image-structure).
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 printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
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.
static boost::mutex rng_mutex_
Random number generator mutex.
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.
boost::shared_ptr< PointCloud< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
void createChild(const std::size_t idx)
Creates child node idx.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
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.
static boost::mt19937 rand_gen_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator.
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.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
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
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
boost::uuids::basic_random_generator< boost::mt19937 > OutofcoreOctreeDiskContainer< PointT >::uuid_gen_ & rand_gen_
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree,...
OutofcoreOctreeBaseNode * parent_
super-node
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.
static const std::string node_container_extension
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 OutofcoreOctreeBaseNode * getChildPtr(size_t index_arg) const
Returns a pointer to the child in octant index_arg.
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.
virtual boost::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
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.
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_.
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_