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 00041 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 00042 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 00043 00044 // C++ 00045 #include <iostream> 00046 #include <fstream> 00047 #include <sstream> 00048 #include <string> 00049 #include <exception> 00050 00051 #include <pcl/common/common.h> 00052 #include <pcl/visualization/common/common.h> 00053 #include <pcl/outofcore/octree_base_node.h> 00054 #include <pcl/filters/random_sample.h> 00055 #include <pcl/filters/extract_indices.h> 00056 00057 // JSON 00058 #include <pcl/outofcore/cJSON.h> 00059 00060 namespace pcl 00061 { 00062 namespace outofcore 00063 { 00064 00065 template<typename ContainerT, typename PointT> 00066 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename = "node"; 00067 00068 template<typename ContainerT, typename PointT> 00069 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename = "node"; 00070 00071 template<typename ContainerT, typename PointT> 00072 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension = ".oct_idx"; 00073 00074 template<typename ContainerT, typename PointT> 00075 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension = ".oct_dat"; 00076 00077 template<typename ContainerT, typename PointT> 00078 boost::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_; 00079 00080 template<typename ContainerT, typename PointT> 00081 boost::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rand_gen_; 00082 00083 template<typename ContainerT, typename PointT> 00084 const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125; 00085 00086 template<typename ContainerT, typename PointT> 00087 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension = ".pcd"; 00088 00089 template<typename ContainerT, typename PointT> 00090 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode () 00091 : m_tree_ () 00092 , root_node_ (NULL) 00093 , parent_ (NULL) 00094 , depth_ (0) 00095 , children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0))) 00096 , num_children_ (0) 00097 , num_loaded_children_ (0) 00098 , payload_ () 00099 , node_metadata_ () 00100 { 00101 node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ()); 00102 node_metadata_->setOutofcoreVersion (3); 00103 } 00104 00105 //////////////////////////////////////////////////////////////////////////////// 00106 00107 template<typename ContainerT, typename PointT> 00108 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const boost::filesystem::path& directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all) 00109 : m_tree_ () 00110 , root_node_ () 00111 , parent_ (super) 00112 , depth_ () 00113 , children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0))) 00114 , num_children_ (0) 00115 , num_loaded_children_ (0) 00116 , payload_ () 00117 , node_metadata_ () 00118 { 00119 node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ()); 00120 node_metadata_->setOutofcoreVersion (3); 00121 00122 //Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL) 00123 if (super == NULL) 00124 { 00125 node_metadata_->setDirectoryPathname (directory_path.parent_path ()); 00126 node_metadata_->setMetadataFilename (directory_path); 00127 depth_ = 0; 00128 root_node_ = this; 00129 00130 //Check if the specified directory to load currently exists; if not, don't continue 00131 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ())) 00132 { 00133 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ()); 00134 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory"); 00135 } 00136 } 00137 else 00138 { 00139 node_metadata_->setDirectoryPathname (directory_path); 00140 depth_ = super->getDepth () + 1; 00141 root_node_ = super->root_node_; 00142 00143 boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator 00144 00145 //flag to test if the desired metadata file was found 00146 bool b_loaded = 0; 00147 00148 for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it) 00149 { 00150 const boost::filesystem::path& file = *directory_it; 00151 00152 if (!boost::filesystem::is_directory (file)) 00153 { 00154 if (boost::filesystem::extension (file) == node_index_extension) 00155 { 00156 b_loaded = node_metadata_->loadMetadataFromDisk (file); 00157 break; 00158 } 00159 } 00160 } 00161 00162 if (!b_loaded) 00163 { 00164 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n"); 00165 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index"); 00166 } 00167 } 00168 00169 //load the metadata 00170 loadFromFile (node_metadata_->getMetadataFilename (), super); 00171 00172 //set the number of children in this node 00173 num_children_ = this->countNumChildren (); 00174 00175 if (load_all) 00176 { 00177 loadChildren (true); 00178 } 00179 } 00180 //////////////////////////////////////////////////////////////////////////////// 00181 00182 template<typename ContainerT, typename PointT> 00183 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name) 00184 : m_tree_ (tree) 00185 , root_node_ () 00186 , parent_ () 00187 , depth_ () 00188 , children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*> (0))) 00189 , num_children_ (0) 00190 , num_loaded_children_ (0) 00191 , payload_ () 00192 , node_metadata_ (new OutofcoreOctreeNodeMetadata ()) 00193 { 00194 assert (tree != NULL); 00195 node_metadata_->setOutofcoreVersion (3); 00196 init_root_node (bb_min, bb_max, tree, root_name); 00197 } 00198 00199 //////////////////////////////////////////////////////////////////////////////// 00200 00201 template<typename ContainerT, typename PointT> void 00202 OutofcoreOctreeBaseNode<ContainerT, PointT>::init_root_node (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path& root_name) 00203 { 00204 assert (tree != NULL); 00205 00206 parent_ = NULL; 00207 root_node_ = this; 00208 m_tree_ = tree; 00209 depth_ = 0; 00210 00211 //Mark the children as unallocated 00212 num_children_ = 0; 00213 00214 Eigen::Vector3d tmp_max = bb_max; 00215 Eigen::Vector3d tmp_min = bb_min; 00216 00217 // Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded 00218 double epsilon = 1e-8; 00219 tmp_max = tmp_max + epsilon*Eigen::Vector3d (1.0, 1.0, 1.0); 00220 00221 node_metadata_->setBoundingBox (tmp_min, tmp_max); 00222 node_metadata_->setDirectoryPathname (root_name.parent_path ()); 00223 node_metadata_->setOutofcoreVersion (3); 00224 00225 // If the root directory doesn't exist create it 00226 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ())) 00227 { 00228 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 00229 } 00230 // If the root directory is a file, do not continue 00231 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ())) 00232 { 00233 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ()); 00234 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists"); 00235 } 00236 00237 // Create a unique id for node file name 00238 std::string uuid; 00239 00240 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid); 00241 00242 std::string node_container_name; 00243 00244 node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension; 00245 00246 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ()); 00247 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name)); 00248 00249 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 00250 node_metadata_->serializeMetadataToDisk (); 00251 00252 // Create data container, ie octree_disk_container, octree_ram_container 00253 payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ())); 00254 } 00255 00256 //////////////////////////////////////////////////////////////////////////////// 00257 00258 template<typename ContainerT, typename PointT> 00259 OutofcoreOctreeBaseNode<ContainerT, PointT>::~OutofcoreOctreeBaseNode () 00260 { 00261 // Recursively delete all children and this nodes data 00262 recFreeChildren (); 00263 } 00264 00265 //////////////////////////////////////////////////////////////////////////////// 00266 00267 template<typename ContainerT, typename PointT> size_t 00268 OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumChildren () const 00269 { 00270 size_t child_count = 0; 00271 00272 for(size_t i=0; i<8; i++) 00273 { 00274 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 00275 if (boost::filesystem::exists (child_path)) 00276 child_count++; 00277 } 00278 return (child_count); 00279 } 00280 00281 //////////////////////////////////////////////////////////////////////////////// 00282 00283 template<typename ContainerT, typename PointT> void 00284 OutofcoreOctreeBaseNode<ContainerT, PointT>::saveIdx (bool recursive) 00285 { 00286 node_metadata_->serializeMetadataToDisk (); 00287 00288 if (recursive) 00289 { 00290 for (size_t i = 0; i < 8; i++) 00291 { 00292 if (children_[i]) 00293 children_[i]->saveIdx (true); 00294 } 00295 } 00296 } 00297 00298 //////////////////////////////////////////////////////////////////////////////// 00299 00300 template<typename ContainerT, typename PointT> bool 00301 OutofcoreOctreeBaseNode<ContainerT, PointT>::hasUnloadedChildren () const 00302 { 00303 if (this->getNumLoadedChildren () < this->getNumChildren ()) 00304 return (true); 00305 else 00306 return (false); 00307 } 00308 //////////////////////////////////////////////////////////////////////////////// 00309 00310 template<typename ContainerT, typename PointT> void 00311 OutofcoreOctreeBaseNode<ContainerT, PointT>::loadChildren (bool recursive) 00312 { 00313 //if we have fewer children loaded than exist on disk, load them 00314 if (num_loaded_children_ < this->getNumChildren ()) 00315 { 00316 //check all 8 possible child directories 00317 for (int i = 0; i < 8; i++) 00318 { 00319 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 00320 //if the directory exists and the child hasn't been created (set to 0 by this node's constructor) 00321 if (boost::filesystem::exists (child_dir) && this->children_[i] == 0) 00322 { 00323 //load the child node 00324 this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive); 00325 //keep track of the children loaded 00326 num_loaded_children_++; 00327 } 00328 } 00329 } 00330 assert (num_loaded_children_ == this->getNumChildren ()); 00331 } 00332 //////////////////////////////////////////////////////////////////////////////// 00333 00334 template<typename ContainerT, typename PointT> void 00335 OutofcoreOctreeBaseNode<ContainerT, PointT>::recFreeChildren () 00336 { 00337 if (num_children_ == 0) 00338 { 00339 return; 00340 } 00341 00342 for (size_t i = 0; i < 8; i++) 00343 { 00344 if (children_[i]) 00345 { 00346 OutofcoreOctreeBaseNode<ContainerT, PointT>* current = children_[i]; 00347 delete (current); 00348 } 00349 } 00350 children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0)); 00351 num_children_ = 0; 00352 } 00353 //////////////////////////////////////////////////////////////////////////////// 00354 00355 template<typename ContainerT, typename PointT> uint64_t 00356 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const AlignedPointTVector& p, const bool skip_bb_check) 00357 { 00358 //quit if there are no points to add 00359 if (p.empty ()) 00360 { 00361 return (0); 00362 } 00363 00364 //if this depth is the max depth of the tree, then add the points 00365 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 00366 return (addDataAtMaxDepth( p, skip_bb_check)); 00367 00368 if (hasUnloadedChildren ()) 00369 loadChildren (false); 00370 00371 std::vector < std::vector<const PointT*> > c; 00372 c.resize (8); 00373 for (size_t i = 0; i < 8; i++) 00374 { 00375 c[i].reserve (p.size () / 8); 00376 } 00377 00378 const size_t len = p.size (); 00379 for (size_t i = 0; i < len; i++) 00380 { 00381 const PointT& pt = p[i]; 00382 00383 if (!skip_bb_check) 00384 { 00385 if (!this->pointInBoundingBox (pt)) 00386 { 00387 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ ); 00388 continue; 00389 } 00390 } 00391 00392 uint8_t box = 0; 00393 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 00394 00395 box = static_cast<uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0)); 00396 c[static_cast<size_t>(box)].push_back (&pt); 00397 } 00398 00399 boost::uint64_t points_added = 0; 00400 for (size_t i = 0; i < 8; i++) 00401 { 00402 if (c[i].empty ()) 00403 continue; 00404 if (!children_[i]) 00405 createChild (i); 00406 points_added += children_[i]->addDataToLeaf (c[i], true); 00407 c[i].clear (); 00408 } 00409 return (points_added); 00410 } 00411 //////////////////////////////////////////////////////////////////////////////// 00412 00413 00414 template<typename ContainerT, typename PointT> boost::uint64_t 00415 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check) 00416 { 00417 if (p.empty ()) 00418 { 00419 return (0); 00420 } 00421 00422 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 00423 { 00424 //trust me, just add the points 00425 if (skip_bb_check) 00426 { 00427 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ()); 00428 00429 payload_->insertRange (p.data (), p.size ()); 00430 00431 return (p.size ()); 00432 } 00433 else//check which points belong to this node, throw away the rest 00434 { 00435 std::vector<const PointT*> buff; 00436 BOOST_FOREACH(const PointT* pt, p) 00437 { 00438 if(pointInBoundingBox(*pt)) 00439 { 00440 buff.push_back(pt); 00441 } 00442 } 00443 00444 if (!buff.empty ()) 00445 { 00446 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ()); 00447 payload_->insertRange (buff.data (), buff.size ()); 00448 // payload_->insertRange ( buff ); 00449 00450 } 00451 return (buff.size ()); 00452 } 00453 } 00454 else 00455 { 00456 if (this->hasUnloadedChildren ()) 00457 { 00458 loadChildren (false); 00459 } 00460 00461 std::vector < std::vector<const PointT*> > c; 00462 c.resize (8); 00463 for (size_t i = 0; i < 8; i++) 00464 { 00465 c[i].reserve (p.size () / 8); 00466 } 00467 00468 const size_t len = p.size (); 00469 for (size_t i = 0; i < len; i++) 00470 { 00471 //const PointT& pt = p[i]; 00472 if (!skip_bb_check) 00473 { 00474 if (!this->pointInBoundingBox (*p[i])) 00475 { 00476 // std::cerr << "failed to place point!!!" << std::endl; 00477 continue; 00478 } 00479 } 00480 00481 uint8_t box = 00; 00482 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 00483 //hash each coordinate to the appropriate octant 00484 box = static_cast<uint8_t> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] ))); 00485 //3 bit, 8 octants 00486 c[box].push_back (p[i]); 00487 } 00488 00489 boost::uint64_t points_added = 0; 00490 for (size_t i = 0; i < 8; i++) 00491 { 00492 if (c[i].empty ()) 00493 continue; 00494 if (!children_[i]) 00495 createChild (i); 00496 points_added += children_[i]->addDataToLeaf (c[i], true); 00497 c[i].clear (); 00498 } 00499 return (points_added); 00500 } 00501 // std::cerr << "failed to place point!!!" << std::endl; 00502 return (0); 00503 } 00504 //////////////////////////////////////////////////////////////////////////////// 00505 00506 00507 template<typename ContainerT, typename PointT> boost::uint64_t 00508 OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check) 00509 { 00510 assert (this->root_node_->m_tree_ != NULL); 00511 00512 if (input_cloud->height*input_cloud->width == 0) 00513 return (0); 00514 00515 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 00516 return (addDataAtMaxDepth (input_cloud, true)); 00517 00518 if( num_children_ < 8 ) 00519 if(hasUnloadedChildren ()) 00520 loadChildren (false); 00521 00522 if( skip_bb_check == false ) 00523 { 00524 00525 //indices to store the points for each bin 00526 //these lists will be used to copy data to new point clouds and pass down recursively 00527 std::vector < std::vector<int> > indices; 00528 indices.resize (8); 00529 00530 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ()); 00531 00532 for(size_t k=0; k<indices.size (); k++) 00533 { 00534 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k); 00535 } 00536 00537 boost::uint64_t points_added = 0; 00538 00539 for(size_t i=0; i<8; i++) 00540 { 00541 if ( indices[i].empty () ) 00542 continue; 00543 00544 if ( children_[i] == false ) 00545 { 00546 createChild (i); 00547 } 00548 00549 pcl::PCLPointCloud2::Ptr dst_cloud (new pcl::PCLPointCloud2 () ); 00550 00551 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__); 00552 00553 //copy the points from extracted indices from input cloud to destination cloud 00554 pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ; 00555 00556 //recursively add the new cloud to the data 00557 points_added += children_[i]->addPointCloud (dst_cloud, false); 00558 indices[i].clear (); 00559 } 00560 00561 return (points_added); 00562 } 00563 00564 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n"); 00565 00566 return 0; 00567 } 00568 00569 00570 //////////////////////////////////////////////////////////////////////////////// 00571 template<typename ContainerT, typename PointT> void 00572 OutofcoreOctreeBaseNode<ContainerT, PointT>::randomSample(const AlignedPointTVector& p, AlignedPointTVector& insertBuff, const bool skip_bb_check) 00573 { 00574 assert (this->root_node_->m_tree_ != NULL); 00575 00576 AlignedPointTVector sampleBuff; 00577 if (!skip_bb_check) 00578 { 00579 BOOST_FOREACH (const PointT& pt, p) 00580 if(pointInBoundingBox(pt)) 00581 sampleBuff.push_back(pt); 00582 } 00583 else 00584 { 00585 sampleBuff = p; 00586 } 00587 00588 // Derive percentage from specified sample_percent and tree depth 00589 const double percent = pow(sample_percent_, double((this->root_node_->m_tree_->getDepth () - depth_))); 00590 const uint64_t samplesize = static_cast<uint64_t>(percent * static_cast<double>(sampleBuff.size())); 00591 const uint64_t inputsize = sampleBuff.size(); 00592 00593 if(samplesize > 0) 00594 { 00595 // Resize buffer to sample size 00596 insertBuff.resize(samplesize); 00597 00598 // Create random number generator 00599 boost::mutex::scoped_lock lock(rng_mutex_); 00600 boost::uniform_int<boost::uint64_t> buffdist(0, inputsize-1); 00601 boost::variate_generator<boost::mt19937&, boost::uniform_int<boost::uint64_t> > buffdie(rand_gen_, buffdist); 00602 00603 // Randomly pick sampled points 00604 for(boost::uint64_t i = 0; i < samplesize; ++i) 00605 { 00606 boost::uint64_t buffstart = buffdie(); 00607 insertBuff[i] = ( sampleBuff[buffstart] ); 00608 } 00609 } 00610 // Have to do it the slow way 00611 else 00612 { 00613 boost::mutex::scoped_lock lock(rng_mutex_); 00614 boost::bernoulli_distribution<double> buffdist(percent); 00615 boost::variate_generator<boost::mt19937&, boost::bernoulli_distribution<double> > buffcoin(rand_gen_, buffdist); 00616 00617 for(boost::uint64_t i = 0; i < inputsize; ++i) 00618 if(buffcoin()) 00619 insertBuff.push_back( p[i] ); 00620 } 00621 } 00622 //////////////////////////////////////////////////////////////////////////////// 00623 00624 template<typename ContainerT, typename PointT> boost::uint64_t 00625 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const AlignedPointTVector& p, const bool skip_bb_check) 00626 { 00627 assert (this->root_node_->m_tree_ != NULL); 00628 00629 // Trust me, just add the points 00630 if (skip_bb_check) 00631 { 00632 // Increment point count for node 00633 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ()); 00634 00635 // Insert point data 00636 payload_->insertRange ( p ); 00637 00638 return (p.size ()); 00639 } 00640 00641 // Add points found within the current node's bounding box 00642 else 00643 { 00644 AlignedPointTVector buff; 00645 const size_t len = p.size (); 00646 00647 for (size_t i = 0; i < len; i++) 00648 { 00649 if (pointInBoundingBox (p[i])) 00650 { 00651 buff.push_back (p[i]); 00652 } 00653 } 00654 00655 if (!buff.empty ()) 00656 { 00657 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ()); 00658 payload_->insertRange ( buff ); 00659 00660 } 00661 return (buff.size ()); 00662 } 00663 } 00664 //////////////////////////////////////////////////////////////////////////////// 00665 template<typename ContainerT, typename PointT> boost::uint64_t 00666 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check) 00667 { 00668 //this assumes data is already in the correct bin 00669 if(skip_bb_check == true) 00670 { 00671 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_); 00672 00673 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height ); 00674 payload_->insertRange (input_cloud); 00675 00676 return (input_cloud->width*input_cloud->height); 00677 } 00678 else 00679 { 00680 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n"); 00681 return (0); 00682 } 00683 } 00684 00685 00686 //////////////////////////////////////////////////////////////////////////////// 00687 template<typename ContainerT, typename PointT> void 00688 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check) 00689 { 00690 // Reserve space for children nodes 00691 c.resize(8); 00692 for(size_t i = 0; i < 8; i++) 00693 c[i].reserve(p.size() / 8); 00694 00695 const size_t len = p.size(); 00696 for(size_t i = 0; i < len; i++) 00697 { 00698 const PointT& pt = p[i]; 00699 00700 if(!skip_bb_check) 00701 if(!this->pointInBoundingBox(pt)) 00702 continue; 00703 00704 subdividePoint (pt, c); 00705 } 00706 } 00707 //////////////////////////////////////////////////////////////////////////////// 00708 00709 template<typename ContainerT, typename PointT> void 00710 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c) 00711 { 00712 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 00713 size_t octant = 0; 00714 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0); 00715 c[octant].push_back (point); 00716 } 00717 00718 //////////////////////////////////////////////////////////////////////////////// 00719 template<typename ContainerT, typename PointT> boost::uint64_t 00720 OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud) //, const bool skip_bb_check = false ) 00721 { 00722 boost::uint64_t points_added = 0; 00723 00724 if ( input_cloud->width * input_cloud->height == 0 ) 00725 { 00726 return (0); 00727 } 00728 00729 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 ) 00730 { 00731 uint64_t points_added = addDataAtMaxDepth (input_cloud, true); 00732 assert (points_added > 0); 00733 return (points_added); 00734 } 00735 00736 if (num_children_ < 8 ) 00737 { 00738 if ( hasUnloadedChildren () ) 00739 { 00740 loadChildren (false); 00741 } 00742 } 00743 00744 //------------------------------------------------------------ 00745 //subsample data: 00746 // 1. Get indices from a random sample 00747 // 2. Extract those indices with the extract indices class (in order to also get the complement) 00748 //------------------------------------------------------------ 00749 pcl::RandomSample<pcl::PCLPointCloud2> random_sampler; 00750 random_sampler.setInputCloud (input_cloud); 00751 00752 //set sample size to 1/8 of total points (12.5%) 00753 uint64_t sample_size = input_cloud->width*input_cloud->height / 8; 00754 random_sampler.setSample (static_cast<unsigned int> (sample_size)); 00755 00756 //create our destination 00757 pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () ); 00758 00759 //create destination for indices 00760 pcl::IndicesPtr downsampled_cloud_indices ( new std::vector< int > () ); 00761 random_sampler.filter (*downsampled_cloud_indices); 00762 00763 //extract the "random subset", size by setSampleSize 00764 pcl::ExtractIndices<pcl::PCLPointCloud2> extractor; 00765 extractor.setInputCloud (input_cloud); 00766 extractor.setIndices (downsampled_cloud_indices); 00767 extractor.filter (*downsampled_cloud); 00768 00769 //extract the complement of those points (i.e. everything remaining) 00770 pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () ); 00771 extractor.setNegative (true); 00772 extractor.filter (*remaining_points); 00773 00774 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 ); 00775 00776 //insert subsampled data to the node's disk container payload 00777 if ( downsampled_cloud->width * downsampled_cloud->height != 0 ) 00778 { 00779 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height ); 00780 payload_->insertRange (downsampled_cloud); 00781 points_added += downsampled_cloud->width*downsampled_cloud->height ; 00782 } 00783 00784 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height); 00785 00786 //subdivide remaining data by destination octant 00787 std::vector<std::vector<int> > indices; 00788 indices.resize (8); 00789 00790 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ()); 00791 00792 //pass each set of points to the appropriate child octant 00793 for(size_t i=0; i<8; i++) 00794 { 00795 00796 if(indices[i].empty ()) 00797 continue; 00798 00799 if( children_[i] == false ) 00800 { 00801 assert (i < 8); 00802 createChild (i); 00803 } 00804 00805 //copy correct indices into a temporary cloud 00806 pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ()); 00807 pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud); 00808 00809 //recursively add points and keep track of how many were successfully added to the tree 00810 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud); 00811 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); 00812 00813 } 00814 assert (points_added == input_cloud->width*input_cloud->height); 00815 return (points_added); 00816 } 00817 //////////////////////////////////////////////////////////////////////////////// 00818 00819 template<typename ContainerT, typename PointT> boost::uint64_t 00820 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf_and_genLOD (const AlignedPointTVector& p, const bool skip_bb_check) 00821 { 00822 // If there are no points return 00823 if (p.empty ()) 00824 return (0); 00825 00826 // when adding data and generating sampled LOD 00827 // If the max depth has been reached 00828 assert (this->root_node_->m_tree_ != NULL ); 00829 00830 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 00831 { 00832 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n"); 00833 return (addDataAtMaxDepth(p, false)); 00834 } 00835 00836 // Create child nodes of the current node but not grand children+ 00837 if (this->hasUnloadedChildren ()) 00838 loadChildren (false /*no recursive loading*/); 00839 00840 // Randomly sample data 00841 AlignedPointTVector insertBuff; 00842 randomSample(p, insertBuff, skip_bb_check); 00843 00844 if(!insertBuff.empty()) 00845 { 00846 // Increment point count for node 00847 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size()); 00848 // Insert sampled point data 00849 payload_->insertRange (insertBuff); 00850 00851 } 00852 00853 //subdivide vec to pass data down lower 00854 std::vector<AlignedPointTVector> c; 00855 subdividePoints(p, c, skip_bb_check); 00856 00857 boost::uint64_t points_added = 0; 00858 for(size_t i = 0; i < 8; i++) 00859 { 00860 // If child doesn't have points 00861 if(c[i].empty()) 00862 continue; 00863 00864 // If child doesn't exist 00865 if(!children_[i]) 00866 createChild(i); 00867 00868 // Recursively build children 00869 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true); 00870 c[i].clear(); 00871 } 00872 00873 return (points_added); 00874 } 00875 //////////////////////////////////////////////////////////////////////////////// 00876 00877 template<typename ContainerT, typename PointT> void 00878 OutofcoreOctreeBaseNode<ContainerT, PointT>::createChild (const size_t idx) 00879 { 00880 assert (idx < 8); 00881 00882 //if already has 8 children, return 00883 if (children_[idx] || (num_children_ == 8)) 00884 { 00885 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s",this->node_metadata_->getMetadataFilename ().c_str ()); 00886 return; 00887 } 00888 00889 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin (); 00890 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0); 00891 00892 Eigen::Vector3d childbb_min; 00893 Eigen::Vector3d childbb_max; 00894 00895 int x, y, z; 00896 if (idx > 3) 00897 { 00898 x = ((idx == 5) || (idx == 7)) ? 1 : 0; 00899 y = ((idx == 6) || (idx == 7)) ? 1 : 0; 00900 z = 1; 00901 } 00902 else 00903 { 00904 x = ((idx == 1) || (idx == 3)) ? 1 : 0; 00905 y = ((idx == 2) || (idx == 3)) ? 1 : 0; 00906 z = 0; 00907 } 00908 00909 childbb_min[2] = start[2] + static_cast<double> (z) * step[2]; 00910 childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2]; 00911 00912 childbb_min[1] = start[1] + static_cast<double> (y) * step[1]; 00913 childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1]; 00914 00915 childbb_min[0] = start[0] + static_cast<double> (x) * step[0]; 00916 childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0]; 00917 00918 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (boost::lexical_cast<std::string> (idx)); 00919 children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this); 00920 00921 num_children_++; 00922 } 00923 //////////////////////////////////////////////////////////////////////////////// 00924 00925 template<typename ContainerT, typename PointT> bool 00926 pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point) 00927 { 00928 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) && 00929 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) && 00930 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2]))) 00931 { 00932 return (true); 00933 00934 } 00935 return (false); 00936 } 00937 00938 00939 //////////////////////////////////////////////////////////////////////////////// 00940 template<typename ContainerT, typename PointT> bool 00941 OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const PointT& p) const 00942 { 00943 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin (); 00944 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax (); 00945 00946 if (((min[0] <= p.x) && (p.x < max[0])) && 00947 ((min[1] <= p.y) && (p.y < max[1])) && 00948 ((min[2] <= p.z) && (p.z < max[2]))) 00949 { 00950 return (true); 00951 00952 } 00953 return (false); 00954 } 00955 00956 //////////////////////////////////////////////////////////////////////////////// 00957 template<typename ContainerT, typename PointT> void 00958 OutofcoreOctreeBaseNode<ContainerT, PointT>::printBoundingBox (const size_t query_depth) const 00959 { 00960 Eigen::Vector3d min; 00961 Eigen::Vector3d max; 00962 node_metadata_->getBoundingBox (min, max); 00963 00964 if (this->depth_ < query_depth){ 00965 for (size_t i = 0; i < this->depth_; i++) 00966 std::cout << " "; 00967 00968 std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - "; 00969 std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - "; 00970 std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1]; 00971 std::cout << ", " << max[2] - min[2] << "]" << std::endl; 00972 00973 if (num_children_ > 0) 00974 { 00975 for (size_t i = 0; i < 8; i++) 00976 { 00977 if (children_[i]) 00978 children_[i]->printBoundingBox (query_depth); 00979 } 00980 } 00981 } 00982 } 00983 00984 //////////////////////////////////////////////////////////////////////////////// 00985 template<typename ContainerT, typename PointT> void 00986 OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const size_t query_depth) 00987 { 00988 if (this->depth_ < query_depth){ 00989 if (num_children_ > 0) 00990 { 00991 for (size_t i = 0; i < 8; i++) 00992 { 00993 if (children_[i]) 00994 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth); 00995 } 00996 } 00997 } 00998 else 00999 { 01000 PointT voxel_center; 01001 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 01002 voxel_center.x = static_cast<float>(mid_xyz[0]); 01003 voxel_center.y = static_cast<float>(mid_xyz[1]); 01004 voxel_center.z = static_cast<float>(mid_xyz[2]); 01005 01006 voxel_centers.push_back(voxel_center); 01007 } 01008 } 01009 01010 //////////////////////////////////////////////////////////////////////////////// 01011 // Eigen::Vector3d cornerOffsets[] = 01012 // { 01013 // Eigen::Vector3d(-1.0, -1.0, -1.0), // - - - 01014 // Eigen::Vector3d( 1.0, -1.0, -1.0), // - - + 01015 // Eigen::Vector3d(-1.0, 1.0, -1.0), // - + - 01016 // Eigen::Vector3d( 1.0, 1.0, -1.0), // - + + 01017 // Eigen::Vector3d(-1.0, -1.0, 1.0), // + - - 01018 // Eigen::Vector3d( 1.0, -1.0, 1.0), // + - + 01019 // Eigen::Vector3d(-1.0, 1.0, 1.0), // + + - 01020 // Eigen::Vector3d( 1.0, 1.0, 1.0) // + + + 01021 // }; 01022 // 01023 // // Note that the input vector must already be negated when using this code for halfplane tests 01024 // int 01025 // vectorToIndex(Eigen::Vector3d normal) 01026 // { 01027 // int index = 0; 01028 // 01029 // if (normal.z () >= 0) index |= 1; 01030 // if (normal.y () >= 0) index |= 2; 01031 // if (normal.x () >= 0) index |= 4; 01032 // 01033 // return index; 01034 // } 01035 // 01036 // void 01037 // get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb) 01038 // { 01039 // 01040 // p_vertex = min_bb; 01041 // n_vertex = max_bb; 01042 // 01043 // if (normal.x () >= 0) 01044 // { 01045 // n_vertex.x () = min_bb.x (); 01046 // p_vertex.x () = max_bb.x (); 01047 // } 01048 // 01049 // if (normal.y () >= 0) 01050 // { 01051 // n_vertex.y () = min_bb.y (); 01052 // p_vertex.y () = max_bb.y (); 01053 // } 01054 // 01055 // if (normal.z () >= 0) 01056 // { 01057 // p_vertex.z () = max_bb.z (); 01058 // n_vertex.z () = min_bb.z (); 01059 // } 01060 // } 01061 01062 template<typename Container, typename PointT> void 01063 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) 01064 { 01065 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth()); 01066 } 01067 01068 template<typename Container, typename PointT> void 01069 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check) 01070 { 01071 01072 enum {INSIDE, INTERSECT, OUTSIDE}; 01073 01074 int result = INSIDE; 01075 01076 if (this->depth_ > query_depth) 01077 { 01078 return; 01079 } 01080 01081 // if (this->depth_ > query_depth) 01082 // return; 01083 01084 if (!skip_vfc_check) 01085 { 01086 for(int i =0; i < 6; i++){ 01087 double a = planes[(i*4)]; 01088 double b = planes[(i*4)+1]; 01089 double c = planes[(i*4)+2]; 01090 double d = planes[(i*4)+3]; 01091 01092 //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl; 01093 01094 Eigen::Vector3d normal(a, b, c); 01095 01096 Eigen::Vector3d min_bb; 01097 Eigen::Vector3d max_bb; 01098 node_metadata_->getBoundingBox(min_bb, max_bb); 01099 01100 // Basic VFC algorithm 01101 Eigen::Vector3d center = node_metadata_->getVoxelCenter(); 01102 Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())), 01103 fabs (static_cast<double> (max_bb.y () - center.y ())), 01104 fabs (static_cast<double> (max_bb.z () - center.z ()))); 01105 01106 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d; 01107 double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c)); 01108 01109 if (m + n < 0){ 01110 result = OUTSIDE; 01111 break; 01112 } 01113 01114 if (m - n < 0) result = INTERSECT; 01115 01116 // // n-p implementation 01117 // Eigen::Vector3d p_vertex; //pos vertex 01118 // Eigen::Vector3d n_vertex; //neg vertex 01119 // get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb); 01120 // 01121 // cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << endl; 01122 // cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << endl; 01123 01124 // is the positive vertex outside? 01125 // if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0) 01126 // { 01127 // result = OUTSIDE; 01128 // } 01129 // // is the negative vertex outside? 01130 // else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0) 01131 // { 01132 // result = INTERSECT; 01133 // } 01134 01135 // 01136 // 01137 // // This should be the same as below 01138 // if (normal.dot(n_vertex) + d > 0) 01139 // { 01140 // result = OUTSIDE; 01141 // } 01142 // 01143 // if (normal.dot(p_vertex) + d >= 0) 01144 // { 01145 // result = INTERSECT; 01146 // } 01147 01148 // This should be the same as above 01149 // double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ()); 01150 // cout << "m = " << m << endl; 01151 // if (m > -d) 01152 // { 01153 // result = OUTSIDE; 01154 // } 01155 // 01156 // double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ()); 01157 // cout << "n = " << n << endl; 01158 // if (n > -d) 01159 // { 01160 // result = INTERSECT; 01161 // } 01162 } 01163 } 01164 01165 if (result == OUTSIDE) 01166 { 01167 return; 01168 } 01169 01170 // switch(result){ 01171 // case OUTSIDE: 01172 // //cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << endl; 01173 // return; 01174 // case INTERSECT: 01175 // //cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << endl; 01176 // break; 01177 // case INSIDE: 01178 // //cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << endl; 01179 // break; 01180 // } 01181 01182 // Add files breadth first 01183 if (this->depth_ == query_depth && payload_->getDataSize () > 0) 01184 //if (payload_->getDataSize () > 0) 01185 { 01186 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 01187 } 01188 01189 if (hasUnloadedChildren ()) 01190 { 01191 loadChildren (false); 01192 } 01193 01194 if (this->getNumChildren () > 0) 01195 { 01196 for (size_t i = 0; i < 8; i++) 01197 { 01198 if (children_[i]) 01199 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/); 01200 } 01201 } 01202 // else if (hasUnloadedChildren ()) 01203 // { 01204 // loadChildren (false); 01205 // 01206 // for (size_t i = 0; i < 8; i++) 01207 // { 01208 // if (children_[i]) 01209 // children_[i]->queryFrustum (planes, file_names, query_depth); 01210 // } 01211 // } 01212 //} 01213 } 01214 01215 //////////////////////////////////////////////////////////////////////////////// 01216 01217 template<typename Container, typename PointT> void 01218 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check) 01219 { 01220 01221 // If we're above our query depth 01222 if (this->depth_ > query_depth) 01223 { 01224 return; 01225 } 01226 01227 // Bounding Box 01228 Eigen::Vector3d min_bb; 01229 Eigen::Vector3d max_bb; 01230 node_metadata_->getBoundingBox(min_bb, max_bb); 01231 01232 // Frustum Culling 01233 enum {INSIDE, INTERSECT, OUTSIDE}; 01234 01235 int result = INSIDE; 01236 01237 if (!skip_vfc_check) 01238 { 01239 for(int i =0; i < 6; i++){ 01240 double a = planes[(i*4)]; 01241 double b = planes[(i*4)+1]; 01242 double c = planes[(i*4)+2]; 01243 double d = planes[(i*4)+3]; 01244 01245 //cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << endl; 01246 01247 Eigen::Vector3d normal(a, b, c); 01248 01249 // Basic VFC algorithm 01250 Eigen::Vector3d center = node_metadata_->getVoxelCenter(); 01251 Eigen::Vector3d radius (fabs (static_cast<double> (max_bb.x () - center.x ())), 01252 fabs (static_cast<double> (max_bb.y () - center.y ())), 01253 fabs (static_cast<double> (max_bb.z () - center.z ()))); 01254 01255 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d; 01256 double n = (radius.x () * fabs(a)) + (radius.y () * fabs(b)) + (radius.z () * fabs(c)); 01257 01258 if (m + n < 0){ 01259 result = OUTSIDE; 01260 break; 01261 } 01262 01263 if (m - n < 0) result = INTERSECT; 01264 01265 } 01266 } 01267 01268 if (result == OUTSIDE) 01269 { 01270 return; 01271 } 01272 01273 // Bounding box projection 01274 // 3--------2 01275 // /| /| Y 0 = xmin, ymin, zmin 01276 // / | / | | 6 = xmax, ymax. zmax 01277 // 7--------6 | | 01278 // | | | | | 01279 // | 0-----|--1 +------X 01280 // | / | / / 01281 // |/ |/ / 01282 // 4--------5 Z 01283 01284 // bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0); 01285 // bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0); 01286 // bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0); 01287 // bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0); 01288 // bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0); 01289 // bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0); 01290 // bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0); 01291 // bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0); 01292 01293 int width = 500; 01294 int height = 500; 01295 01296 float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height); 01297 //float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix); 01298 01299 // for (int i=0; i < this->depth_; i++) std::cout << " "; 01300 // std::cout << this->depth_ << ": " << coverage << std::endl; 01301 01302 // Add files breadth first 01303 if (this->depth_ <= query_depth && payload_->getDataSize () > 0) 01304 //if (payload_->getDataSize () > 0) 01305 { 01306 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 01307 } 01308 01309 //if (coverage <= 0.075) 01310 if (coverage <= 10000) 01311 return; 01312 01313 if (hasUnloadedChildren ()) 01314 { 01315 loadChildren (false); 01316 } 01317 01318 if (this->getNumChildren () > 0) 01319 { 01320 for (size_t i = 0; i < 8; i++) 01321 { 01322 if (children_[i]) 01323 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/); 01324 } 01325 } 01326 } 01327 01328 //////////////////////////////////////////////////////////////////////////////// 01329 template<typename ContainerT, typename PointT> void 01330 OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const size_t query_depth) 01331 { 01332 if (this->depth_ < query_depth){ 01333 if (num_children_ > 0) 01334 { 01335 for (size_t i = 0; i < 8; i++) 01336 { 01337 if (children_[i]) 01338 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth); 01339 } 01340 } 01341 } 01342 else 01343 { 01344 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter (); 01345 voxel_centers.push_back(voxel_center); 01346 } 01347 } 01348 01349 01350 //////////////////////////////////////////////////////////////////////////////// 01351 01352 template<typename ContainerT, typename PointT> void 01353 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const boost::uint32_t query_depth, std::list<std::string>& file_names) 01354 { 01355 01356 Eigen::Vector3d my_min = min_bb; 01357 Eigen::Vector3d my_max = max_bb; 01358 01359 if (intersectsWithBoundingBox (my_min, my_max)) 01360 { 01361 if (this->depth_ < query_depth) 01362 { 01363 if (this->getNumChildren () > 0) 01364 { 01365 for (size_t i = 0; i < 8; i++) 01366 { 01367 if (children_[i]) 01368 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names); 01369 } 01370 } 01371 else if (hasUnloadedChildren ()) 01372 { 01373 loadChildren (false); 01374 01375 for (size_t i = 0; i < 8; i++) 01376 { 01377 if (children_[i]) 01378 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names); 01379 } 01380 } 01381 return; 01382 } 01383 01384 if (payload_->getDataSize () > 0) 01385 { 01386 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 01387 } 01388 } 01389 } 01390 //////////////////////////////////////////////////////////////////////////////// 01391 01392 template<typename ContainerT, typename PointT> void 01393 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, size_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) 01394 { 01395 uint64_t startingSize = dst_blob->width*dst_blob->height; 01396 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize ); 01397 01398 // If the queried bounding box has any intersection with this node's bounding box 01399 if (intersectsWithBoundingBox (min_bb, max_bb)) 01400 { 01401 // If we aren't at the max desired depth 01402 if (this->depth_ < query_depth) 01403 { 01404 //if this node doesn't have any children, we are at the max depth for this query 01405 if ((num_children_ == 0) && (hasUnloadedChildren ())) 01406 loadChildren (false); 01407 01408 //if this node has children 01409 if (num_children_ > 0) 01410 { 01411 //recursively store any points that fall into the queried bounding box into v and return 01412 for (size_t i = 0; i < 8; i++) 01413 { 01414 if (children_[i]) 01415 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob); 01416 } 01417 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 01418 return; 01419 } 01420 } 01421 else //otherwise if we are at the max depth 01422 { 01423 //get all the points from the payload and return (easy with PCLPointCloud2) 01424 pcl::PCLPointCloud2::Ptr tmp_blob (new pcl::PCLPointCloud2 ()); 01425 pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ()); 01426 //load all the data in this node from disk 01427 payload_->readRange (0, payload_->size (), tmp_blob); 01428 01429 if( tmp_blob->width*tmp_blob->height == 0 ) 01430 return; 01431 01432 //if this node's bounding box falls completely within the queried bounding box, keep all the points 01433 if (inBoundingBox (min_bb, max_bb)) 01434 { 01435 //concatenate all of what was just read into the main dst_blob 01436 //(is it safe to do in place?) 01437 01438 //if there is already something in the destination blob (remember this method is recursive) 01439 if( dst_blob->width*dst_blob->height != 0 ) 01440 { 01441 PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 01442 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__); 01443 int res = pcl::concatenatePointCloud (*dst_blob, *tmp_blob, *dst_blob); 01444 (void)res; 01445 assert (res == 1); 01446 01447 PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 01448 } 01449 //otherwise, just copy the tmp_blob into the dst_blob 01450 else 01451 { 01452 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n"); 01453 pcl::copyPointCloud (*tmp_blob, *dst_blob); 01454 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height); 01455 } 01456 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 01457 return; 01458 } 01459 else 01460 { 01461 //otherwise queried bounding box only partially intersects this 01462 //node's bounding box, so we have to check all the points in 01463 //this box for intersection with queried bounding box 01464 01465 01466 // PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Partial extraction of points in bounding box. Desired: %.2lf %.2lf %2lf, %.2lf %.2lf %.2lf; This node BB: %.2lf %.2lf %.2lf, %.2lf %.2lf %.2lf\n", __FUNCTION__, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2], min_[0], min_[1], min_[2], max_[0], max_[1], max_[2] ); 01467 01468 //put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox) 01469 typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () ); 01470 pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud ); 01471 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height ); 01472 01473 Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f); 01474 Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f ); 01475 01476 std::vector<int> indices; 01477 01478 pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices ); 01479 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () ); 01480 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () ); 01481 01482 if ( indices.size () > 0 ) 01483 { 01484 if( dst_blob->width*dst_blob->height > 0 ) 01485 { 01486 //need a new tmp destination with extracted points within BB 01487 pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ()); 01488 01489 //copy just the points marked in indices 01490 pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb ); 01491 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () ); 01492 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () ); 01493 //concatenate those points into the returned dst_blob 01494 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__); 01495 boost::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height; 01496 (void)orig_points_in_destination; 01497 int res = pcl::concatenatePointCloud (*dst_blob, *tmp_blob_within_bb, *dst_blob); 01498 (void)res; 01499 assert (res == 1); 01500 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination); 01501 01502 } 01503 else 01504 { 01505 pcl::copyPointCloud ( *tmp_blob, indices, *dst_blob ); 01506 assert ( dst_blob->width*dst_blob->height == indices.size () ); 01507 } 01508 } 01509 } 01510 } 01511 } 01512 01513 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize ); 01514 } 01515 01516 template<typename ContainerT, typename PointT> void 01517 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, size_t query_depth, AlignedPointTVector& v) 01518 { 01519 01520 //if the queried bounding box has any intersection with this node's bounding box 01521 if (intersectsWithBoundingBox (min_bb, max_bb)) 01522 { 01523 //if we aren't at the max desired depth 01524 if (this->depth_ < query_depth) 01525 { 01526 //if this node doesn't have any children, we are at the max depth for this query 01527 if (this->hasUnloadedChildren ()) 01528 { 01529 this->loadChildren (false); 01530 } 01531 01532 //if this node has children 01533 if (getNumChildren () > 0) 01534 { 01535 if(hasUnloadedChildren ()) 01536 loadChildren (false); 01537 01538 //recursively store any points that fall into the queried bounding box into v and return 01539 for (size_t i = 0; i < 8; i++) 01540 { 01541 if (children_[i]) 01542 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v); 01543 } 01544 return; 01545 } 01546 } 01547 //otherwise if we are at the max depth 01548 else 01549 { 01550 //if this node's bounding box falls completely within the queried bounding box 01551 if (inBoundingBox (min_bb, max_bb)) 01552 { 01553 //get all the points from the payload and return 01554 AlignedPointTVector payload_cache; 01555 payload_->readRange (0, payload_->size (), payload_cache); 01556 v.insert (v.end (), payload_cache.begin (), payload_cache.end ()); 01557 return; 01558 } 01559 //otherwise queried bounding box only partially intersects this 01560 //node's bounding box, so we have to check all the points in 01561 //this box for intersection with queried bounding box 01562 else 01563 { 01564 //read _all_ the points in from the disk container 01565 AlignedPointTVector payload_cache; 01566 payload_->readRange (0, payload_->size (), payload_cache); 01567 01568 uint64_t len = payload_->size (); 01569 //iterate through each of them 01570 for (uint64_t i = 0; i < len; i++) 01571 { 01572 const PointT& p = payload_cache[i]; 01573 //if it falls within this bounding box 01574 if (pointInBoundingBox (min_bb, max_bb, p)) 01575 { 01576 //store it in the list 01577 v.push_back (p); 01578 } 01579 else 01580 { 01581 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]); 01582 } 01583 } 01584 } 01585 } 01586 } 01587 } 01588 01589 //////////////////////////////////////////////////////////////////////////////// 01590 template<typename ContainerT, typename PointT> void 01591 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent) 01592 { 01593 if (intersectsWithBoundingBox (min_bb, max_bb)) 01594 { 01595 if (this->depth_ < query_depth) 01596 { 01597 if (this->hasUnloadedChildren ()) 01598 this->loadChildren (false); 01599 01600 if (this->getNumChildren () > 0) 01601 { 01602 for (size_t i=0; i<8; i++) 01603 { 01604 //recursively traverse (depth first) 01605 if (children_[i]!=0) 01606 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent); 01607 } 01608 return; 01609 } 01610 } 01611 //otherwise, at max depth --> read from disk, subsample, concatenate 01612 else 01613 { 01614 01615 if (inBoundingBox (min_bb, max_bb)) 01616 { 01617 pcl::PCLPointCloud2::Ptr tmp_blob; 01618 this->payload_->read (tmp_blob); 01619 uint64_t num_pts = tmp_blob->width*tmp_blob->height; 01620 01621 double sample_points = static_cast<double>(num_pts) * percent; 01622 if (num_pts > 0) 01623 { 01624 //always sample at least one point 01625 sample_points = sample_points > 0 ? sample_points : 1; 01626 } 01627 else 01628 { 01629 return; 01630 } 01631 01632 01633 pcl::RandomSample<pcl::PCLPointCloud2> random_sampler; 01634 random_sampler.setInputCloud (tmp_blob); 01635 01636 pcl::PCLPointCloud2::Ptr downsampled_points (new pcl::PCLPointCloud2 ()); 01637 01638 //set sample size as percent * number of points read 01639 random_sampler.setSample (static_cast<unsigned int> (sample_points)); 01640 01641 pcl::ExtractIndices<pcl::PCLPointCloud2> extractor; 01642 01643 pcl::IndicesPtr downsampled_cloud_indices (new std::vector<int> ()); 01644 random_sampler.filter (*downsampled_cloud_indices); 01645 extractor.setIndices (downsampled_cloud_indices); 01646 extractor.filter (*downsampled_points); 01647 01648 //concatenate the result into the destination cloud 01649 pcl::concatenatePointCloud (*dst_blob, *downsampled_points, *dst_blob); 01650 } 01651 } 01652 } 01653 } 01654 01655 01656 //////////////////////////////////////////////////////////////////////////////// 01657 template<typename ContainerT, typename PointT> void 01658 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, boost::uint64_t query_depth, const double percent, AlignedPointTVector& dst) 01659 { 01660 //check if the queried bounding box has any intersection with this node's bounding box 01661 if (intersectsWithBoundingBox (min_bb, max_bb)) 01662 { 01663 //if we are not at the max depth for queried nodes 01664 if (this->depth_ < query_depth) 01665 { 01666 //check if we don't have children 01667 if ((num_children_ == 0) && (hasUnloadedChildren ())) 01668 { 01669 loadChildren (false); 01670 } 01671 //if we do have children 01672 if (num_children_ > 0) 01673 { 01674 //recursively add their valid points within the queried bounding box to the list v 01675 for (size_t i = 0; i < 8; i++) 01676 { 01677 if (children_[i]) 01678 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst); 01679 } 01680 return; 01681 } 01682 } 01683 //otherwise we are at the max depth, so we add all our points or some of our points 01684 else 01685 { 01686 //if this node's bounding box falls completely within the queried bounding box 01687 if (inBoundingBox (min_bb, max_bb)) 01688 { 01689 //add a random sample of all the points 01690 AlignedPointTVector payload_cache; 01691 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache); 01692 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ()); 01693 return; 01694 } 01695 //otherwise the queried bounding box only partially intersects with this node's bounding box 01696 else 01697 { 01698 //brute force selection of all valid points 01699 AlignedPointTVector payload_cache_within_region; 01700 { 01701 AlignedPointTVector payload_cache; 01702 payload_->readRange (0, payload_->size (), payload_cache); 01703 for (size_t i = 0; i < payload_->size (); i++) 01704 { 01705 const PointT& p = payload_cache[i]; 01706 if (pointInBoundingBox (min_bb, max_bb, p)) 01707 { 01708 payload_cache_within_region.push_back (p); 01709 } 01710 } 01711 }//force the payload cache to deconstruct here 01712 01713 //use STL random_shuffle and push back a random selection of the points onto our list 01714 std::random_shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end ()); 01715 size_t numpick = static_cast<size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));; 01716 01717 for (size_t i = 0; i < numpick; i++) 01718 { 01719 dst.push_back (payload_cache_within_region[i]); 01720 } 01721 } 01722 } 01723 } 01724 } 01725 //////////////////////////////////////////////////////////////////////////////// 01726 01727 //dir is current level. we put this nodes files into it 01728 template<typename ContainerT, typename PointT> 01729 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super) 01730 : m_tree_ () 01731 , root_node_ () 01732 , parent_ () 01733 , depth_ () 01734 , children_ (std::vector <OutofcoreOctreeBaseNode<ContainerT, PointT>*> (8,static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(0))) 01735 , num_children_ () 01736 , num_loaded_children_ (0) 01737 , payload_ () 01738 , node_metadata_ () 01739 { 01740 node_metadata_ = boost::shared_ptr<OutofcoreOctreeNodeMetadata> (new OutofcoreOctreeNodeMetadata ()); 01741 node_metadata_->setOutofcoreVersion (3); 01742 01743 if (super == NULL) 01744 { 01745 PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" ); 01746 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent"); 01747 } 01748 01749 this->parent_ = super; 01750 root_node_ = super->root_node_; 01751 m_tree_ = super->root_node_->m_tree_; 01752 assert (m_tree_ != NULL); 01753 01754 depth_ = super->depth_ + 1; 01755 num_children_ = 0; 01756 01757 node_metadata_->setBoundingBox (bb_min, bb_max); 01758 01759 std::string uuid_idx; 01760 std::string uuid_cont; 01761 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_idx); 01762 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_cont); 01763 01764 std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension; 01765 01766 std::string node_container_name; 01767 node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension; 01768 01769 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir)); 01770 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name)); 01771 node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name)); 01772 01773 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 01774 01775 payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ())); 01776 this->saveIdx (false); 01777 } 01778 01779 //////////////////////////////////////////////////////////////////////////////// 01780 01781 template<typename ContainerT, typename PointT> void 01782 OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec (std::list<PointT>& v) 01783 { 01784 if ((num_children_ == 0) && (hasUnloadedChildren ())) 01785 { 01786 loadChildren (false); 01787 } 01788 01789 for (size_t i = 0; i < num_children_; i++) 01790 { 01791 children_[i]->copyAllCurrentAndChildPointsRec (v); 01792 } 01793 01794 AlignedPointTVector payload_cache; 01795 payload_->readRange (0, payload_->size (), payload_cache); 01796 01797 { 01798 //boost::mutex::scoped_lock lock(queryBBIncludes_vector_mutex); 01799 v.insert (v.end (), payload_cache.begin (), payload_cache.end ()); 01800 } 01801 } 01802 01803 //////////////////////////////////////////////////////////////////////////////// 01804 01805 template<typename ContainerT, typename PointT> void 01806 OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec_sub (std::list<PointT>& v, const double percent) 01807 { 01808 if ((num_children_ == 0) && (hasUnloadedChildren ())) 01809 { 01810 loadChildren (false); 01811 } 01812 01813 for (size_t i = 0; i < 8; i++) 01814 { 01815 if (children_[i]) 01816 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent); 01817 } 01818 01819 std::vector<PointT> payload_cache; 01820 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache); 01821 01822 for (size_t i = 0; i < payload_cache.size (); i++) 01823 { 01824 v.push_back (payload_cache[i]); 01825 } 01826 } 01827 01828 //////////////////////////////////////////////////////////////////////////////// 01829 01830 template<typename ContainerT, typename PointT> inline bool 01831 OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const 01832 { 01833 Eigen::Vector3d min, max; 01834 node_metadata_->getBoundingBox (min, max); 01835 01836 //Check whether any portion of min_bb, max_bb falls within min,max 01837 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0]))) 01838 { 01839 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1]))) 01840 { 01841 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2]))) 01842 { 01843 return (true); 01844 } 01845 } 01846 } 01847 01848 return (false); 01849 } 01850 //////////////////////////////////////////////////////////////////////////////// 01851 01852 template<typename ContainerT, typename PointT> inline bool 01853 OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const 01854 { 01855 Eigen::Vector3d min, max; 01856 01857 node_metadata_->getBoundingBox (min, max); 01858 01859 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0])) 01860 { 01861 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1])) 01862 { 01863 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2])) 01864 { 01865 return (true); 01866 } 01867 } 01868 } 01869 01870 return (false); 01871 } 01872 //////////////////////////////////////////////////////////////////////////////// 01873 01874 template<typename ContainerT, typename PointT> inline bool 01875 OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, 01876 const PointT& p) 01877 { 01878 //by convention, minimum boundary is included; maximum boundary is not 01879 if ((min_bb[0] <= p.x) && (p.x < max_bb[0])) 01880 { 01881 if ((min_bb[1] <= p.y) && (p.y < max_bb[1])) 01882 { 01883 if ((min_bb[2] <= p.z) && (p.z < max_bb[2])) 01884 { 01885 return (true); 01886 } 01887 } 01888 } 01889 return (false); 01890 } 01891 01892 //////////////////////////////////////////////////////////////////////////////// 01893 01894 template<typename ContainerT, typename PointT> void 01895 OutofcoreOctreeBaseNode<ContainerT, PointT>::writeVPythonVisual (std::ofstream& file) 01896 { 01897 Eigen::Vector3d min; 01898 Eigen::Vector3d max; 01899 node_metadata_->getBoundingBox (min, max); 01900 01901 double l = max[0] - min[0]; 01902 double h = max[1] - min[1]; 01903 double w = max[2] - min[2]; 01904 file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h 01905 << ", width=" << w << " )\n"; 01906 01907 for (size_t i = 0; i < num_children_; i++) 01908 { 01909 children_[i]->writeVPythonVisual (file); 01910 } 01911 } 01912 01913 //////////////////////////////////////////////////////////////////////////////// 01914 01915 template<typename ContainerT, typename PointT> int 01916 OutofcoreOctreeBaseNode<ContainerT, PointT>::read (pcl::PCLPointCloud2::Ptr &output_cloud) 01917 { 01918 return (this->payload_->read (output_cloud)); 01919 } 01920 01921 //////////////////////////////////////////////////////////////////////////////// 01922 01923 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>* 01924 OutofcoreOctreeBaseNode<ContainerT, PointT>::getChildPtr (size_t index_arg) const 01925 { 01926 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg); 01927 return (children_[index_arg]); 01928 } 01929 01930 //////////////////////////////////////////////////////////////////////////////// 01931 template<typename ContainerT, typename PointT> boost::uint64_t 01932 OutofcoreOctreeBaseNode<ContainerT, PointT>::getDataSize () const 01933 { 01934 return (this->payload_->getDataSize ()); 01935 } 01936 01937 //////////////////////////////////////////////////////////////////////////////// 01938 01939 template<typename ContainerT, typename PointT> size_t 01940 OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumLoadedChildren () const 01941 { 01942 size_t loaded_children_count = 0; 01943 01944 for (size_t i=0; i<8; i++) 01945 { 01946 if (children_[i] != 0) 01947 loaded_children_count++; 01948 } 01949 01950 return (loaded_children_count); 01951 } 01952 01953 //////////////////////////////////////////////////////////////////////////////// 01954 01955 template<typename ContainerT, typename PointT> void 01956 OutofcoreOctreeBaseNode<ContainerT, PointT>::loadFromFile (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super) 01957 { 01958 PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ()); 01959 node_metadata_->loadMetadataFromDisk (path); 01960 01961 //this shouldn't be part of 'loadFromFile' 01962 this->parent_ = super; 01963 01964 if (num_children_ > 0) 01965 recFreeChildren (); 01966 01967 this->num_children_ = 0; 01968 this->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (node_metadata_->getPCDFilename ())); 01969 } 01970 01971 //////////////////////////////////////////////////////////////////////////////// 01972 01973 template<typename ContainerT, typename PointT> void 01974 OutofcoreOctreeBaseNode<ContainerT, PointT>::convertToXYZRecursive () 01975 { 01976 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz"); 01977 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname; 01978 payload_->convertToXYZ (xyzfile); 01979 01980 if (hasUnloadedChildren ()) 01981 { 01982 loadChildren (false); 01983 } 01984 01985 for (size_t i = 0; i < 8; i++) 01986 { 01987 if (children_[i]) 01988 children_[i]->convertToXYZ (); 01989 } 01990 } 01991 01992 //////////////////////////////////////////////////////////////////////////////// 01993 01994 template<typename ContainerT, typename PointT> void 01995 OutofcoreOctreeBaseNode<ContainerT, PointT>::flushToDiskRecursive () 01996 { 01997 for (size_t i = 0; i < 8; i++) 01998 { 01999 if (children_[i]) 02000 children_[i]->flushToDiskRecursive (); 02001 } 02002 } 02003 02004 //////////////////////////////////////////////////////////////////////////////// 02005 02006 template<typename ContainerT, typename PointT> void 02007 OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz) 02008 { 02009 if (indices.size () < 8) 02010 indices.resize (8); 02011 02012 int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") ); 02013 int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") ); 02014 int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") ); 02015 02016 int x_offset = input_cloud->fields[x_idx].offset; 02017 int y_offset = input_cloud->fields[y_idx].offset; 02018 int z_offset = input_cloud->fields[z_idx].offset; 02019 02020 for ( size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step ) 02021 { 02022 PointT local_pt; 02023 02024 local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset])); 02025 local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset])); 02026 local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset])); 02027 02028 if (!pcl_isfinite (local_pt.x) || !pcl_isfinite (local_pt.y) || !pcl_isfinite (local_pt.z)) 02029 continue; 02030 02031 if(!this->pointInBoundingBox (local_pt)) 02032 { 02033 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); 02034 } 02035 02036 assert (this->pointInBoundingBox (local_pt) == true); 02037 02038 //compute the box we are in 02039 size_t box = 0; 02040 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0); 02041 assert (box < 8); 02042 02043 //insert to the vector of indices 02044 indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step)); 02045 } 02046 } 02047 //////////////////////////////////////////////////////////////////////////////// 02048 02049 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated 02050 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>* 02051 makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super) 02052 { 02053 OutofcoreOctreeBaseNode<ContainerT, PointT>* thisnode = new OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer < PointT > , PointT > (); 02054 //octree_disk_node (); 02055 02056 if (super == NULL) 02057 { 02058 thisnode->thisdir_ = path.parent_path (); 02059 02060 if (!boost::filesystem::exists (thisnode->thisdir_)) 02061 { 02062 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () ); 02063 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory"); 02064 } 02065 02066 thisnode->thisnodeindex_ = path; 02067 02068 thisnode->depth_ = 0; 02069 thisnode->root_node_ = thisnode; 02070 } 02071 else 02072 { 02073 thisnode->thisdir_ = path; 02074 thisnode->depth_ = super->depth_ + 1; 02075 thisnode->root_node_ = super->root_node_; 02076 02077 if (thisnode->depth_ > thisnode->root->max_depth_) 02078 { 02079 thisnode->root->max_depth_ = thisnode->depth_; 02080 } 02081 02082 boost::filesystem::directory_iterator diterend; 02083 bool loaded = false; 02084 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter) 02085 { 02086 const boost::filesystem::path& file = *diter; 02087 if (!boost::filesystem::is_directory (file)) 02088 { 02089 if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension) 02090 { 02091 thisnode->thisnodeindex_ = file; 02092 loaded = true; 02093 break; 02094 } 02095 } 02096 } 02097 02098 if (!loaded) 02099 { 02100 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n"); 02101 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file"); 02102 } 02103 02104 } 02105 thisnode->max_depth_ = 0; 02106 02107 { 02108 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in); 02109 02110 f >> thisnode->min_[0]; 02111 f >> thisnode->min_[1]; 02112 f >> thisnode->min_[2]; 02113 f >> thisnode->max_[0]; 02114 f >> thisnode->max_[1]; 02115 f >> thisnode->max_[2]; 02116 02117 std::string filename; 02118 f >> filename; 02119 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename; 02120 02121 f.close (); 02122 02123 thisnode->payload_ = boost::shared_ptr<ContainerT> (new ContainerT (thisnode->thisnodestorage_)); 02124 } 02125 02126 thisnode->parent_ = super; 02127 children_.clear (); 02128 children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0)); 02129 thisnode->num_children_ = 0; 02130 02131 return (thisnode); 02132 } 02133 02134 //////////////////////////////////////////////////////////////////////////////// 02135 02136 //accelerate search 02137 template<typename ContainerT, typename PointT> void 02138 queryBBIntersects_noload (const boost::filesystem::path& root_node, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name) 02139 { 02140 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL); 02141 if (root == NULL) 02142 { 02143 std::cout << "test"; 02144 } 02145 if (root->intersectsWithBoundingBox (min, max)) 02146 { 02147 if (query_depth == root->max_depth_) 02148 { 02149 if (!root->payload_->empty ()) 02150 { 02151 bin_name.push_back (root->thisnodestorage_.string ()); 02152 } 02153 return; 02154 } 02155 02156 for (int i = 0; i < 8; i++) 02157 { 02158 boost::filesystem::path child_dir = root->thisdir_ 02159 / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 02160 if (boost::filesystem::exists (child_dir)) 02161 { 02162 root->children_[i] = makenode_norec (child_dir, root); 02163 root->num_children_++; 02164 queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name); 02165 } 02166 } 02167 } 02168 delete root; 02169 } 02170 02171 //////////////////////////////////////////////////////////////////////////////// 02172 02173 template<typename ContainerT, typename PointT> void 02174 queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::uint32_t query_depth, std::list<std::string>& bin_name) 02175 { 02176 if (current->intersectsWithBoundingBox (min, max)) 02177 { 02178 if (current->depth_ == query_depth) 02179 { 02180 if (!current->payload_->empty ()) 02181 { 02182 bin_name.push_back (current->thisnodestorage_.string ()); 02183 } 02184 } 02185 else 02186 { 02187 for (int i = 0; i < 8; i++) 02188 { 02189 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 02190 if (boost::filesystem::exists (child_dir)) 02191 { 02192 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current); 02193 current->num_children_++; 02194 queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name); 02195 } 02196 } 02197 } 02198 } 02199 } 02200 #endif 02201 //////////////////////////////////////////////////////////////////////////////// 02202 02203 }//namespace outofcore 02204 }//namespace pcl 02205 02206 //#define PCL_INSTANTIATE.... 02207 02208 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_