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++)
357 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
358 return (addDataAtMaxDepth( p, skip_bb_check));
360 if (hasUnloadedChildren ())
361 loadChildren (
false);
363 std::vector < std::vector<const PointT*> > c;
365 for (std::size_t i = 0; i < 8; i++)
367 c[i].reserve (p.size () / 8);
370 const std::size_t len = p.size ();
371 for (std::size_t i = 0; i < len; i++)
379 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ );
385 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
387 box =
static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0));
388 c[
static_cast<std::size_t
>(box)].push_back (&pt);
392 for (std::size_t i = 0; i < 8; i++)
398 points_added += children_[i]->addDataToLeaf (c[i],
true);
401 return (points_added);
414 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
419 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
421 payload_->insertRange (p.data (), p.size ());
426 std::vector<const PointT*> buff;
427 for (
const PointT* pt : p)
437 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
438 payload_->insertRange (buff.data (), buff.size ());
442 return (buff.size ());
445 if (this->hasUnloadedChildren ())
447 loadChildren (
false);
450 std::vector < std::vector<const PointT*> > c;
452 for (std::size_t i = 0; i < 8; i++)
454 c[i].reserve (p.size () / 8);
457 const std::size_t len = p.size ();
458 for (std::size_t i = 0; i < len; i++)
471 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
473 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] )));
475 c[box].push_back (p[i]);
479 for (std::size_t i = 0; i < 8; i++)
485 points_added += children_[i]->addDataToLeaf (c[i],
true);
488 return (points_added);
496 assert (this->root_node_->m_tree_ != NULL);
498 if (input_cloud->height*input_cloud->width == 0)
501 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
502 return (addDataAtMaxDepth (input_cloud,
true));
504 if( num_children_ < 8 )
505 if(hasUnloadedChildren ())
506 loadChildren (
false);
513 std::vector < std::vector<int> > indices;
516 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ());
518 for(std::size_t k=0; k<indices.size (); k++)
520 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k);
525 for(std::size_t i=0; i<8; i++)
527 if ( indices[i].empty () )
530 if (children_[i] ==
nullptr)
537 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__);
543 points_added += children_[i]->addPointCloud (dst_cloud,
false);
547 return (points_added);
550 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
557 template<
typename ContainerT,
typename Po
intT>
void
560 assert (this->root_node_->m_tree_ != NULL);
569 sampleBuff.push_back(pt);
579 const double percent = pow(sample_percent_,
double((this->root_node_->m_tree_->getDepth () - depth_)));
586 insertBuff.resize(samplesize);
589 std::lock_guard<std::mutex> lock(rng_mutex_);
590 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1);
596 insertBuff[i] = ( sampleBuff[buffstart] );
602 std::lock_guard<std::mutex> lock(rng_mutex_);
603 std::bernoulli_distribution buffdist(percent);
607 insertBuff.push_back( p[i] );
615 assert (this->root_node_->m_tree_ != NULL);
621 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
624 payload_->insertRange ( p );
630 AlignedPointTVector buff;
631 const std::size_t len = p.size ();
633 for (std::size_t i = 0; i < len; i++)
637 buff.push_back (p[i]);
643 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ());
644 payload_->insertRange ( buff );
646 return (buff.size ());
655 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_);
657 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height );
658 payload_->insertRange (input_cloud);
660 return (input_cloud->width*input_cloud->height);
662 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
668 template<
typename ContainerT,
typename Po
intT>
void
673 for(std::size_t i = 0; i < 8; i++)
674 c[i].reserve(p.size() / 8);
676 const std::size_t len = p.size();
677 for(std::size_t i = 0; i < len; i++)
685 subdividePoint (pt, c);
690 template<
typename ContainerT,
typename Po
intT>
void
693 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
694 std::size_t octant = 0;
695 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0);
696 c[octant].push_back (point);
705 if ( input_cloud->width * input_cloud->height == 0 )
710 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 )
712 std::uint64_t points_added = addDataAtMaxDepth (input_cloud,
true);
713 assert (points_added > 0);
714 return (points_added);
717 if (num_children_ < 8 )
719 if ( hasUnloadedChildren () )
721 loadChildren (
false);
734 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8;
735 random_sampler.
setSample (
static_cast<unsigned int> (sample_size));
741 pcl::IndicesPtr downsampled_cloud_indices (
new std::vector< int > () );
742 random_sampler.
filter (*downsampled_cloud_indices);
747 extractor.
setIndices (downsampled_cloud_indices);
748 extractor.
filter (*downsampled_cloud);
753 extractor.
filter (*remaining_points);
755 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 );
758 if ( downsampled_cloud->width * downsampled_cloud->height != 0 )
760 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height );
761 payload_->insertRange (downsampled_cloud);
762 points_added += downsampled_cloud->width*downsampled_cloud->height ;
765 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height);
768 std::vector<std::vector<int> > indices;
771 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ());
774 for(std::size_t i=0; i<8; i++)
777 if(indices[i].empty ())
780 if (children_[i] ==
nullptr)
791 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud);
792 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);
795 assert (points_added == input_cloud->width*input_cloud->height);
796 return (points_added);
809 assert (this->root_node_->m_tree_ != NULL );
811 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
813 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
814 return (addDataAtMaxDepth(p,
false));
818 if (this->hasUnloadedChildren ())
819 loadChildren (
false );
823 randomSample(p, insertBuff, skip_bb_check);
825 if(!insertBuff.empty())
828 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size());
830 payload_->insertRange (insertBuff);
835 std::vector<AlignedPointTVector> c;
836 subdividePoints(p, c, skip_bb_check);
839 for(std::size_t i = 0; i < 8; i++)
850 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i],
true);
854 return (points_added);
858 template<
typename ContainerT,
typename Po
intT>
void
864 if (children_[idx] || (num_children_ == 8))
866 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ());
870 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
871 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
873 Eigen::Vector3d childbb_min;
874 Eigen::Vector3d childbb_max;
879 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
880 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
885 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
886 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
890 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
891 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
893 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
894 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
896 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
897 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
899 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
906 template<
typename ContainerT,
typename Po
intT>
bool
907 pointInBoundingBox (
const Eigen::Vector3d& min_bb,
const Eigen::Vector3d& max_bb,
const Eigen::Vector3d& point)
909 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) &&
910 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) &&
911 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2])))
921 template<
typename ContainerT,
typename Po
intT>
bool
924 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
925 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
927 if (((min[0] <= p.x) && (p.x < max[0])) &&
928 ((min[1] <= p.y) && (p.y < max[1])) &&
929 ((min[2] <= p.z) && (p.z < max[2])))
938 template<
typename ContainerT,
typename Po
intT>
void
943 node_metadata_->getBoundingBox (min, max);
945 if (this->depth_ < query_depth){
946 for (std::size_t i = 0; i < this->depth_; i++)
949 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
950 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
951 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
952 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
954 if (num_children_ > 0)
956 for (std::size_t i = 0; i < 8; i++)
959 children_[i]->printBoundingBox (query_depth);
966 template<
typename ContainerT,
typename Po
intT>
void
969 if (this->depth_ < query_depth){
970 if (num_children_ > 0)
972 for (std::size_t i = 0; i < 8; i++)
975 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
982 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter ();
983 voxel_center.x =
static_cast<float>(mid_xyz[0]);
984 voxel_center.y =
static_cast<float>(mid_xyz[1]);
985 voxel_center.z =
static_cast<float>(mid_xyz[2]);
987 voxel_centers.push_back(voxel_center);
1043 template<
typename Container,
typename Po
intT>
void
1046 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth());
1049 template<
typename Container,
typename Po
intT>
void
1053 enum {INSIDE, INTERSECT, OUTSIDE};
1055 int result = INSIDE;
1057 if (this->depth_ > query_depth)
1065 if (!skip_vfc_check)
1067 for(
int i =0; i < 6; i++){
1068 double a = planes[(i*4)];
1069 double b = planes[(i*4)+1];
1070 double c = planes[(i*4)+2];
1071 double d = planes[(i*4)+3];
1075 Eigen::Vector3d normal(a, b, c);
1077 Eigen::Vector3d min_bb;
1078 Eigen::Vector3d max_bb;
1079 node_metadata_->getBoundingBox(min_bb, max_bb);
1082 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1083 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1084 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1085 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1087 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1088 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1095 if (m - n < 0) result = INTERSECT;
1146 if (result == OUTSIDE)
1164 if (this->depth_ == query_depth && payload_->getDataSize () > 0)
1167 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1170 if (hasUnloadedChildren ())
1172 loadChildren (
false);
1175 if (this->getNumChildren () > 0)
1177 for (std::size_t i = 0; i < 8; i++)
1180 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) );
1198 template<
typename Container,
typename Po
intT>
void
1203 if (this->depth_ > query_depth)
1209 Eigen::Vector3d min_bb;
1210 Eigen::Vector3d max_bb;
1211 node_metadata_->getBoundingBox(min_bb, max_bb);
1214 enum {INSIDE, INTERSECT, OUTSIDE};
1216 int result = INSIDE;
1218 if (!skip_vfc_check)
1220 for(
int i =0; i < 6; i++){
1221 double a = planes[(i*4)];
1222 double b = planes[(i*4)+1];
1223 double c = planes[(i*4)+2];
1224 double d = planes[(i*4)+3];
1228 Eigen::Vector3d normal(a, b, c);
1231 Eigen::Vector3d center = node_metadata_->getVoxelCenter();
1232 Eigen::Vector3d radius (std::abs (
static_cast<double> (max_bb.x () - center.x ())),
1233 std::abs (
static_cast<double> (max_bb.y () - center.y ())),
1234 std::abs (
static_cast<double> (max_bb.z () - center.z ())));
1236 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d;
1237 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c));
1244 if (m - n < 0) result = INTERSECT;
1249 if (result == OUTSIDE)
1284 if (this->depth_ <= query_depth && payload_->getDataSize () > 0)
1287 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1291 if (coverage <= 10000)
1294 if (hasUnloadedChildren ())
1296 loadChildren (
false);
1299 if (this->getNumChildren () > 0)
1301 for (std::size_t i = 0; i < 8; i++)
1304 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) );
1310 template<
typename ContainerT,
typename Po
intT>
void
1313 if (this->depth_ < query_depth){
1314 if (num_children_ > 0)
1316 for (std::size_t i = 0; i < 8; i++)
1319 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
1325 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter ();
1326 voxel_centers.push_back(voxel_center);
1333 template<
typename ContainerT,
typename Po
intT>
void
1337 Eigen::Vector3d my_min = min_bb;
1338 Eigen::Vector3d my_max = max_bb;
1340 if (intersectsWithBoundingBox (my_min, my_max))
1342 if (this->depth_ < query_depth)
1344 if (this->getNumChildren () > 0)
1346 for (std::size_t i = 0; i < 8; i++)
1349 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1352 else if (hasUnloadedChildren ())
1354 loadChildren (
false);
1356 for (std::size_t i = 0; i < 8; i++)
1359 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names);
1365 if (payload_->getDataSize () > 0)
1367 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1373 template<
typename ContainerT,
typename Po
intT>
void
1376 std::uint64_t startingSize = dst_blob->width*dst_blob->height;
1377 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize );
1380 if (intersectsWithBoundingBox (min_bb, max_bb))
1383 if (this->depth_ < query_depth)
1386 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1387 loadChildren (
false);
1390 if (num_children_ > 0)
1393 for (std::size_t i = 0; i < 8; i++)
1396 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob);
1398 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1408 payload_->readRange (0, payload_->size (), tmp_blob);
1410 if( tmp_blob->width*tmp_blob->height == 0 )
1414 if (inBoundingBox (min_bb, max_bb))
1420 if( dst_blob->width*dst_blob->height != 0 )
1422 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1423 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__);
1428 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1433 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1435 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height);
1437 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height );
1448 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->
width*tmp_cloud->
height );
1450 Eigen::Vector4f min_pt (
static_cast<float> ( min_bb[0] ),
static_cast<float> ( min_bb[1] ),
static_cast<float> ( min_bb[2] ), 1.0f);
1451 Eigen::Vector4f max_pt (
static_cast<float> ( max_bb[0] ),
static_cast<float> ( max_bb[1] ) ,
static_cast<float>( max_bb[2] ), 1.0f );
1453 std::vector<int> indices;
1456 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () );
1457 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->
width*tmp_cloud->
height - indices.size () );
1459 if ( !indices.empty () )
1461 if( dst_blob->width*dst_blob->height > 0 )
1468 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () );
1469 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () );
1471 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__);
1472 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height;
1473 (void)orig_points_in_destination;
1477 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination);
1483 assert ( dst_blob->width*dst_blob->height == indices.size () );
1489 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize );
1492 template<
typename ContainerT,
typename Po
intT>
void
1497 if (intersectsWithBoundingBox (min_bb, max_bb))
1500 if (this->depth_ < query_depth)
1503 if (this->hasUnloadedChildren ())
1505 this->loadChildren (
false);
1509 if (getNumChildren () > 0)
1511 if(hasUnloadedChildren ())
1512 loadChildren (
false);
1515 for (std::size_t i = 0; i < 8; i++)
1518 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v);
1527 if (inBoundingBox (min_bb, max_bb))
1530 AlignedPointTVector payload_cache;
1531 payload_->readRange (0, payload_->size (), payload_cache);
1532 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1539 AlignedPointTVector payload_cache;
1540 payload_->readRange (0, payload_->size (), payload_cache);
1546 const PointT& p = payload_cache[i];
1555 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]);
1563 template<
typename ContainerT,
typename Po
intT>
void
1566 if (intersectsWithBoundingBox (min_bb, max_bb))
1568 if (this->depth_ < query_depth)
1570 if (this->hasUnloadedChildren ())
1571 this->loadChildren (
false);
1573 if (this->getNumChildren () > 0)
1575 for (std::size_t i=0; i<8; i++)
1578 if (children_[i]!=
nullptr)
1579 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent);
1588 if (inBoundingBox (min_bb, max_bb))
1591 this->payload_->read (tmp_blob);
1594 double sample_points =
static_cast<double>(num_pts) * percent;
1598 sample_points = sample_points > 1 ? sample_points : 1;
1612 random_sampler.
setSample (
static_cast<unsigned int> (sample_points));
1618 random_sampler.
filter (*downsampled_cloud_indices);
1619 extractor.
setIndices (downsampled_cloud_indices);
1620 extractor.
filter (*downsampled_points);
1631 template<
typename ContainerT,
typename Po
intT>
void
1635 if (intersectsWithBoundingBox (min_bb, max_bb))
1638 if (this->depth_ < query_depth)
1641 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1643 loadChildren (
false);
1646 if (num_children_ > 0)
1649 for (std::size_t i = 0; i < 8; i++)
1652 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst);
1661 if (inBoundingBox (min_bb, max_bb))
1664 AlignedPointTVector payload_cache;
1665 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1666 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
1671 AlignedPointTVector payload_cache_within_region;
1673 AlignedPointTVector payload_cache;
1674 payload_->readRange (0, payload_->size (), payload_cache);
1675 for (std::size_t i = 0; i < payload_->size (); i++)
1677 const PointT& p = payload_cache[i];
1680 payload_cache_within_region.push_back (p);
1686 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()()));
1687 std::size_t numpick =
static_cast<std::size_t
> (percent *
static_cast<double> (payload_cache_within_region.size ()));;
1689 for (std::size_t i = 0; i < numpick; i++)
1691 dst.push_back (payload_cache_within_region[i]);
1699 template<
typename ContainerT,
typename Po
intT>
1705 , children_ (8, nullptr)
1707 , num_loaded_children_ (0)
1713 if (super ==
nullptr)
1715 PCL_ERROR (
"[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" );
1716 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent");
1729 std::string uuid_idx;
1730 std::string uuid_cont;
1736 std::string node_container_name;
1739 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir));
1743 boost::filesystem::create_directory (
node_metadata_->getDirectoryPathname ());
1751 template<
typename ContainerT,
typename Po
intT>
void
1754 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1756 loadChildren (
false);
1759 for (std::size_t i = 0; i < num_children_; i++)
1761 children_[i]->copyAllCurrentAndChildPointsRec (v);
1765 payload_->readRange (0, payload_->size (), payload_cache);
1768 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1774 template<
typename ContainerT,
typename Po
intT>
void
1777 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1779 loadChildren (
false);
1782 for (std::size_t i = 0; i < 8; i++)
1785 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent);
1788 std::vector<PointT> payload_cache;
1789 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1791 for (std::size_t i = 0; i < payload_cache.size (); i++)
1793 v.push_back (payload_cache[i]);
1799 template<
typename ContainerT,
typename Po
intT>
inline bool
1802 Eigen::Vector3d min, max;
1803 node_metadata_->getBoundingBox (min, max);
1806 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0])))
1808 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1])))
1810 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2])))
1821 template<
typename ContainerT,
typename Po
intT>
inline bool
1824 Eigen::Vector3d min, max;
1826 node_metadata_->getBoundingBox (min, max);
1828 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0]))
1830 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1]))
1832 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2]))
1843 template<
typename ContainerT,
typename Po
intT>
inline bool
1848 if ((min_bb[0] <= p.x) && (p.x < max_bb[0]))
1850 if ((min_bb[1] <= p.y) && (p.y < max_bb[1]))
1852 if ((min_bb[2] <= p.z) && (p.z < max_bb[2]))
1863 template<
typename ContainerT,
typename Po
intT>
void
1866 Eigen::Vector3d min;
1867 Eigen::Vector3d max;
1868 node_metadata_->getBoundingBox (min, max);
1870 double l = max[0] - min[0];
1871 double h = max[1] - min[1];
1872 double w = max[2] - min[2];
1873 file <<
"box( pos=(" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"), length=" << l <<
", height=" << h
1874 <<
", width=" << w <<
" )\n";
1876 for (std::size_t i = 0; i < num_children_; i++)
1878 children_[i]->writeVPythonVisual (file);
1884 template<
typename ContainerT,
typename Po
intT>
int
1887 return (this->payload_->read (output_cloud));
1895 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg);
1896 return (children_[index_arg]);
1900 template<
typename ContainerT,
typename Po
intT>
std::uint64_t
1903 return (this->payload_->getDataSize ());
1908 template<
typename ContainerT,
typename Po
intT> std::size_t
1911 std::size_t loaded_children_count = 0;
1913 for (std::size_t i=0; i<8; i++)
1915 if (children_[i] !=
nullptr)
1916 loaded_children_count++;
1919 return (loaded_children_count);
1924 template<
typename ContainerT,
typename Po
intT>
void
1927 PCL_DEBUG (
"[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ());
1928 node_metadata_->loadMetadataFromDisk (path);
1931 this->parent_ = super;
1933 if (num_children_ > 0)
1936 this->num_children_ = 0;
1937 this->payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
1942 template<
typename ContainerT,
typename Po
intT>
void
1945 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (
".dat.xyz");
1946 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname;
1947 payload_->convertToXYZ (xyzfile);
1949 if (hasUnloadedChildren ())
1951 loadChildren (
false);
1954 for (std::size_t i = 0; i < 8; i++)
1957 children_[i]->convertToXYZ ();
1963 template<
typename ContainerT,
typename Po
intT>
void
1966 for (std::size_t i = 0; i < 8; i++)
1969 children_[i]->flushToDiskRecursive ();
1975 template<
typename ContainerT,
typename Po
intT>
void
1978 if (indices.size () < 8)
1985 int x_offset = input_cloud->fields[x_idx].offset;
1986 int y_offset = input_cloud->fields[y_idx].offset;
1987 int z_offset = input_cloud->fields[z_idx].offset;
1989 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step )
1993 local_pt.x = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + x_offset]));
1994 local_pt.y = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + y_offset]));
1995 local_pt.z = * (
reinterpret_cast<float*
>(&input_cloud->data[point_idx + z_offset]));
1997 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z))
2002 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);
2008 std::size_t box = 0;
2009 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0);
2013 indices[box].push_back (
static_cast<int> (point_idx/input_cloud->point_step));
2018 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated
2027 thisnode->thisdir_ = path.parent_path ();
2029 if (!boost::filesystem::exists (thisnode->thisdir_))
2031 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () );
2032 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory");
2035 thisnode->thisnodeindex_ = path;
2042 thisnode->thisdir_ = path;
2046 if (thisnode->
depth_ > thisnode->root->max_depth_)
2048 thisnode->root->max_depth_ = thisnode->
depth_;
2051 boost::filesystem::directory_iterator diterend;
2052 bool loaded =
false;
2053 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter)
2055 const boost::filesystem::path& file = *diter;
2056 if (!boost::filesystem::is_directory (file))
2060 thisnode->thisnodeindex_ = file;
2069 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n");
2070 PCL_THROW_EXCEPTION (PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file");
2074 thisnode->max_depth_ = 0;
2077 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in);
2079 f >> thisnode->min_[0];
2080 f >> thisnode->min_[1];
2081 f >> thisnode->min_[2];
2082 f >> thisnode->max_[0];
2083 f >> thisnode->max_[1];
2084 f >> thisnode->max_[2];
2086 std::string filename;
2088 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename;
2092 thisnode->
payload_.reset (
new ContainerT (thisnode->thisnodestorage_));
2097 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2106 template<
typename ContainerT,
typename Po
intT>
void
2107 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)
2109 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL);
2112 std::cout <<
"test";
2114 if (root->intersectsWithBoundingBox (min, max))
2116 if (query_depth == root->max_depth_)
2118 if (!root->payload_->empty ())
2120 bin_name.push_back (root->thisnodestorage_.string ());
2125 for (
int i = 0; i < 8; i++)
2127 boost::filesystem::path child_dir = root->thisdir_
2128 / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2129 if (boost::filesystem::exists (child_dir))
2132 root->num_children_++;
2142 template<
typename ContainerT,
typename Po
intT>
void
2143 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)
2145 if (current->intersectsWithBoundingBox (min, max))
2147 if (current->depth_ == query_depth)
2149 if (!current->payload_->empty ())
2151 bin_name.push_back (current->thisnodestorage_.string ());
2156 for (
int i = 0; i < 8; i++)
2158 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i));
2159 if (boost::filesystem::exists (child_dir))
2161 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current);
2162 current->num_children_++;
2177 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_