41#ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
42#define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_
53#include <pcl/common/utils.h>
54#include <pcl/visualization/common/common.h>
55#include <pcl/outofcore/octree_base_node.h>
56#include <pcl/filters/random_sample.h>
57#include <pcl/filters/extract_indices.h>
60#include <pcl/outofcore/cJSON.h>
67 template<
typename ContainerT,
typename Po
intT>
70 template<
typename ContainerT,
typename Po
intT>
73 template<
typename ContainerT,
typename Po
intT>
76 template<
typename ContainerT,
typename Po
intT>
79 template<
typename ContainerT,
typename Po
intT>
82 template<
typename ContainerT,
typename Po
intT>
85 template<
typename ContainerT,
typename Po
intT>
88 template<
typename ContainerT,
typename Po
intT>
91 template<
typename ContainerT,
typename Po
intT>
99 , num_loaded_children_ (0)
108 template<
typename ContainerT,
typename Po
intT>
116 , num_loaded_children_ (0)
123 if (
super ==
nullptr)
131 if (!boost::filesystem::exists (
node_metadata_->getDirectoryPathname ()))
133 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n",
node_metadata_->getDirectoryPathname ().c_str ());
134 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory");
152 if (!boost::filesystem::is_directory (
file))
164 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n");
165 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index");
182 template<
typename ContainerT,
typename Po
intT>
190 , num_loaded_children_ (0)
201 template<
typename ContainerT,
typename Po
intT>
void
222 node_metadata_->setDirectoryPathname (
root_name.parent_path ());
223 node_metadata_->setOutofcoreVersion (3);
226 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ()))
228 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
231 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ()))
233 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ());
234 PCL_THROW_EXCEPTION (
PCLException,
"[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists");
246 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () /
root_name.filename ());
247 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (
node_container_name));
249 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ());
250 node_metadata_->serializeMetadataToDisk ();
253 payload_.reset (
new ContainerT (node_metadata_->getPCDFilename ()));
258 template<
typename ContainerT,
typename Po
intT>
267 template<
typename ContainerT,
typename Po
intT> std::size_t
272 for(std::size_t i=0; i<8; i++)
274 boost::filesystem::path
child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
283 template<
typename ContainerT,
typename Po
intT>
void
286 node_metadata_->serializeMetadataToDisk ();
290 for (std::size_t i = 0; i < 8; i++)
293 children_[i]->saveIdx (
true);
300 template<
typename ContainerT,
typename Po
intT>
bool
303 return (this->getNumLoadedChildren () < this->getNumChildren ());
307 template<
typename ContainerT,
typename Po
intT>
void
314 for (
int i = 0; i < 8; i++)
316 boost::filesystem::path
child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i));
318 if (boost::filesystem::exists (
child_dir) && this->children_[i] ==
nullptr)
323 num_loaded_children_++;
327 assert (num_loaded_children_ == this->getNumChildren ());
331 template<
typename ContainerT,
typename Po
intT>
void
334 if (num_children_ == 0)
339 for (std::size_t i = 0; i < 8; i++)
348 template<
typename ContainerT,
typename Po
intT> std::uint64_t
358 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
361 if (hasUnloadedChildren ())
362 loadChildren (
false);
364 std::vector < std::vector<const PointT*> >
c;
366 for (std::size_t i = 0; i < 8; i++)
368 c[i].reserve (p.size () / 8);
371 const std::size_t
len = p.size ();
372 for (std::size_t i = 0; i <
len; i++)
380 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n",
__FUNCTION__ );
385 std::uint8_t
box = 0;
386 Eigen::Vector3d
mid_xyz = node_metadata_->getVoxelCenter ();
389 c[
static_cast<std::size_t
>(
box)].push_back (&
pt);
393 for (std::size_t i = 0; i < 8; i++)
407 template<
typename ContainerT,
typename Po
intT> std::uint64_t
415 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
420 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
422 payload_->insertRange (p.data (), p.size ());
427 std::vector<const PointT*>
buff;
438 root_node_->m_tree_->incrementPointsInLOD (this->depth_,
buff.
size ());
446 if (this->hasUnloadedChildren ())
448 loadChildren (
false);
451 std::vector < std::vector<const PointT*> >
c;
453 for (std::size_t i = 0; i < 8; i++)
455 c[i].reserve (p.size () / 8);
458 const std::size_t
len = p.size ();
459 for (std::size_t i = 0; i <
len; i++)
471 std::uint8_t
box = 00;
472 Eigen::Vector3d
mid_xyz = node_metadata_->getVoxelCenter ();
474 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] )));
476 c[
box].push_back (p[i]);
480 for (std::size_t i = 0; i < 8; i++)
494 template<
typename ContainerT,
typename Po
intT> std::uint64_t
502 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
505 if( num_children_ < 8 )
506 if(hasUnloadedChildren ())
507 loadChildren (
false);
514 std::vector < pcl::Indices > indices;
517 this->sortOctantIndices (
input_cloud, indices, node_metadata_->getVoxelCenter ());
519 for(std::size_t k=0; k<indices.size (); k++)
521 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n",
__FUNCTION__, indices[k].size (), k);
526 for(std::size_t i=0; i<8; i++)
528 if ( indices[i].empty () )
531 if (children_[i] ==
nullptr)
538 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n",
__FUNCTION__);
551 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n");
558 template<
typename ContainerT,
typename Po
intT>
void
580 const double percent =
pow(sample_percent_,
double((this->root_node_->m_tree_->getDepth () - depth_)));
590 std::lock_guard<std::mutex> lock(rng_mutex_);
603 std::lock_guard<std::mutex> lock(rng_mutex_);
606 for(std::uint64_t i = 0; i <
inputsize; ++i)
613 template<
typename ContainerT,
typename Po
intT> std::uint64_t
622 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ());
625 payload_->insertRange ( p );
632 const std::size_t
len = p.size ();
634 for (std::size_t i = 0; i <
len; i++)
638 buff.push_back (p[i]);
644 root_node_->m_tree_->incrementPointsInLOD (this->depth_,
buff.
size ());
645 payload_->insertRange (
buff );
650 template<
typename ContainerT,
typename Po
intT> std::uint64_t
663 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n");
669 template<
typename ContainerT,
typename Po
intT>
void
674 for(std::size_t i = 0; i < 8; i++)
675 c[i].reserve(p.size() / 8);
677 const std::size_t
len = p.size();
678 for(std::size_t i = 0; i <
len; i++)
686 subdividePoint (
pt,
c);
691 template<
typename ContainerT,
typename Po
intT>
void
694 Eigen::Vector3d
mid_xyz = node_metadata_->getVoxelCenter ();
701 template<
typename ContainerT,
typename Po
intT> std::uint64_t
711 if ( this->depth_ == this->root_node_->m_tree_->getDepth () ||
input_cloud->width*
input_cloud->height < 8 )
718 if (num_children_ < 8 )
720 if ( hasUnloadedChildren () )
722 loadChildren (
false);
769 std::vector<pcl::Indices> indices;
772 this->sortOctantIndices (
remaining_points, indices, node_metadata_->getVoxelCenter ());
775 for(std::size_t i=0; i<8; i++)
778 if(indices[i].empty ())
781 if (children_[i] ==
nullptr)
801 template<
typename ContainerT,
typename Po
intT> std::uint64_t
812 if (this->depth_ == this->root_node_->m_tree_->getDepth ())
814 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n");
815 return (addDataAtMaxDepth(p,
false));
819 if (this->hasUnloadedChildren ())
820 loadChildren (
false );
829 root_node_->m_tree_->incrementPointsInLOD (this->depth_,
insertBuff.
size());
836 std::vector<AlignedPointTVector>
c;
840 for(std::size_t i = 0; i < 8; i++)
851 points_added += children_[i]->addDataToLeaf_and_genLOD(
c[i],
true);
859 template<
typename ContainerT,
typename Po
intT>
void
865 if (children_[idx] || (num_children_ == 8))
867 PCL_ERROR (
"[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ());
871 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin ();
872 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/
static_cast<double>(2.0);
880 x = ((idx == 5) || (idx == 7)) ? 1 : 0;
881 y = ((idx == 6) || (idx == 7)) ? 1 : 0;
886 x = ((idx == 1) || (idx == 3)) ? 1 : 0;
887 y = ((idx == 2) || (idx == 3)) ? 1 : 0;
891 childbb_min[2] = start[2] +
static_cast<double> (z) * step[2];
892 childbb_max[2] = start[2] +
static_cast<double> (z + 1) * step[2];
894 childbb_min[1] = start[1] +
static_cast<double> (y) * step[1];
895 childbb_max[1] = start[1] +
static_cast<double> (y + 1) * step[1];
897 childbb_min[0] = start[0] +
static_cast<double> (x) * step[0];
898 childbb_max[0] = start[0] +
static_cast<double> (x + 1) * step[0];
900 boost::filesystem::path
childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx));
907 template<
typename ContainerT,
typename Po
intT>
bool
910 if (((
min_bb[0] <= point[0]) && (point[0] <
max_bb[0])) &&
922 template<
typename ContainerT,
typename Po
intT>
bool
925 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin ();
926 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax ();
928 if (((min[0] <= p.x) && (p.x < max[0])) &&
929 ((min[1] <= p.y) && (p.y < max[1])) &&
930 ((min[2] <= p.z) && (p.z < max[2])))
939 template<
typename ContainerT,
typename Po
intT>
void
944 node_metadata_->getBoundingBox (min, max);
947 for (std::size_t i = 0; i < this->depth_; i++)
950 std::cout <<
"[" << min[0] <<
", " << min[1] <<
", " << min[2] <<
"] - ";
951 std::cout <<
"[" << max[0] <<
", " << max[1] <<
", " << max[2] <<
"] - ";
952 std::cout <<
"[" << max[0] - min[0] <<
", " << max[1] - min[1];
953 std::cout <<
", " << max[2] - min[2] <<
"]" << std::endl;
955 if (num_children_ > 0)
957 for (std::size_t i = 0; i < 8; i++)
967 template<
typename ContainerT,
typename Po
intT>
void
971 if (num_children_ > 0)
973 for (std::size_t i = 0; i < 8; i++)
983 Eigen::Vector3d
mid_xyz = node_metadata_->getVoxelCenter ();
1044 template<
typename Container,
typename Po
intT>
void
1050 template<
typename Container,
typename Po
intT>
void
1068 for(
int i =0; i < 6; i++){
1069 double a =
planes[(i*4)];
1070 double b =
planes[(i*4)+1];
1072 double d =
planes[(i*4)+3];
1076 Eigen::Vector3d normal(a, b,
c);
1083 Eigen::Vector3d
center = node_metadata_->getVoxelCenter();
1084 Eigen::Vector3d radius (std::abs (
static_cast<double> (
max_bb.x () -
center.x ())),
1085 std::abs (
static_cast<double> (
max_bb.y () -
center.y ())),
1086 std::abs (
static_cast<double> (
max_bb.z () -
center.z ())));
1089 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(
c));
1165 if (this->depth_ ==
query_depth && payload_->getDataSize () > 0)
1168 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1171 if (hasUnloadedChildren ())
1173 loadChildren (
false);
1176 if (this->getNumChildren () > 0)
1178 for (std::size_t i = 0; i < 8; i++)
1199 template<
typename Container,
typename Po
intT>
void
1221 for(
int i =0; i < 6; i++){
1222 double a =
planes[(i*4)];
1223 double b =
planes[(i*4)+1];
1225 double d =
planes[(i*4)+3];
1229 Eigen::Vector3d normal(a, b,
c);
1232 Eigen::Vector3d
center = node_metadata_->getVoxelCenter();
1233 Eigen::Vector3d radius (std::abs (
static_cast<double> (
max_bb.x () -
center.x ())),
1234 std::abs (
static_cast<double> (
max_bb.y () -
center.y ())),
1235 std::abs (
static_cast<double> (
max_bb.z () -
center.z ())));
1238 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(
c));
1288 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1292 if (coverage <= 10000)
1295 if (hasUnloadedChildren ())
1297 loadChildren (
false);
1300 if (this->getNumChildren () > 0)
1302 for (std::size_t i = 0; i < 8; i++)
1311 template<
typename ContainerT,
typename Po
intT>
void
1315 if (num_children_ > 0)
1317 for (std::size_t i = 0; i < 8; i++)
1326 Eigen::Vector3d
voxel_center = node_metadata_->getVoxelCenter ();
1334 template<
typename ContainerT,
typename Po
intT>
void
1345 if (this->getNumChildren () > 0)
1347 for (std::size_t i = 0; i < 8; i++)
1353 else if (hasUnloadedChildren ())
1355 loadChildren (
false);
1357 for (std::size_t i = 0; i < 8; i++)
1366 if (payload_->getDataSize () > 0)
1368 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ());
1374 template<
typename ContainerT,
typename Po
intT>
void
1378 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n",
__FUNCTION__,
startingSize );
1387 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1388 loadChildren (
false);
1391 if (num_children_ > 0)
1394 for (std::size_t i = 0; i < 8; i++)
1399 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n",
__FUNCTION__,
dst_blob->width*
dst_blob->height );
1409 payload_->readRange (0, payload_->size (),
tmp_blob);
1423 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n",
__FUNCTION__,
dst_blob->width*
dst_blob->height );
1424 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n",
__FUNCTION__);
1429 PCL_DEBUG (
"[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n",
__FUNCTION__,
dst_blob->width*
dst_blob->height );
1434 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n");
1438 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n",
__FUNCTION__,
dst_blob->width*
dst_blob->height );
1451 Eigen::Vector4f
min_pt (
static_cast<float> (
min_bb[0] ),
static_cast<float> (
min_bb[1] ),
static_cast<float> (
min_bb[2] ), 1.0f);
1452 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 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n",
__FUNCTION__, indices.
size () );
1460 if ( !indices.empty () )
1472 PCL_DEBUG (
"[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n",
__FUNCTION__);
1492 template<
typename ContainerT,
typename Po
intT>
void
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++)
1542 std::uint64_t
len = payload_->
size ();
1544 for (std::uint64_t i = 0; i <
len; 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
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)
1631 template<
typename ContainerT,
typename Po
intT>
void
1641 if ((num_children_ == 0) && (hasUnloadedChildren ()))
1643 loadChildren (
false);
1646 if (num_children_ > 0)
1649 for (std::size_t i = 0; i < 8; i++)
1675 for (std::size_t i = 0; i < payload_->size (); i++)
1689 for (std::size_t i = 0; i <
numpick; i++)
1699 template<
typename ContainerT,
typename Po
intT>
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");
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);
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);
1799 template<
typename ContainerT,
typename Po
intT>
inline bool
1802 Eigen::Vector3d min, max;
1803 node_metadata_->getBoundingBox (min, max);
1821 template<
typename ContainerT,
typename Po
intT>
inline bool
1824 Eigen::Vector3d min, max;
1826 node_metadata_->getBoundingBox (min, max);
1843 template<
typename ContainerT,
typename Po
intT>
inline bool
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
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
1913 for (std::size_t i=0; i<8; i++)
1915 if (children_[i] !=
nullptr)
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)
2008 std::size_t
box = 0;
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");
2042 thisnode->thisdir_ = path;
2043 thisnode->depth_ = super->depth_ + 1;
2044 thisnode->root_node_ = super->root_node_;
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_));
2095 thisnode->parent_ = super;
2097 children_.resize (8,
static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*
> (0));
2098 thisnode->num_children_ = 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_++;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
A base class for all pcl exceptions which inherits from std::runtime_error.
shared_ptr< PointCloud< PointT > > Ptr
std::size_t depth_
Depth in the tree, root is 0, root's children are 1, ...
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_index_basename
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...
static const std::string node_index_extension
virtual std::uint64_t getDataSize() const
Gets the number of points available in the PCD file.
void copyAllCurrentAndChildPointsRec_sub(std::list< PointT > &v, const double percent)
void loadFromFile(const boost::filesystem::path &path, OutofcoreOctreeBaseNode *super)
Loads the nodes metadata from the JSON file.
bool hasUnloadedChildren() const
Returns whether or not a node has unloaded children data.
void randomSample(const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check)
Randomly sample point data.
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.
OutofcoreOctreeBase< ContainerT, PointT > * m_tree_
The tree we belong to.
static std::mutex rng_mutex_
Random number generator mutex.
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 double sample_percent_
std::uint64_t num_children_
Number of children on disk.
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...
void writeVPythonVisual(std::ofstream &file)
Write a python visual script to file.
OutofcoreOctreeBaseNode()
Empty constructor; sets pointers for children and for bounding boxes to 0.
virtual std::size_t countNumChildren() const
Counts the number of child directories on disk; used to update num_children_.
void convertToXYZRecursive()
Recursively converts data files to ascii XZY files.
OutofcoreOctreeBaseNode * parent_
super-node
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::shared_ptr< ContainerT > payload_
what holds the points.
virtual int read(pcl::PCLPointCloud2::Ptr &output_cloud)
OutofcoreOctreeBaseNode * root_node_
The root node of the tree we belong to.
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.
void saveIdx(bool recursive)
Save node's metadata to file.
void queryFrustum(const double planes[24], std::list< std::string > &file_names)
OutofcoreOctreeNodeMetadata::Ptr node_metadata_
std::vector< PointT, Eigen::aligned_allocator< PointT > > AlignedPointTVector
void sortOctantIndices(const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &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...
void getOccupiedVoxelCentersRecursive(AlignedPointTVector &voxel_centers, const std::size_t query_depth)
Gets a vector of occupied voxel centers.
static const std::string node_container_basename
static const std::string pcd_extension
Extension for this class to find the pcd files on disk.
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)
static std::mt19937 rng_
Mersenne Twister: A 623-dimensionally equidistributed uniform pseudo-random number generator.
void subdividePoint(const PointT &point, std::vector< AlignedPointTVector > &c)
Subdivide a single point into a specific child node.
~OutofcoreOctreeBaseNode()
Will recursively delete all children calling recFreeChildrein.
virtual void loadChildren(bool recursive)
Load nodes child data creating new nodes for each.
virtual std::size_t countNumLoadedChildren() const
Counts the number of loaded chilren by testing the children_ array; used to update num_loaded_chilren...
void flushToDiskRecursive()
std::uint64_t addDataAtMaxDepth(const AlignedPointTVector &p, const bool skip_bb_check=true)
Add data to the leaf when at max depth of tree.
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.
virtual OutofcoreOctreeBaseNode * getChildPtr(std::size_t index_arg) const
Returns a pointer to the child in octant index_arg.
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
static const std::string node_container_extension
void recFreeChildren()
Method which recursively free children of this node.
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 createChild(const std::size_t idx)
Creates child node idx.
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.
virtual void printBoundingBox(const std::size_t query_depth) const
Write the voxel size to stdout at query_depth.
static void getRandomUUIDString(std::string &s)
Generate a universally unique identifier (UUID)
Define standard C methods and C++ classes that are common to all methods.
void getPointsInBox(const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt, Indices &indices)
Get a set of points residing in a box given its bounds.
PCL_EXPORTS bool concatenate(const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< PointT > &cloud_out)
Concatenate two pcl::PointCloud<PointT>
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.
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...
bool pointInBoundingBox(const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point)
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 ignore(const T &...)
Utility function to eliminate unused variable warnings.
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)
int getFieldIndex(const pcl::PointCloud< PointT > &, const std::string &field_name, std::vector< pcl::PCLPointField > &fields)
shared_ptr< Indices > IndicesPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
shared_ptr< ::pcl::PCLPointCloud2 > Ptr
A point structure representing Euclidean xyz coordinates, and the RGB color.