41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
52 #include <pcl/common/common.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>
67 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename =
"node";
69 template<
typename ContainerT,
typename Po
intT>
70 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename =
"node";
72 template<
typename ContainerT,
typename Po
intT>
73 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension =
".oct_idx";
75 template<
typename ContainerT,
typename Po
intT>
76 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension =
".oct_dat";
78 template<
typename ContainerT,
typename Po
intT>
79 std::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_;
81 template<
typename ContainerT,
typename Po
intT>
82 std::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_;
84 template<
typename ContainerT,
typename Po
intT>
85 const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125;
87 template<
typename ContainerT,
typename Po
intT>
88 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension =
".pcd";
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++)
347 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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__ );
384 std::uint8_t box = 0;
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);
391 std::uint64_t points_added = 0;
392 for (std::size_t i = 0; i < 8; i++)
398 points_added += children_[i]->addDataToLeaf (c[i],
true);
401 return (points_added);
406 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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++)
470 std::uint8_t box = 00;
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]);
478 std::uint64_t points_added = 0;
479 for (std::size_t i = 0; i < 8; i++)
485 points_added += children_[i]->addDataToLeaf (c[i],
true);
488 return (points_added);
493 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
523 std::uint64_t points_added = 0;
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_)));
580 const std::uint64_t samplesize =
static_cast<std::uint64_t
>(percent *
static_cast<double>(sampleBuff.size()));
581 const std::uint64_t inputsize = sampleBuff.size();
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);
593 for(std::uint64_t i = 0; i < samplesize; ++i)
595 std::uint64_t buffstart = buffdist(rng_);
596 insertBuff[i] = ( sampleBuff[buffstart] );
602 std::lock_guard<std::mutex> lock(rng_mutex_);
603 std::bernoulli_distribution buffdist(percent);
605 for(std::uint64_t i = 0; i < inputsize; ++i)
607 insertBuff.push_back( p[i] );
612 template<
typename ContainerT,
typename Po
intT> std::uint64_t
615 assert (this->root_node_->m_tree_ != NULL);
621 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
624 payload_->insertRange ( p );
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 ());
649 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
700 template<
typename ContainerT,
typename Po
intT> std::uint64_t
703 std::uint64_t points_added = 0;
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);
800 template<
typename ContainerT,
typename Po
intT> std::uint64_t
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);
838 std::uint64_t points_added = 0;
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))
1531 payload_->readRange (0, payload_->size (), payload_cache);
1532 v.insert (v.end (), payload_cache.begin (), payload_cache.end ());
1540 payload_->readRange (0, payload_->size (), payload_cache);
1542 std::uint64_t len = payload_->size ();
1544 for (std::uint64_t i = 0; i < len; i++)
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);
1592 std::uint64_t num_pts = tmp_blob->width*tmp_blob->height;
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))
1665 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache);
1666 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ());
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))
2058 if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
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_
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 object using a field_map...
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
shared_ptr< PointCloud< PointT > > Ptr
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) ...
std::size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
virtual void queryBBIncludes(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, std::size_t query_depth, AlignedPointTVector &dst)
Recursively add points that fall into the queried bounding box up to the query_depth.
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.
RandomSample applies a random sampling with uniform probability.
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
shared_ptr< Indices > IndicesPtr
void setNegative(bool negative)
Set whether the regular conditions for points filtering should apply, or the inverted conditions...
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
OutofcoreOctreeBaseNode< ContainerT, PointT > * makenode_norec(const boost::filesystem::path &path, OutofcoreOctreeBaseNode< ContainerT, PointT > *super)
Non-class function which creates a single child leaf; used with queryBBIntersects_noload to avoid loa...
~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, std::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
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
Get the index of a specified field (i.e., dimension/channel)
void recFreeChildren()
Method which recursively free children of this node.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
virtual std::size_t getDepth() const
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
void createChild(const std::size_t idx)
Creates child node idx.
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
virtual std::uint64_t addPointCloud(const pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check=false)
Add a single PCLPointCloud2 object into the octree.
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
void 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)
Non-class method which performs a bounding box query without loading any of the point cloud data from...
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
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 std::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 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...
std::shared_ptr< ContainerT > payload_
what holds the points.
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.
static const std::string node_container_basename
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud
std::uint32_t height
The point cloud height (if organized as an image-structure).
void saveIdx(bool recursive)
Save node's metadata to file.
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual std::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...
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.
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
OutofcoreOctreeBaseNode * parent_
super-node
void filter(std::vector< int > &indices)
Calls the filtering method and returns the filtered point cloud indices.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void setInputCloud(const PCLPointCloud2ConstPtr &cloud)
Provide a pointer to the input dataset.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
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...
std::uint64_t num_children_
Number of children on disk.
This code defines the octree used for point storage at Urban Robotics.
void setSample(unsigned int sample)
Set number of indices to be sampled.
virtual void queryBBIntersects(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const std::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...
virtual std::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...
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...