Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012, Urban Robotics, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of Willow Garage, Inc. nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 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 // JSON 00047 #include <pcl/outofcore/cJSON.h> 00048 00049 #include <pcl/filters/random_sample.h> 00050 #include <pcl/filters/extract_indices.h> 00051 00052 // C++ 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 00064 //////////////////////////////////////////////////////////////////////////////// 00065 //Static constants 00066 //////////////////////////////////////////////////////////////////////////////// 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 00074 //////////////////////////////////////////////////////////////////////////////// 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 //validate the root filename 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 // Create root_node_node 00091 root_node_ = new OutofcoreOctreeBaseNode<ContainerT, PointT> (root_name, NULL, load_all); 00092 // Set root_node_nodes tree to the newly created tree 00093 root_node_->m_tree_ = this; 00094 00095 // Set the path to the outofcore octree metadata (unique to the root folder) ending in .octree 00096 boost::filesystem::path treepath = root_name.parent_path () / (boost::filesystem::basename (root_name) + TREE_EXTENSION_); 00097 00098 //Load the JSON metadata 00099 metadata_->loadMetadataFromDisk (treepath); 00100 } 00101 00102 //////////////////////////////////////////////////////////////////////////////// 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 //Enlarge the bounding box to a cube so our voxels will be cubes 00113 Eigen::Vector3d tmp_min = min; 00114 Eigen::Vector3d tmp_max = max; 00115 this->enlargeToCube (tmp_min, tmp_max); 00116 00117 //Compute the depth of the tree given the resolution 00118 boost::uint64_t depth = this->calculateDepth (tmp_min, tmp_max, resolution_arg); 00119 00120 //Create a new outofcore tree 00121 this->init (depth, tmp_min, tmp_max, root_node_name, coord_sys); 00122 } 00123 00124 //////////////////////////////////////////////////////////////////////////////// 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 //Create a new outofcore tree 00135 this->init (max_depth, min, max, root_node_name, coord_sys); 00136 } 00137 00138 //////////////////////////////////////////////////////////////////////////////// 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 //Validate the extension of the pathname 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 //Check to make sure that we are not overwriting existing data 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 // Get fullpath and recreate directories 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 // Create root node 00168 root_node_= new OutofcoreOctreeBaseNode<ContainerT, PointT> (tmp_min, tmp_max, this, root_name); 00169 root_node_->m_tree_ = this; 00170 00171 // Set root nodes file path 00172 boost::filesystem::path treepath = dir / (boost::filesystem::basename (root_name) + TREE_EXTENSION_); 00173 00174 //fill the fields of the metadata 00175 metadata_->setCoordinateSystem (coord_sys); 00176 metadata_->setDepth (depth); 00177 metadata_->setLODPoints (depth+1); 00178 metadata_->setMetadataFilename (treepath); 00179 metadata_->setOutofcoreVersion (OUTOFCORE_VERSION_); 00180 //metadata_->setPointType ( <point type string here> ); 00181 00182 //save to disk 00183 metadata_->serializeMetadataToDisk (); 00184 } 00185 00186 00187 //////////////////////////////////////////////////////////////////////////////// 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 00197 //////////////////////////////////////////////////////////////////////////////// 00198 00199 template<typename ContainerT, typename PointT> void 00200 OutofcoreOctreeBase<ContainerT, PointT>::saveToFile () 00201 { 00202 this->metadata_->serializeMetadataToDisk (); 00203 } 00204 00205 //////////////////////////////////////////////////////////////////////////////// 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 00221 //////////////////////////////////////////////////////////////////////////////// 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 00229 //////////////////////////////////////////////////////////////////////////////// 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 // assert (input_cloud->width*input_cloud->height == pt_added); 00236 return (pt_added); 00237 } 00238 00239 00240 //////////////////////////////////////////////////////////////////////////////// 00241 00242 template<typename ContainerT, typename PointT> boost::uint64_t 00243 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (PointCloudConstPtr point_cloud) 00244 { 00245 // Lock the tree while writing 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 00251 //////////////////////////////////////////////////////////////////////////////// 00252 00253 template<typename ContainerT, typename PointT> boost::uint64_t 00254 OutofcoreOctreeBase<ContainerT, PointT>::addPointCloud_and_genLOD (pcl::PCLPointCloud2::Ptr &input_cloud) 00255 { 00256 // Lock the tree while writing 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 00267 //////////////////////////////////////////////////////////////////////////////// 00268 00269 template<typename ContainerT, typename PointT> boost::uint64_t 00270 OutofcoreOctreeBase<ContainerT, PointT>::addDataToLeaf_and_genLOD (AlignedPointTVector& src) 00271 { 00272 // Lock the tree while writing 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 00278 //////////////////////////////////////////////////////////////////////////////// 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 00287 //////////////////////////////////////////////////////////////////////////////// 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 00296 //////////////////////////////////////////////////////////////////////////////// 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 00310 //////////////////////////////////////////////////////////////////////////////// 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 00321 //////////////////////////////////////////////////////////////////////////////// 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 00335 //////////////////////////////////////////////////////////////////////////////// 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 00345 //////////////////////////////////////////////////////////////////////////////// 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 00359 //////////////////////////////////////////////////////////////////////////////// 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 00372 //////////////////////////////////////////////////////////////////////////////// 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 00381 //////////////////////////////////////////////////////////////////////////////// 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 00397 //////////////////////////////////////////////////////////////////////////////// 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 00413 //////////////////////////////////////////////////////////////////////////////// 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 00430 //////////////////////////////////////////////////////////////////////////////// 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 00442 //////////////////////////////////////////////////////////////////////////////// 00443 00444 template<typename ContainerT, typename PointT> void 00445 OutofcoreOctreeBase<ContainerT, PointT>::flushToDisk () 00446 { 00447 root_node_->flushToDisk (); 00448 } 00449 00450 //////////////////////////////////////////////////////////////////////////////// 00451 00452 template<typename ContainerT, typename PointT> void 00453 OutofcoreOctreeBase<ContainerT, PointT>::flushToDiskLazy () 00454 { 00455 root_node_->flushToDiskLazy (); 00456 } 00457 00458 //////////////////////////////////////////////////////////////////////////////// 00459 00460 template<typename ContainerT, typename PointT> void 00461 OutofcoreOctreeBase<ContainerT, PointT>::convertToXYZ () 00462 { 00463 saveToFile (); 00464 root_node_->convertToXYZ (); 00465 } 00466 00467 //////////////////////////////////////////////////////////////////////////////// 00468 00469 template<typename ContainerT, typename PointT> void 00470 OutofcoreOctreeBase<ContainerT, PointT>::DeAllocEmptyNodeCache () 00471 { 00472 DeAllocEmptyNodeCache (root_node_); 00473 } 00474 00475 //////////////////////////////////////////////////////////////////////////////// 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 00492 //////////////////////////////////////////////////////////////////////////////// 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 00499 //////////////////////////////////////////////////////////////////////////////// 00500 template<typename ContainerT, typename PointT> pcl::Filter<pcl::PCLPointCloud2>::Ptr 00501 OutofcoreOctreeBase<ContainerT, PointT>::getLODFilter () 00502 { 00503 return (lod_filter_ptr_); 00504 } 00505 00506 //////////////////////////////////////////////////////////////////////////////// 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 00514 //////////////////////////////////////////////////////////////////////////////// 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 00522 //////////////////////////////////////////////////////////////////////////////// 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 00546 //////////////////////////////////////////////////////////////////////////////// 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 00557 //////////////////////////////////////////////////////////////////////////////// 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 00578 //////////////////////////////////////////////////////////////////////////////// 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 00589 //////////////////////////////////////////////////////////////////////////////// 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 //read the data from the PCD file associated with the leaf; it is full resolution 00609 leaf->read (leaf_input_cloud); 00610 assert (leaf_input_cloud->width*leaf_input_cloud->height > 0); 00611 00612 //go up the tree, re-downsampling the full resolution leaf cloud at lower and lower resolution 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 //subsample data: 00623 // 1. Get indices from a random sample 00624 // 2. Extract those indices with the extract indices class (in order to also get the complement) 00625 //------------------------------------------------------------ 00626 00627 lod_filter_ptr_->setInputCloud (leaf_input_cloud); 00628 00629 //set sample size to 1/8 of total points (12.5%) 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 //create our destination 00638 pcl::PCLPointCloud2::Ptr downsampled_cloud (new pcl::PCLPointCloud2 ()); 00639 00640 //create destination for indices 00641 pcl::IndicesPtr downsampled_cloud_indices (new std::vector< int > ()); 00642 lod_filter_ptr_->filter (*downsampled_cloud_indices); 00643 00644 //extract the "random subset", size by setSampleSize 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 //write to the target 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//not at leaf, keep going down 00659 { 00660 //clear this node while walking down the tree in case we are updating the LOD 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 //skip that child if it doesn't exist 00674 if (next_branch.back () != 0) 00675 buildLODRecursive (next_branch); 00676 00677 next_branch.pop_back (); 00678 } 00679 } 00680 } 00681 //////////////////////////////////////////////////////////////////////////////// 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 /*true->increment*/); 00693 } 00694 00695 //////////////////////////////////////////////////////////////////////////////// 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 00709 //////////////////////////////////////////////////////////////////////////////// 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 00726 //////////////////////////////////////////////////////////////////////////////// 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 //Assume cube 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 }//namespace outofcore 00743 }//namespace pcl 00744 00745 #endif //PCL_OUTOFCORE_OCTREE_BASE_IMPL_H_