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>
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.
virtual void printBoundingBox(const size_t query_depth) const
Write the voxel size to stdout at query_depth.
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) ...
boost::uint64_t size() const
Number of points in the payload.
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
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...
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 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
uint32_t width
The point cloud width (if organized as an image-structure).
virtual size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
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)
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.
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.
virtual size_t getNumChildren() const
Returns the total number of children on disk.
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 saveIdx(bool recursive)
Save node's metadata to file.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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 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.
uint64_t num_loaded_children_
Number of loaded children this node has.
virtual size_t getNumLoadedChildren() const
Count loaded chilren.
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_