00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
00041 #define PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_
00042
00043
00044 #include <pcl/outofcore/octree_base.h>
00045
00046
00047 #include <pcl/outofcore/cJSON.h>
00048
00049 #include <pcl/filters/random_sample.h>
00050 #include <pcl/filters/extract_indices.h>
00051
00052
00053 #include <iostream>
00054 #include <fstream>
00055 #include <sstream>
00056 #include <string>
00057 #include <exception>
00058
00059 namespace pcl
00060 {
00061 namespace outofcore
00062 {
00063
00065
00067
00068 template<typename ContainerT, typename PointT>
00069 const std::string OutofcoreOctreeBase<ContainerT, PointT>::TREE_EXTENSION_ = ".octree";
00070
00071 template<typename ContainerT, typename PointT>
00072 const int OutofcoreOctreeBase<ContainerT, PointT>::OUTOFCORE_VERSION_ = static_cast<int>(3);
00073
00075
00076 template<typename ContainerT, typename PointT>
00077 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::filesystem::path& root_name, const bool load_all)
00078 : root_node_ ()
00079 , read_write_mutex_ ()
00080 , metadata_ (new OutofcoreOctreeBaseMetadata ())
00081 , sample_percent_ (0.125)
00082 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
00083 {
00084
00085 if (!this->checkExtension (root_name))
00086 {
00087 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
00088 }
00089
00090
00091 root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, NULL, load_all);
00092
00093 root_node_->m_tree_ = this;
00094
00095
00096 boost::filesystem::path treepath = root_name.parent_path () / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
00097
00098
00099 metadata_->loadMetadataFromDisk (treepath);
00100 }
00101
00103
00104 template<typename ContainerT, typename PointT>
00105 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
00106 : root_node_()
00107 , read_write_mutex_ ()
00108 , metadata_ (new OutofcoreOctreeBaseMetadata ())
00109 , sample_percent_ (0.125)
00110 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
00111 {
00112
00113 Eigen::Vector3d tmp_min = min;
00114 Eigen::Vector3d tmp_max = max;
00115 this->enlargeToCube (tmp_min, tmp_max);
00116
00117
00118 boost::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg);
00119
00120
00121 this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys);
00122 }
00123
00125
00126 template<typename ContainerT, typename PointT>
00127 OutofcoreOctreeBase<ContainerT, PointT>::OutofcoreOctreeBase (const boost::uint64_t max_depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_node_name, const std::string& coord_sys)
00128 : root_node_()
00129 , read_write_mutex_ ()
00130 , metadata_ (new OutofcoreOctreeBaseMetadata ())
00131 , sample_percent_ (0.125)
00132 , lod_filter_ptr_ (new pcl::RandomSample<pcl::PCLPointCloud2> ())
00133 {
00134
00135 this->init (max_depth, min, max, root_node_name, coord_sys);
00136 }
00137
00139 template<typename ContainerT, typename PointT> void
00140 OutofcoreOctreeBase<ContainerT, PointT>::init (const uint64_t& depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_name, const std::string& coord_sys)
00141 {
00142
00143 if (!this->checkExtension (root_name))
00144 {
00145 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Bad extension. Outofcore Octrees must have a root node ending in .oct_idx\n");
00146 }
00147
00148
00149 if (boost::filesystem::exists (root_name.parent_path ()))
00150 {
00151 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase] A dir named %s already exists. Overwriting an existing tree is not supported.\n", root_name.parent_path ().c_str () );
00152 PCL_THROW_EXCEPTION ( PCLException, "[pcl::outofcore::OutofcoreOctreeBase] Directory exists; Overwriting an existing tree is not supported\n");
00153 }
00154
00155
00156 boost::filesystem::path dir = root_name.parent_path ();
00157
00158 if (!boost::filesystem::exists (dir))
00159 {
00160 boost::filesystem::create_directory (dir);
00161 }
00162
00163 Eigen::Vector3d tmp_min = min;
00164 Eigen::Vector3d tmp_max = max;
00165 this->enlargeToCube (tmp_min, tmp_max);
00166
00167
00168 root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name);
00169 root_node_->m_tree_ = this;
00170
00171
00172 boost::filesystem::path treepath = dir / (boost::filesystem::basename (root_name) + TREE_EXTENSION_);
00173
00174
00175 metadata_->setCoordinateSystem (coord_sys);
00176 metadata_->setDepth (depth);
00177 metadata_->setLODPoints (depth+1);
00178 metadata_->setMetadataFilename (treepath);
00179 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_);
00180
00181
00182
00183 metadata_->serializeMetadataToDisk ();
00184 }
00185
00186
00188 template<typename ContainerT, typename PointT>
00189 OutofcoreOctreeBase<ContainerT, PointT>::~OutofcoreOctreeBase ()
00190 {
00191 root_node_->flushToDiskRecursive ();
00192
00193 saveToFile ();
00194 delete (root_node_);
00195 }
00196
00198
00199 template<typename ContainerT, typename PointT> void
00200 OutofcoreOctreeBase<ContainerT, PointT>::saveToFile ()
00201 {
00202 this->metadata_->serializeMetadataToDisk ();
00203 }
00204
00206
00207 template<typename ContainerT, typename PointT> boost::uint64_t
00208 OutofcoreOctreeBase<ContainerT, PointT>::addDataToLeaf (const AlignedPointTVector& p)
00209 {
00210 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
00211
00212 const bool _FORCE_BB_CHECK = true;
00213
00214 uint64_t pt_added = root_node_->addDataToLeaf (p, _FORCE_BB_CHECK);
00215
00216 assert (p.size () == pt_added);
00217
00218 return (pt_added);
00219 }
00220
00222
00223 template<typename ContainerT, typename PointT> boost::uint64_t
00224 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud (PointCloudConstPtr point_cloud)
00225 {
00226 return (addDataToLeaf (point_cloud->points));
00227 }
00228
00230
00231 template<typename ContainerT, typename PointT> boost::uint64_t
00232 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check)
00233 {
00234 uint64_t pt_added = this->root_node_->addPointCloud (input_cloud, skip_bb_check) ;
00235
00236 return (pt_added);
00237 }
00238
00239
00241
00242 template<typename ContainerT, typename PointT> boost::uint64_t
00243 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (PointCloudConstPtr point_cloud)
00244 {
00245
00246 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
00247 boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (point_cloud->points, false);
00248 return (pt_added);
00249 }
00250
00252
00253 template<typename ContainerT, typename PointT> boost::uint64_t
00254 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (pcl::PCLPointCloud2::Ptr &input_cloud)
00255 {
00256
00257 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
00258 boost::uint64_t pt_added = root_node_->addPointCloud_and_genLOD (input_cloud);
00259
00260 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::%s] Points added %lu, points in input cloud, %lu\n",__FUNCTION__, pt_added, input_cloud->width*input_cloud->height );
00261
00262 assert ( input_cloud->width*input_cloud->height == pt_added );
00263
00264 return (pt_added);
00265 }
00266
00268
00269 template<typename ContainerT, typename PointT> boost::uint64_t
00270 OutofcoreOctreeBase<ContainerT, PointT>::addDataToLeaf_and_genLOD (AlignedPointTVector& src)
00271 {
00272
00273 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
00274 boost::uint64_t pt_added = root_node_->addDataToLeaf_and_genLOD (src, false);
00275 return (pt_added);
00276 }
00277
00279
00280 template<typename Container, typename PointT> void
00281 OutofcoreOctreeBase<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) const
00282 {
00283 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00284 root_node_->queryFrustum (planes, file_names, this->getTreeDepth());
00285 }
00286
00288
00289 template<typename Container, typename PointT> void
00290 OutofcoreOctreeBase<Container, PointT>::queryFrustum(const double *planes, std::list<std::string>& file_names, const boost::uint32_t query_depth) const
00291 {
00292 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00293 root_node_->queryFrustum (planes, file_names, query_depth);
00294 }
00295
00297
00298 template<typename Container, typename PointT> void
00299 OutofcoreOctreeBase<Container, PointT>::queryFrustum (
00300 const double *planes,
00301 const Eigen::Vector3d &eye,
00302 const Eigen::Matrix4d &view_projection_matrix,
00303 std::list<std::string>& file_names,
00304 const boost::uint32_t query_depth) const
00305 {
00306 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00307 root_node_->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth);
00308 }
00309
00311
00312 template<typename ContainerT, typename PointT> void
00313 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, AlignedPointTVector& dst) const
00314 {
00315 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00316 dst.clear ();
00317 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode] Querying Bounding Box %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf", min[0], min[1], min[2], max[0], max[1], max[2]);
00318 root_node_->queryBBIncludes (min, max, query_depth, dst);
00319 }
00320
00322
00323 template<typename ContainerT, typename PointT> void
00324 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) const
00325 {
00326 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00327
00328 dst_blob->data.clear ();
00329 dst_blob->width = 0;
00330 dst_blob->height =1;
00331
00332 root_node_->queryBBIncludes ( min, max, query_depth, dst_blob );
00333 }
00334
00336
00337 template<typename ContainerT, typename PointT> void
00338 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint64_t query_depth, const double percent, AlignedPointTVector& dst) const
00339 {
00340 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00341 dst.clear ();
00342 root_node_->queryBBIncludes_subsample (min, max, query_depth, percent, dst);
00343 }
00344
00346 template<typename ContainerT, typename PointT> void
00347 OutofcoreOctreeBase<ContainerT, PointT>::queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent)
00348 {
00349 if (percent==1.0)
00350 {
00351 root_node_->queryBBIncludes (min, max, query_depth, dst_blob);
00352 }
00353 else
00354 {
00355 root_node_->queryBBIncludes_subsample (min, max, query_depth, dst_blob, percent);
00356 }
00357 }
00358
00360
00361 template<typename ContainerT, typename PointT> bool
00362 OutofcoreOctreeBase<ContainerT, PointT>::getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const
00363 {
00364 if (root_node_!= NULL)
00365 {
00366 root_node_->getBoundingBox (min, max);
00367 return true;
00368 }
00369 return false;
00370 }
00371
00373
00374 template<typename ContainerT, typename PointT> void
00375 OutofcoreOctreeBase<ContainerT, PointT>::printBoundingBox(const size_t query_depth) const
00376 {
00377 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00378 root_node_->printBoundingBox (query_depth);
00379 }
00380
00382
00383 template<typename ContainerT, typename PointT> void
00384 OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, const size_t query_depth) const
00385 {
00386 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00387 if (query_depth > metadata_->getDepth ())
00388 {
00389 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
00390 }
00391 else
00392 {
00393 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
00394 }
00395 }
00396
00398
00399 template<typename ContainerT, typename PointT> void
00400 OutofcoreOctreeBase<ContainerT, PointT>::getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const size_t query_depth) const
00401 {
00402 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00403 if (query_depth > metadata_->getDepth ())
00404 {
00405 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, metadata_->getDepth ());
00406 }
00407 else
00408 {
00409 root_node_->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth);
00410 }
00411 }
00412
00414
00415 template<typename ContainerT, typename PointT> void
00416 OutofcoreOctreeBase<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name) const
00417 {
00418 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
00419 bin_name.clear ();
00420 #if defined _MSC_VER
00421 #pragma warning(push)
00422 #pragma warning(disable : 4267)
00423 #endif
00424 root_node_->queryBBIntersects (min, max, query_depth, bin_name);
00425 #if defined _MSC_VER
00426 #pragma warning(pop)
00427 #endif
00428 }
00429
00431
00432 template<typename ContainerT, typename PointT> void
00433 OutofcoreOctreeBase<ContainerT, PointT>::writeVPythonVisual (const boost::filesystem::path filename)
00434 {
00435 std::ofstream f (filename.c_str ());
00436
00437 f << "from visual import *\n\n";
00438
00439 root_node_->writeVPythonVisual (f);
00440 }
00441
00443
00444 template<typename ContainerT, typename PointT> void
00445 OutofcoreOctreeBase<ContainerT, PointT>::flushToDisk ()
00446 {
00447 root_node_->flushToDisk ();
00448 }
00449
00451
00452 template<typename ContainerT, typename PointT> void
00453 OutofcoreOctreeBase<ContainerT, PointT>::flushToDiskLazy ()
00454 {
00455 root_node_->flushToDiskLazy ();
00456 }
00457
00459
00460 template<typename ContainerT, typename PointT> void
00461 OutofcoreOctreeBase<ContainerT, PointT>::convertToXYZ ()
00462 {
00463 saveToFile ();
00464 root_node_->convertToXYZ ();
00465 }
00466
00468
00469 template<typename ContainerT, typename PointT> void
00470 OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache ()
00471 {
00472 DeAllocEmptyNodeCache (root_node_);
00473 }
00474
00476
00477 template<typename ContainerT, typename PointT> void
00478 OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache (OutofcoreOctreeBaseNode<ContainerT, PointT>* current)
00479 {
00480 if (current->size () == 0)
00481 {
00482 current->flush_DeAlloc_this_only ();
00483 }
00484
00485 for (int i = 0; i < current->numchildren (); i++)
00486 {
00487 DeAllocEmptyNodeCache (current->children[i]);
00488 }
00489
00490 }
00491
00493 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
00494 OutofcoreOctreeBase<ContainerT, PointT>::getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const
00495 {
00496 return (branch_arg.getChildPtr (childIdx_arg));
00497 }
00498
00500 template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr
00501 OutofcoreOctreeBase<ContainerT, PointT>::getLODFilter ()
00502 {
00503 return (lod_filter_ptr_);
00504 }
00505
00507
00508 template<typename ContainerT, typename PointT> const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
00509 OutofcoreOctreeBase<ContainerT, PointT>::getLODFilter () const
00510 {
00511 return (lod_filter_ptr_);
00512 }
00513
00515
00516 template<typename ContainerT, typename PointT> void
00517 OutofcoreOctreeBase<ContainerT, PointT>::setLODFilter (const pcl::Filter<pcl::PCLPointCloud2>::Ptr& filter_arg)
00518 {
00519 lod_filter_ptr_ = filter_arg;
00520 }
00521
00523
00524 template<typename ContainerT, typename PointT> bool
00525 OutofcoreOctreeBase<ContainerT, PointT>::getBinDimension (double& x, double& y) const
00526 {
00527 if (root_node_== NULL)
00528 {
00529 x = 0;
00530 y = 0;
00531 return (false);
00532 }
00533
00534 Eigen::Vector3d min, max;
00535 this->getBoundingBox (min, max);
00536
00537 double depth = static_cast<double> (metadata_->getDepth ());
00538 Eigen::Vector3d diff = max-min;
00539
00540 y = diff[1] * pow (.5, depth);
00541 x = diff[0] * pow (.5, depth);
00542
00543 return (true);
00544 }
00545
00547
00548 template<typename ContainerT, typename PointT> double
00549 OutofcoreOctreeBase<ContainerT, PointT>::getVoxelSideLength (const boost::uint64_t& depth) const
00550 {
00551 Eigen::Vector3d min, max;
00552 this->getBoundingBox (min, max);
00553 double result = (max[0] - min[0]) * pow (.5, static_cast<double> (metadata_->getDepth ())) * static_cast<double> (1 << (metadata_->getDepth () - depth));
00554 return (result);
00555 }
00556
00558
00559 template<typename ContainerT, typename PointT> void
00560 OutofcoreOctreeBase<ContainerT, PointT>::buildLOD ()
00561 {
00562 if (root_node_== NULL)
00563 {
00564 PCL_ERROR ("Root node is null; aborting buildLOD.\n");
00565 return;
00566 }
00567
00568 boost::unique_lock < boost::shared_mutex > lock (read_write_mutex_);
00569
00570 const int number_of_nodes = 1;
00571
00572 std::vector<BranchNode*> current_branch (number_of_nodes, static_cast<BranchNode*>(0));
00573 current_branch[0] = root_node_;
00574 assert (current_branch.back () != 0);
00575 this->buildLODRecursive (current_branch);
00576 }
00577
00579
00580 template<typename ContainerT, typename PointT> void
00581 OutofcoreOctreeBase<ContainerT, PointT>::printBoundingBox (OutofcoreOctreeBaseNode<ContainerT, PointT>& node) const
00582 {
00583 Eigen::Vector3d min, max;
00584 node.getBoundingBox (min,max);
00585 PCL_INFO ("[pcl::outofcore::OutofcoreOctreeBase::%s] min(%lf,%lf,%lf), max(%lf,%lf,%lf)\n", __FUNCTION__, min[0], min[1], min[2], max[0], max[1], max[2]);
00586 }
00587
00588
00590
00591 template<typename ContainerT, typename PointT> void
00592 OutofcoreOctreeBase<ContainerT, PointT>::buildLODRecursive (const std::vector<BranchNode*>& current_branch)
00593 {
00594 PCL_DEBUG ("%s Building LOD at depth %d",__PRETTY_FUNCTION__, current_branch.size ());
00595
00596 if (!current_branch.back ())
00597 {
00598 return;
00599 }
00600
00601 if (current_branch.back ()->getNodeType () == pcl::octree::LEAF_NODE)
00602 {
00603 assert (current_branch.back ()->getDepth () == this->getDepth ());
00604
00605 BranchNode* leaf = current_branch.back ();
00606
00607 pcl::PCLPointCloud2::Ptr leaf_input_cloud (new pcl::PCLPointCloud2 ());
00608
00609 leaf->read (leaf_input_cloud);
00610 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0);
00611
00612
00613 for (int64_t level = static_cast<int64_t>(current_branch.size ()-1); level >= 1; level--)
00614 {
00615 BranchNode* target_parent = current_branch[level-1];
00616 assert (target_parent != 0);
00617 double exponent = static_cast<double>(current_branch.size () - target_parent->getDepth ());
00618 double current_depth_sample_percent = pow (sample_percent_, exponent);
00619
00620 assert (current_depth_sample_percent > 0.0);
00621
00622
00623
00624
00625
00626
00627 lod_filter_ptr_->setInputCloud (leaf_input_cloud);
00628
00629
00630 uint64_t sample_size = static_cast<uint64_t> (static_cast<double> (leaf_input_cloud->width*leaf_input_cloud->height) * current_depth_sample_percent);
00631
00632 if (sample_size == 0)
00633 sample_size = 1;
00634
00635 lod_filter_ptr_->setSample (static_cast<unsigned int>(sample_size));
00636
00637
00638 pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ());
00639
00640
00641 pcl::IndicesPtr downsampled_cloud_indices (new std::vector< int > ());
00642 lod_filter_ptr_->filter (*downsampled_cloud_indices);
00643
00644
00645 pcl::ExtractIndices<pcl::PCLPointCloud2> extractor;
00646 extractor.setInputCloud (leaf_input_cloud);
00647 extractor.setIndices (downsampled_cloud_indices);
00648 extractor.filter (*downsampled_cloud);
00649
00650
00651 if (downsampled_cloud->width*downsampled_cloud->height > 0)
00652 {
00653 target_parent->payload_->insertRange (downsampled_cloud);
00654 this->incrementPointsInLOD (target_parent->getDepth (), downsampled_cloud->width*downsampled_cloud->height);
00655 }
00656 }
00657 }
00658 else
00659 {
00660
00661 current_branch.back ()->clearData ();
00662
00663 std::vector<BranchNode*> next_branch (current_branch);
00664
00665 if (current_branch.back ()->hasUnloadedChildren ())
00666 {
00667 current_branch.back ()->loadChildren (false);
00668 }
00669
00670 for (size_t i = 0; i < 8; i++)
00671 {
00672 next_branch.push_back (current_branch.back ()->getChildPtr (i));
00673
00674 if (next_branch.back () != 0)
00675 buildLODRecursive (next_branch);
00676
00677 next_branch.pop_back ();
00678 }
00679 }
00680 }
00682
00683 template<typename ContainerT, typename PointT> void
00684 OutofcoreOctreeBase<ContainerT, PointT>::incrementPointsInLOD (boost::uint64_t depth, boost::uint64_t new_point_count)
00685 {
00686 if (std::numeric_limits<uint64_t>::max () - metadata_->getLODPoints (depth) < new_point_count)
00687 {
00688 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBase::incrementPointsInLOD] Overflow error. Too many points in depth %d of outofcore octree with root at %s\n", depth, metadata_->getMetadataFilename().c_str());
00689 PCL_THROW_EXCEPTION (PCLException, "Overflow error");
00690 }
00691
00692 metadata_->setLODPoints (depth, new_point_count, true );
00693 }
00694
00696
00697 template<typename ContainerT, typename PointT> bool
00698 OutofcoreOctreeBase<ContainerT, PointT>::checkExtension (const boost::filesystem::path& path_name)
00699 {
00700 if (boost::filesystem::extension (path_name) != OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension)
00701 {
00702 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBase] Wrong root node file extension: %s. The tree must have a root node ending in %s\n", boost::filesystem::extension (path_name).c_str (), OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension.c_str () );
00703 return (0);
00704 }
00705
00706 return (1);
00707 }
00708
00710
00711 template<typename ContainerT, typename PointT> void
00712 OutofcoreOctreeBase<ContainerT, PointT>::enlargeToCube (Eigen::Vector3d& bb_min, Eigen::Vector3d& bb_max)
00713 {
00714 Eigen::Vector3d diff = bb_max - bb_min;
00715 assert (diff[0] > 0);
00716 assert (diff[1] > 0);
00717 assert (diff[2] > 0);
00718 Eigen::Vector3d center = (bb_max + bb_min)/2.0;
00719
00720 double max_sidelength = std::max (std::max (fabs (diff[0]), fabs (diff[1])), fabs (diff[2]));
00721 assert (max_sidelength > 0);
00722 bb_min = center - Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
00723 bb_max = center + Eigen::Vector3d (1.0, 1.0, 1.0)*(max_sidelength/2.0);
00724 }
00725
00727
00728 template<typename ContainerT, typename PointT> boost::uint64_t
00729 OutofcoreOctreeBase<ContainerT, PointT>::calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution)
00730 {
00731
00732 double side_length = max_bb[0] - min_bb[0];
00733
00734 if (side_length < leaf_resolution)
00735 return (0);
00736
00737 boost::uint64_t res = static_cast<boost::uint64_t> (std::ceil (log2f (static_cast<float> (side_length / leaf_resolution))));
00738
00739 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBase::calculateDepth] Setting depth to %d\n",res);
00740 return (res);
00741 }
00742 }
00743 }
00744
00745 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_