41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
53 #include <pcl/visualization/common/common.h>
54 #include <pcl/outofcore/octree_base_node.h>
55 #include <pcl/filters/random_sample.h>
56 #include <pcl/filters/extract_indices.h>
59 #include <pcl/outofcore/cJSON.h>
66 template<
typename ContainerT,
typename Po
intT>
69 template<
typename ContainerT,
typename Po
intT>
72 template<
typename ContainerT,
typename Po
intT>
75 template<
typename ContainerT,
typename Po
intT>
78 template<
typename ContainerT,
typename Po
intT>
81 template<
typename ContainerT,
typename Po
intT>
84 template<
typename ContainerT,
typename Po
intT>
87 template<
typename ContainerT,
typename Po
intT>
90 template<
typename ContainerT,
typename Po
intT>
96 , children_ (8, nullptr)
98 , num_loaded_children_ (0)
107 template<
typename ContainerT,
typename Po
intT>
113 , children_ (8, nullptr)
115 , num_loaded_children_ (0)
122 if (super ==
nullptr)
124 node_metadata_->setDirectoryPathname (directory_path.parent_path ());
130 if (!boost::filesystem::exists (
node_metadata_->getDirectoryPathname ()))
132 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n",
node_metadata_->getDirectoryPathname ().c_str ());
133 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
142 boost::filesystem::directory_iterator directory_it_end;
145 bool b_loaded =
false;
147 for (boost::filesystem::directory_iterator directory_it (
node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it)
149 const boost::filesystem::path& file = *directory_it;
151 if (!boost::filesystem::is_directory (file))
163 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
164 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
181 template<
typename ContainerT,
typename Po
intT>
187 , children_ (8, nullptr)
189 , num_loaded_children_ (0)
193 assert (tree != NULL);
200 template<
typename ContainerT,
typename Po
intT>
void
203 assert (tree != NULL);
213 Eigen::Vector3d tmp_max = bb_max;
214 Eigen::Vector3d tmp_min = bb_min;
217 double epsilon = 1e-8;
218 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0);
220 node_metadata_->setBoundingBox (tmp_min, tmp_max);
221 node_metadata_->setDirectoryPathname (root_name.parent_path ());
222 node_metadata_->setOutofcoreVersion (3);
225 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
227 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
230 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
232 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
233 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
241 std::string node_container_name;
243 node_container_name = uuid + std::string (
"_") + node_container_basename + pcd_extension;
245 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ());
246 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name));
248 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
249 node_metadata_->serializeMetadataToDisk ();
252 payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
257 template<
typename ContainerT,
typename Po
intT>
266 template<
typename ContainerT,
typename Po
intT> std::size_t
269 std::size_t child_count = 0;
271 for(std::size_t i=0; i<8; i++)
273 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
274 if (boost::filesystem::exists (child_path))
277 return (child_count);
282 template<
typename ContainerT,
typename Po
intT>
void
285 node_metadata_->serializeMetadataToDisk ();
289 for (std::size_t i = 0; i < 8; i++)
292 children_[i]->saveIdx (
true);
299 template<
typename ContainerT,
typename Po
intT>
bool
302 return (this->getNumLoadedChildren () < this->getNumChildren ());
306 template<
typename ContainerT,
typename Po
intT>
void
310 if (num_loaded_children_ < this->getNumChildren ())
313 for (
int i = 0; i < 8; i++)
315 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
317 if (boost::filesystem::exists (child_dir) && this->children_[i] ==
nullptr)
322 num_loaded_children_++;
326 assert (num_loaded_children_ == this->getNumChildren ());
330 template<
typename ContainerT,
typename Po
intT>
void
333 if (num_children_ == 0)
338 for (std::size_t i = 0; i < 8; i++)
361 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
362 return (addDataAtMaxDepth( p, skip_bb_check));
364 if (hasUnloadedChildren ())
365 loadChildren (
false);
367 std::vector < std::vector<const PointT*> > c;
369 for (std::size_t i = 0; i < 8; i++)
371 c[i].reserve (p.size () / 8);
374 const std::size_t len = p.size ();
375 for (std::size_t i = 0; i < len; i++)
383 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
389 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
391 box =
static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
392 c[
static_cast<std::size_t
>(box)].push_back (&pt);
396 for (std::size_t i = 0; i < 8; i++)
402 points_added += children_[i]->addDataToLeaf (c[i],
true);
405 return (points_added);
418 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
423 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
425 payload_->insertRange (p.data (), p.size ());
430 std::vector<const PointT*> buff;
431 for (
const PointT* pt : p)
441 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
442 payload_->insertRange (buff.data (), buff.size ());
446 return (buff.size ());
449 if (this->hasUnloadedChildren ())
451 loadChildren (
false);
454 std::vector < std::vector<const PointT*> > c;
456 for (std::size_t i = 0; i < 8; i++)
458 c[i].reserve (p.size () / 8);
461 const std::size_t len = p.size ();
462 for (std::size_t i = 0; i < len; i++)
475 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
477 box =
static_cast<std::uint8_t> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] )));
479 c[box].push_back (p[i]);
483 for (std::size_t i = 0; i < 8; i++)
489 points_added += children_[i]->addDataToLeaf (c[i],
true);
492 return (points_added);
500 assert (this->root_node_->m_tree_ != NULL);
502 if (input_cloud->height*input_cloud->width == 0)
505 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
506 return (addDataAtMaxDepth (input_cloud,
true));
508 if( num_children_ < 8 )
509 if(hasUnloadedChildren ())
510 loadChildren (
false);
517 std::vector < std::vector<int> > indices;
520 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
522 for(std::size_t k=0; k<indices.size (); k++)
524 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
529 for(std::size_t i=0; i<8; i++)
531 if ( indices[i].empty () )
534 if (children_[i] ==
nullptr)
541 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
547 points_added += children_[i]->addPointCloud (dst_cloud,
false);
551 return (points_added);
554 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
561 template<
typename ContainerT,
typename Po
intT>
void
564 assert (this->root_node_->m_tree_ != NULL);
573 sampleBuff.push_back(pt);
583 const double percent = pow(sample_percent_,
double((this->root_node_->m_tree_->getDepth () - depth_)));
590 insertBuff.resize(samplesize);
593 std::lock_guard<std::mutex> lock(rng_mutex_);
594 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
600 insertBuff[i] = ( sampleBuff[buffstart] );
606 std::lock_guard<std::mutex> lock(rng_mutex_);
607 std::bernoulli_distribution buffdist(percent);
611 insertBuff.push_back( p[i] );
619 assert (this->root_node_->m_tree_ != NULL);
625 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
628 payload_->insertRange ( p );
634 AlignedPointTVector buff;
635 const std::size_t len = p.size ();
637 for (std::size_t i = 0; i < len; i++)
641 buff.push_back (p[i]);
647 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
648 payload_->insertRange ( buff );
650 return (buff.size ());
659 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
661 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
662 payload_->insertRange (input_cloud);
664 return (input_cloud->width*input_cloud->height);
666 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
672 template<
typename ContainerT,
typename Po
intT>
void
677 for(std::size_t i = 0; i < 8; i++)
678 c[i].reserve(p.size() / 8);
680 const std::size_t len = p.size();
681 for(std::size_t i = 0; i < len; i++)
689 subdividePoint (pt, c);
694 template<
typename ContainerT,
typename Po
intT>
void
697 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
698 std::size_t octant = 0;
699 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
700 c[octant].push_back (point);
709 if ( input_cloud->width * input_cloud->height == 0 )
714 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
716 std::uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
717 assert (points_added > 0);
718 return (points_added);
721 if (num_children_ < 8 )
723 if ( hasUnloadedChildren () )
725 loadChildren (
false);
738 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
739 random_sampler.
setSample (
static_cast<unsigned int> (sample_size));
745 pcl::IndicesPtr downsampled_cloud_indices (
new std::vector< int > () );
746 random_sampler.
filter (*downsampled_cloud_indices);
751 extractor.
setIndices (downsampled_cloud_indices);
752 extractor.
filter (*downsampled_cloud);
757 extractor.
filter (*remaining_points);
759 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 );
762 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
764 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
765 payload_->insertRange (downsampled_cloud);
766 points_added += downsampled_cloud->width*downsampled_cloud->height ;
769 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
772 std::vector<std::vector<int> > indices;
775 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
778 for(std::size_t i=0; i<8; i++)
781 if(indices[i].empty ())
784 if (children_[i] ==
nullptr)
795 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
796 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);
799 assert (points_added == input_cloud->width*input_cloud->height);
800 return (points_added);
813 assert (this->root_node_->m_tree_ != NULL );
815 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
817 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
818 return (addDataAtMaxDepth(p,
false));
822 if (this->hasUnloadedChildren ())
823 loadChildren (
false );
827 randomSample(p, insertBuff, skip_bb_check);
829 if(!insertBuff.empty())
832 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
834 payload_->insertRange (insertBuff);
839 std::vector<AlignedPointTVector> c;
840 subdividePoints(p, c, skip_bb_check);
843 for(std::size_t i = 0; i < 8; i++)
854 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
858 return (points_added);
862 template<
typename ContainerT,
typename Po
intT>
void
868 if (children_[idx] || (num_children_ == 8))
870 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
874 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
875 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
877 Eigen::Vector3d childbb_min;
878 Eigen::Vector3d childbb_max;
883 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
884 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
889 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
890 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
894 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
895 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
897 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
898 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
900 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
901 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
903 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
910 template<
typename ContainerT,
typename Po
intT>
bool
911 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
913 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
914 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
915 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
925 template<
typename ContainerT,
typename Po
intT>
bool
928 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
929 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
931 if (((min[0] <= p.x) && (p.x < max[0])) &&
932 ((min[1] <= p.y) && (p.y < max[1])) &&
933 ((min[2] <= p.z) && (p.z < max[2])))
942 template<
typename ContainerT,
typename Po
intT>
void
947 node_metadata_->getBoundingBox (min, max);
949 if (this->depth_ < query_depth){
950 for (std::size_t i = 0; i < this->depth_; i++)
953 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
954 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
955 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
956 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
958 if (num_children_ > 0)
960 for (std::size_t i = 0; i < 8; i++)
963 children_[i]->printBoundingBox (query_depth);
970 template<
typename ContainerT,
typename Po
intT>
void
973 if (this->depth_ < query_depth){
974 if (num_children_ > 0)
976 for (std::size_t i = 0; i < 8; i++)
979 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
986 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
987 voxel_center.x =
static_cast<float>(mid_xyz[0]);
988 voxel_center.y =
static_cast<float>(mid_xyz[1]);
989 voxel_center.z =
static_cast<float>(mid_xyz[2]);
991 voxel_centers.push_back(voxel_center);
1047 template<
typename Container,
typename Po
intT>
void
1050 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1053 template<
typename Container,
typename Po
intT>
void
1057 enum {INSIDE, INTERSECT, OUTSIDE};
1059 int result = INSIDE;
1061 if (this->depth_ > query_depth)
1069 if (!skip_vfc_check)
1071 for(
int i =0; i < 6; i++){
1072 double a = planes[(i*4)];
1073 double b = planes[(i*4)+1];
1074 double c = planes[(i*4)+2];
1075 double d = planes[(i*4)+3];
1079 Eigen::Vector3d normal(a, b, c);
1081 Eigen::Vector3d min_bb;
1082 Eigen::Vector3d max_bb;
1083 node_metadata_->getBoundingBox(min_bb, max_bb);
1086 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1087 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1088 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1089 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1091 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1092 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1099 if (m - n < 0) result = INTERSECT;
1150 if (result == OUTSIDE)
1168 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1171 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1174 if (hasUnloadedChildren ())
1176 loadChildren (
false);
1179 if (this->getNumChildren () > 0)
1181 for (std::size_t i = 0; i < 8; i++)
1184 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1202 template<
typename Container,
typename Po
intT>
void
1207 if (this->depth_ > query_depth)
1213 Eigen::Vector3d min_bb;
1214 Eigen::Vector3d max_bb;
1215 node_metadata_->getBoundingBox(min_bb, max_bb);
1218 enum {INSIDE, INTERSECT, OUTSIDE};
1220 int result = INSIDE;
1222 if (!skip_vfc_check)
1224 for(
int i =0; i < 6; i++){
1225 double a = planes[(i*4)];
1226 double b = planes[(i*4)+1];
1227 double c = planes[(i*4)+2];
1228 double d = planes[(i*4)+3];
1232 Eigen::Vector3d normal(a, b, c);
1235 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1236 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1237 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1238 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1240 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1241 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1248 if (m - n < 0) result = INTERSECT;
1253 if (result == OUTSIDE)
1288 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1291 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1295 if (coverage <= 10000)
1298 if (hasUnloadedChildren ())
1300 loadChildren (
false);
1303 if (this->getNumChildren () > 0)
1305 for (std::size_t i = 0; i < 8; i++)
1308 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1314 template<
typename ContainerT,
typename Po
intT>
void
1317 if (this->depth_ < query_depth){
1318 if (num_children_ > 0)
1320 for (std::size_t i = 0; i < 8; i++)
1323 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1329 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1330 voxel_centers.push_back(voxel_center);
1337 template<
typename ContainerT,
typename Po
intT>
void
1341 Eigen::Vector3d my_min = min_bb;
1342 Eigen::Vector3d my_max = max_bb;
1344 if (intersectsWithBoundingBox (my_min, my_max))
1346 if (this->depth_ < query_depth)
1348 if (this->getNumChildren () > 0)
1350 for (std::size_t i = 0; i < 8; i++)
1353 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1356 else if (hasUnloadedChildren ())
1358 loadChildren (
false);
1360 for (std::size_t i = 0; i < 8; i++)
1363 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1369 if (payload_->getDataSize () > 0)
1371 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1377 template<
typename ContainerT,
typename Po
intT>
void
1380 std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1381 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1384 if (intersectsWithBoundingBox (min_bb, max_bb))
1387 if (this->depth_ < query_depth)
1390 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1391 loadChildren (
false);
1394 if (num_children_ > 0)
1397 for (std::size_t i = 0; i < 8; i++)
1400 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1402 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1412 payload_->readRange (0, payload_->size (), tmp_blob);
1414 if( tmp_blob->width*tmp_blob->height == 0 )
1418 if (inBoundingBox (min_bb, max_bb))
1424 if( dst_blob->width*dst_blob->height != 0 )
1426 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1427 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1432 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1437 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1439 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1441 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1452 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1454 Eigen::Vector4f min_pt (
static_cast<float> ( min_bb[0] ),
static_cast<float> ( min_bb[1] ),
static_cast<float> ( min_bb[2] ), 1.0f);
1455 Eigen::Vector4f max_pt (
static_cast<float> ( max_bb[0] ),
static_cast<float> ( max_bb[1] ) ,
static_cast<float>( max_bb[2] ), 1.0f );
1457 std::vector<int> indices;
1460 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1461 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1463 if ( !indices.empty () )
1465 if( dst_blob->width*dst_blob->height > 0 )
1472 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1473 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1475 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1476 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1477 (void)orig_points_in_destination;
1481 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1487 assert ( dst_blob->width*dst_blob->height == indices.size () );
1493 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1496 template<
typename ContainerT,
typename Po
intT>
void
1501 if (intersectsWithBoundingBox (min_bb, max_bb))
1504 if (this->depth_ < query_depth)
1507 if (this->hasUnloadedChildren ())
1509 this->loadChildren (
false);
1513 if (getNumChildren () > 0)
1515 if(hasUnloadedChildren ())
1516 loadChildren (
false);
1519 for (std::size_t i = 0; i < 8; i++)
1522 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1531 if (inBoundingBox (min_bb, max_bb))
1534 AlignedPointTVector payload_cache;
1535 payload_->readRange (0, payload_->size (), payload_cache);
1536 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1543 AlignedPointTVector payload_cache;
1544 payload_->readRange (0, payload_->size (), payload_cache);
1550 const PointT& p = payload_cache[i];
1559 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]);
1567 template<
typename ContainerT,
typename Po
intT>
void
1570 if (intersectsWithBoundingBox (min_bb, max_bb))
1572 if (this->depth_ < query_depth)
1574 if (this->hasUnloadedChildren ())
1575 this->loadChildren (
false);
1577 if (this->getNumChildren () > 0)
1579 for (std::size_t i=0; i<8; i++)
1582 if (children_[i]!=
nullptr)
1583 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1592 if (inBoundingBox (min_bb, max_bb))
1595 this->payload_->read (tmp_blob);
1598 double sample_points =
static_cast<double>(num_pts) * percent;
1602 sample_points = sample_points > 1 ? sample_points : 1;
1616 random_sampler.
setSample (
static_cast<unsigned int> (sample_points));
1622 random_sampler.
filter (*downsampled_cloud_indices);
1623 extractor.
setIndices (downsampled_cloud_indices);
1624 extractor.
filter (*downsampled_points);
1635 template<
typename ContainerT,
typename Po
intT>
void
1639 if (intersectsWithBoundingBox (min_bb, max_bb))
1642 if (this->depth_ < query_depth)
1645 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1647 loadChildren (
false);
1650 if (num_children_ > 0)
1653 for (std::size_t i = 0; i < 8; i++)
1656 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1665 if (inBoundingBox (min_bb, max_bb))
1668 AlignedPointTVector payload_cache;
1669 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1670 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1675 AlignedPointTVector payload_cache_within_region;
1677 AlignedPointTVector payload_cache;
1678 payload_->readRange (0, payload_->size (), payload_cache);
1679 for (std::size_t i = 0; i < payload_->size (); i++)
1681 const PointT& p = payload_cache[i];
1684 payload_cache_within_region.push_back (p);
1690 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1691 std::size_t numpick =
static_cast<std::size_t
> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1693 for (std::size_t i = 0; i < numpick; i++)
1695 dst.push_back (payload_cache_within_region[i]);
1703 template<
typename ContainerT,
typename Po
intT>
1709 , children_ (8, nullptr)
1711 , num_loaded_children_ (0)
1717 if (super ==
nullptr)
1719 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1720 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1733 std::string uuid_idx;
1734 std::string uuid_cont;
1740 std::string node_container_name;
1743 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1747 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1755 template<
typename ContainerT,
typename Po
intT>
void
1758 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1760 loadChildren (
false);
1763 for (std::size_t i = 0; i < num_children_; i++)
1765 children_[i]->copyAllCurrentAndChildPointsRec (v);
1769 payload_->readRange (0, payload_->size (), payload_cache);
1772 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1778 template<
typename ContainerT,
typename Po
intT>
void
1781 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1783 loadChildren (
false);
1786 for (std::size_t i = 0; i < 8; i++)
1789 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1792 std::vector<PointT> payload_cache;
1793 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1795 for (std::size_t i = 0; i < payload_cache.size (); i++)
1797 v.push_back (payload_cache[i]);
1803 template<
typename ContainerT,
typename Po
intT>
inline bool
1806 Eigen::Vector3d min, max;
1807 node_metadata_->getBoundingBox (min, max);
1810 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1812 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1814 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1825 template<
typename ContainerT,
typename Po
intT>
inline bool
1828 Eigen::Vector3d min, max;
1830 node_metadata_->getBoundingBox (min, max);
1832 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1834 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1836 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1847 template<
typename ContainerT,
typename Po
intT>
inline bool
1852 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1854 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1856 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1867 template<
typename ContainerT,
typename Po
intT>
void
1870 Eigen::Vector3d min;
1871 Eigen::Vector3d max;
1872 node_metadata_->getBoundingBox (min, max);
1874 double l = max[0] - min[0];
1875 double h = max[1] - min[1];
1876 double w = max[2] - min[2];
1877 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1878 <<
", width=" << w <<
" )\n";
1880 for (std::size_t i = 0; i < num_children_; i++)
1882 children_[i]->writeVPythonVisual (file);
1888 template<
typename ContainerT,
typename Po
intT>
int
1891 return (this->payload_->read (output_cloud));
1899 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1900 return (children_[index_arg]);
1904 template<
typename ContainerT,
typename Po
intT>
std::uint64_t
1907 return (this->payload_->getDataSize ());
1912 template<
typename ContainerT,
typename Po
intT> std::size_t
1915 std::size_t loaded_children_count = 0;
1917 for (std::size_t i=0; i<8; i++)
1919 if (children_[i] !=
nullptr)
1920 loaded_children_count++;
1923 return (loaded_children_count);
1928 template<
typename ContainerT,
typename Po
intT>
void
1931 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1932 node_metadata_->loadMetadataFromDisk (path);
1935 this->parent_ = super;
1937 if (num_children_ > 0)
1940 this->num_children_ = 0;
1941 this->payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
1946 template<
typename ContainerT,
typename Po
intT>
void
1949 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1950 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1951 payload_->convertToXYZ (xyzfile);
1953 if (hasUnloadedChildren ())
1955 loadChildren (
false);
1958 for (std::size_t i = 0; i < 8; i++)
1961 children_[i]->convertToXYZ ();
1967 template<
typename ContainerT,
typename Po
intT>
void
1970 for (std::size_t i = 0; i < 8; i++)
1973 children_[i]->flushToDiskRecursive ();
1979 template<
typename ContainerT,
typename Po
intT>
void
1982 if (indices.size () < 8)
1989 int x_offset = input_cloud->fields[x_idx].offset;
1990 int y_offset = input_cloud->fields[y_idx].offset;
1991 int z_offset = input_cloud->fields[z_idx].offset;
1993 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1997 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
1998 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
1999 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
2001 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
2006 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);
2012 std::size_t box = 0;
2013 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2017 indices[box].push_back (
static_cast<int> (point_idx/input_cloud->point_step));
2022 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2031 thisnode->thisdir_ = path.parent_path ();
2033 if (!boost::filesystem::exists (thisnode->thisdir_))
2035 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2036 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2039 thisnode->thisnodeindex_ = path;
2046 thisnode->thisdir_ = path;
2050 if (thisnode->
depth_ > thisnode->root->max_depth_)
2052 thisnode->root->max_depth_ = thisnode->
depth_;
2055 boost::filesystem::directory_iterator diterend;
2056 bool loaded =
false;
2057 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2059 const boost::filesystem::path& file = *diter;
2060 if (!boost::filesystem::is_directory (file))
2064 thisnode->thisnodeindex_ = file;
2073 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2074 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2078 thisnode->max_depth_ = 0;
2081 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2083 f >> thisnode->min_[0];
2084 f >> thisnode->min_[1];
2085 f >> thisnode->min_[2];
2086 f >> thisnode->max_[0];
2087 f >> thisnode->max_[1];
2088 f >> thisnode->max_[2];
2090 std::string filename;
2092 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2096 thisnode->
payload_.reset (
new ContainerT (thisnode->thisnodestorage_));
2101 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2110 template<
typename ContainerT,
typename Po
intT>
void
2111 queryBBIntersects_noload (
const boost::filesystem::path& root_node,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const std::uint32_t query_depth, std::list<std::string>& bin_name)
2113 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2116 std::cout <<
"test";
2118 if (root->intersectsWithBoundingBox (min, max))
2120 if (query_depth == root->max_depth_)
2122 if (!root->payload_->empty ())
2124 bin_name.push_back (root->thisnodestorage_.string ());
2129 for (
int i = 0; i < 8; i++)
2131 boost::filesystem::path child_dir = root->thisdir_
2132 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2133 if (boost::filesystem::exists (child_dir))
2136 root->num_children_++;
2146 template<
typename ContainerT,
typename Po
intT>
void
2147 queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current,
const Eigen::Vector3d& min,
const Eigen::Vector3d& max,
const std::uint32_t query_depth, std::list<std::string>& bin_name)
2149 if (current->intersectsWithBoundingBox (min, max))
2151 if (current->depth_ == query_depth)
2153 if (!current->payload_->empty ())
2155 bin_name.push_back (current->thisnodestorage_.string ());
2160 for (
int i = 0; i < 8; i++)
2162 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2163 if (boost::filesystem::exists (child_dir))
2165 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2166 current->num_children_++;
2181 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_