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>
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>
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);
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;
249 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
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 290 for (
size_t i = 0; i < 8; i++)
300 template<
typename ContainerT,
typename Po
intT>
bool 310 template<
typename ContainerT,
typename Po
intT>
void 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)
334 template<
typename ContainerT,
typename Po
intT>
void 342 for (
size_t i = 0; i < 8; i++)
355 template<
typename ContainerT,
typename Po
intT> uint64_t
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__ );
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
429 payload_->insertRange (p.data (), p.size ());
435 std::vector<const PointT*> buff;
436 BOOST_FOREACH(
const PointT* pt, p)
447 payload_->insertRange (buff.data (), buff.size ());
451 return (buff.size ());
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++)
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
512 if (input_cloud->height*input_cloud->width == 0)
522 if( skip_bb_check ==
false )
527 std::vector < std::vector<int> > indices;
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 () )
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 579 BOOST_FOREACH (
const PointT& pt, p)
581 sampleBuff.push_back(pt);
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);
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] );
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
645 const size_t len = p.size ();
647 for (
size_t i = 0; i < len; i++)
651 buff.push_back (p[i]);
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_);
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++)
709 template<
typename ContainerT,
typename Po
intT>
void 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 )
732 assert (points_added > 0);
733 return (points_added);
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;
793 for(
size_t i=0; i<8; i++)
796 if(indices[i].empty ())
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
832 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
844 if(!insertBuff.empty())
854 std::vector<AlignedPointTVector> c;
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 885 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->
node_metadata_->getMetadataFilename ().c_str ());
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 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;
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){
991 for (
size_t i = 0; i < 8; i++)
994 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
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 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;
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)
1186 file_names.push_back (this->
node_metadata_->getMetadataFilename ().string ());
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;
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);
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)
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){
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;
1361 if (this->
depth_ < query_depth)
1365 for (
size_t i = 0; i < 8; i++)
1368 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1375 for (
size_t i = 0; i < 8; i++)
1378 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
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 );
1402 if (this->
depth_ < query_depth)
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 );
1429 if( tmp_blob->width*tmp_blob->height == 0 )
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 1524 if (this->
depth_ < query_depth)
1539 for (
size_t i = 0; i < 8; i++)
1542 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1556 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
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 1595 if (this->
depth_ < query_depth)
1602 for (
size_t i=0; i<8; i++)
1606 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
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 1664 if (this->
depth_ < query_depth)
1675 for (
size_t i = 0; i < 8; i++)
1678 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1691 payload_->readRangeSubSample (0,
payload_->size (), percent, payload_cache);
1692 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
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>
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 1791 children_[i]->copyAllCurrentAndChildPointsRec (v);
1799 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1805 template<
typename ContainerT,
typename Po
intT>
void 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;
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;
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;
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";
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);
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++)
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 ());
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;
1985 for (
size_t i = 0; i < 8; i++)
1994 template<
typename ContainerT,
typename Po
intT>
void 1997 for (
size_t i = 0; i < 8; i++)
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_));
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)
2143 std::cout <<
"test";
2147 if (query_depth == root->max_depth_)
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))
2173 template<
typename ContainerT,
typename Po
intT>
void 2178 if (current->
depth_ == query_depth)
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);
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.
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.
boost::uint64_t size() const
Number of points in the payload.
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...
friend 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...
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)
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_
std::vector< OutofcoreOctreeBaseNode * > children_
The children of this node.
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).
friend void queryBBIntersects_noload(const boost::filesystem::path &rootnode, 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 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 saveIdx(bool recursive)
Save node's metadata to file.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual size_t getNumLoadedChildren() const
Count loaded chilren.
OutofcoreOctreeBaseNode Class internally representing nodes of an outofcore octree, with accessors to its data via the pcl::outofcore::OutofcoreOctreeDiskContainer class or pcl::outofcore::OutofcoreOctreeRamContainer class, whichever it is templated against.
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.
virtual size_t getNumChildren() const
Returns the total number of children on disk.
uint64_t num_loaded_children_
Number of loaded children this node has.
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_