1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010-2012, Willow Garage, Inc. 6 * Copyright (c) 2012, Urban Robotics, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of Willow Garage, Inc. nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #ifndef PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 42 #define PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 43 44 // C++ 45 #include <iostream> 46 #include <fstream> 47 #include <random> 48 #include <sstream> 49 #include <string> 50 #include <exception> 51 52 #include <pcl/common/common.h> 53 #include <pcl/common/utils.h> // pcl::utils::ignore 54 #include <pcl/visualization/common/common.h> 55 #include <pcl/outofcore/octree_base_node.h> 56 #include <pcl/filters/random_sample.h> 57 #include <pcl/filters/extract_indices.h> 58 59 // JSON 60 #include <pcl/outofcore/cJSON.h> 61 62 namespace pcl 63 { 64 namespace outofcore 65 { 66 67 template<typename ContainerT, typename PointT> 68 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_basename = "node"; 69 70 template<typename ContainerT, typename PointT> 71 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_basename = "node"; 72 73 template<typename ContainerT, typename PointT> 74 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension = ".oct_idx"; 75 76 template<typename ContainerT, typename PointT> 77 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::node_container_extension = ".oct_dat"; 78 79 template<typename ContainerT, typename PointT> 80 std::mutex OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_mutex_; 81 82 template<typename ContainerT, typename PointT> 83 std::mt19937 OutofcoreOctreeBaseNode<ContainerT, PointT>::rng_; 84 85 template<typename ContainerT, typename PointT> 86 const double OutofcoreOctreeBaseNode<ContainerT, PointT>::sample_percent_ = .125; 87 88 template<typename ContainerT, typename PointT> 89 const std::string OutofcoreOctreeBaseNode<ContainerT, PointT>::pcd_extension = ".pcd"; 90 91 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode()92 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode () 93 : m_tree_ () 94 , root_node_ (NULL) 95 , parent_ (NULL) 96 , depth_ (0) 97 , children_ (8, nullptr) 98 , num_children_ (0) 99 , num_loaded_children_ (0) 100 , payload_ () 101 , node_metadata_ (new OutofcoreOctreeNodeMetadata) 102 { 103 node_metadata_->setOutofcoreVersion (3); 104 } 105 106 //////////////////////////////////////////////////////////////////////////////// 107 108 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode(const boost::filesystem::path & directory_path,OutofcoreOctreeBaseNode<ContainerT,PointT> * super,bool load_all)109 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const boost::filesystem::path& directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all) 110 : m_tree_ () 111 , root_node_ () 112 , parent_ (super) 113 , depth_ () 114 , children_ (8, nullptr) 115 , num_children_ (0) 116 , num_loaded_children_ (0) 117 , payload_ () 118 , node_metadata_ (new OutofcoreOctreeNodeMetadata) 119 { 120 node_metadata_->setOutofcoreVersion (3); 121 122 //Check if this is the first node created/loaded (this is true if super, i.e. node's parent is NULL) 123 if (super == nullptr) 124 { 125 node_metadata_->setDirectoryPathname (directory_path.parent_path ()); 126 node_metadata_->setMetadataFilename (directory_path); 127 depth_ = 0; 128 root_node_ = this; 129 130 //Check if the specified directory to load currently exists; if not, don't continue 131 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ())) 132 { 133 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find dir %s\n", node_metadata_->getDirectoryPathname ().c_str ()); 134 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: missing directory"); 135 } 136 } 137 else 138 { 139 node_metadata_->setDirectoryPathname (directory_path); 140 depth_ = super->getDepth () + 1; 141 root_node_ = super->root_node_; 142 143 boost::filesystem::directory_iterator directory_it_end; //empty constructor creates end of iterator 144 145 //flag to test if the desired metadata file was found 146 bool b_loaded = false; 147 148 for (boost::filesystem::directory_iterator directory_it (node_metadata_->getDirectoryPathname ()); directory_it != directory_it_end; ++directory_it) 149 { 150 const boost::filesystem::path& file = *directory_it; 151 152 if (!boost::filesystem::is_directory (file)) 153 { 154 if (boost::filesystem::extension (file) == node_index_extension) 155 { 156 b_loaded = node_metadata_->loadMetadataFromDisk (file); 157 break; 158 } 159 } 160 } 161 162 if (!b_loaded) 163 { 164 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index\n"); 165 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore: Could not find node index"); 166 } 167 } 168 169 //load the metadata 170 loadFromFile (node_metadata_->getMetadataFilename (), super); 171 172 //set the number of children in this node 173 num_children_ = this->countNumChildren (); 174 175 if (load_all) 176 { 177 loadChildren (true); 178 } 179 } 180 //////////////////////////////////////////////////////////////////////////////// 181 182 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode(const Eigen::Vector3d & bb_min,const Eigen::Vector3d & bb_max,OutofcoreOctreeBase<ContainerT,PointT> * const tree,const boost::filesystem::path & root_name)183 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) 184 : m_tree_ (tree) 185 , root_node_ () 186 , parent_ () 187 , depth_ () 188 , children_ (8, nullptr) 189 , num_children_ (0) 190 , num_loaded_children_ (0) 191 , payload_ () 192 , node_metadata_ (new OutofcoreOctreeNodeMetadata ()) 193 { 194 assert (tree != NULL); 195 node_metadata_->setOutofcoreVersion (3); 196 init_root_node (bb_min, bb_max, tree, root_name); 197 } 198 199 //////////////////////////////////////////////////////////////////////////////// 200 201 template<typename ContainerT, typename PointT> void init_root_node(const Eigen::Vector3d & bb_min,const Eigen::Vector3d & bb_max,OutofcoreOctreeBase<ContainerT,PointT> * const tree,const boost::filesystem::path & root_name)202 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) 203 { 204 assert (tree != NULL); 205 206 parent_ = nullptr; 207 root_node_ = this; 208 m_tree_ = tree; 209 depth_ = 0; 210 211 //Mark the children as unallocated 212 num_children_ = 0; 213 214 Eigen::Vector3d tmp_max = bb_max; 215 Eigen::Vector3d tmp_min = bb_min; 216 217 // Need to make the bounding box slightly bigger so points that fall on the max side aren't excluded 218 double epsilon = 1e-8; 219 tmp_max += epsilon*Eigen::Vector3d (1.0, 1.0, 1.0); 220 221 node_metadata_->setBoundingBox (tmp_min, tmp_max); 222 node_metadata_->setDirectoryPathname (root_name.parent_path ()); 223 node_metadata_->setOutofcoreVersion (3); 224 225 // If the root directory doesn't exist create it 226 if (!boost::filesystem::exists (node_metadata_->getDirectoryPathname ())) 227 { 228 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 229 } 230 // If the root directory is a file, do not continue 231 else if (!boost::filesystem::is_directory (node_metadata_->getDirectoryPathname ())) 232 { 233 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Need empty directory structure. Dir %s exists and is a file.\n",node_metadata_->getDirectoryPathname ().c_str ()); 234 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Bad Path: Directory Already Exists"); 235 } 236 237 // Create a unique id for node file name 238 std::string uuid; 239 240 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid); 241 242 std::string node_container_name; 243 244 node_container_name = uuid + std::string ("_") + node_container_basename + pcd_extension; 245 246 node_metadata_->setMetadataFilename (node_metadata_->getDirectoryPathname () / root_name.filename ()); 247 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name)); 248 249 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 250 node_metadata_->serializeMetadataToDisk (); 251 252 // Create data container, ie octree_disk_container, octree_ram_container 253 payload_.reset (new ContainerT (node_metadata_->getPCDFilename ())); 254 } 255 256 //////////////////////////////////////////////////////////////////////////////// 257 258 template<typename ContainerT, typename PointT> ~OutofcoreOctreeBaseNode()259 OutofcoreOctreeBaseNode<ContainerT, PointT>::~OutofcoreOctreeBaseNode () 260 { 261 // Recursively delete all children and this nodes data 262 recFreeChildren (); 263 } 264 265 //////////////////////////////////////////////////////////////////////////////// 266 267 template<typename ContainerT, typename PointT> std::size_t countNumChildren() const268 OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumChildren () const 269 { 270 std::size_t child_count = 0; 271 272 for(std::size_t i=0; i<8; i++) 273 { 274 boost::filesystem::path child_path = this->node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i)); 275 if (boost::filesystem::exists (child_path)) 276 child_count++; 277 } 278 return (child_count); 279 } 280 281 //////////////////////////////////////////////////////////////////////////////// 282 283 template<typename ContainerT, typename PointT> void saveIdx(bool recursive)284 OutofcoreOctreeBaseNode<ContainerT, PointT>::saveIdx (bool recursive) 285 { 286 node_metadata_->serializeMetadataToDisk (); 287 288 if (recursive) 289 { 290 for (std::size_t i = 0; i < 8; i++) 291 { 292 if (children_[i]) 293 children_[i]->saveIdx (true); 294 } 295 } 296 } 297 298 //////////////////////////////////////////////////////////////////////////////// 299 300 template<typename ContainerT, typename PointT> bool hasUnloadedChildren() const301 OutofcoreOctreeBaseNode<ContainerT, PointT>::hasUnloadedChildren () const 302 { 303 return (this->getNumLoadedChildren () < this->getNumChildren ()); 304 } 305 //////////////////////////////////////////////////////////////////////////////// 306 307 template<typename ContainerT, typename PointT> void loadChildren(bool recursive)308 OutofcoreOctreeBaseNode<ContainerT, PointT>::loadChildren (bool recursive) 309 { 310 //if we have fewer children loaded than exist on disk, load them 311 if (num_loaded_children_ < this->getNumChildren ()) 312 { 313 //check all 8 possible child directories 314 for (int i = 0; i < 8; i++) 315 { 316 boost::filesystem::path child_dir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(i)); 317 //if the directory exists and the child hasn't been created (set to 0 by this node's constructor) 318 if (boost::filesystem::exists (child_dir) && this->children_[i] == nullptr) 319 { 320 //load the child node 321 this->children_[i] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (child_dir, this, recursive); 322 //keep track of the children loaded 323 num_loaded_children_++; 324 } 325 } 326 } 327 assert (num_loaded_children_ == this->getNumChildren ()); 328 } 329 //////////////////////////////////////////////////////////////////////////////// 330 331 template<typename ContainerT, typename PointT> void recFreeChildren()332 OutofcoreOctreeBaseNode<ContainerT, PointT>::recFreeChildren () 333 { 334 if (num_children_ == 0) 335 { 336 return; 337 } 338 339 for (std::size_t i = 0; i < 8; i++) 340 { 341 delete static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>*>(children_[i]); 342 } 343 children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (nullptr)); 344 num_children_ = 0; 345 } 346 //////////////////////////////////////////////////////////////////////////////// 347 348 template<typename ContainerT, typename PointT> std::uint64_t addDataToLeaf(const AlignedPointTVector & p,const bool skip_bb_check)349 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const AlignedPointTVector& p, const bool skip_bb_check) 350 { 351 //quit if there are no points to add 352 if (p.empty ()) 353 { 354 return (0); 355 } 356 357 //if this depth is the max depth of the tree, then add the points 358 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 359 return (addDataAtMaxDepth( p, skip_bb_check)); 360 361 if (hasUnloadedChildren ()) 362 loadChildren (false); 363 364 std::vector < std::vector<const PointT*> > c; 365 c.resize (8); 366 for (std::size_t i = 0; i < 8; i++) 367 { 368 c[i].reserve (p.size () / 8); 369 } 370 371 const std::size_t len = p.size (); 372 for (std::size_t i = 0; i < len; i++) 373 { 374 const PointT& pt = p[i]; 375 376 if (!skip_bb_check) 377 { 378 if (!this->pointInBoundingBox (pt)) 379 { 380 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Failed to place point within bounding box\n", __FUNCTION__ ); 381 continue; 382 } 383 } 384 385 std::uint8_t box = 0; 386 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 387 388 box = static_cast<std::uint8_t>(((pt.z >= mid_xyz[2]) << 2) | ((pt.y >= mid_xyz[1]) << 1) | ((pt.x >= mid_xyz[0]) << 0)); 389 c[static_cast<std::size_t>(box)].push_back (&pt); 390 } 391 392 std::uint64_t points_added = 0; 393 for (std::size_t i = 0; i < 8; i++) 394 { 395 if (c[i].empty ()) 396 continue; 397 if (!children_[i]) 398 createChild (i); 399 points_added += children_[i]->addDataToLeaf (c[i], true); 400 c[i].clear (); 401 } 402 return (points_added); 403 } 404 //////////////////////////////////////////////////////////////////////////////// 405 406 407 template<typename ContainerT, typename PointT> std::uint64_t addDataToLeaf(const std::vector<const PointT * > & p,const bool skip_bb_check)408 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf (const std::vector<const PointT*>& p, const bool skip_bb_check) 409 { 410 if (p.empty ()) 411 { 412 return (0); 413 } 414 415 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 416 { 417 //trust me, just add the points 418 if (skip_bb_check) 419 { 420 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ()); 421 422 payload_->insertRange (p.data (), p.size ()); 423 424 return (p.size ()); 425 } 426 //check which points belong to this node, throw away the rest 427 std::vector<const PointT*> buff; 428 for (const PointT* pt : p) 429 { 430 if(pointInBoundingBox(*pt)) 431 { 432 buff.push_back(pt); 433 } 434 } 435 436 if (!buff.empty ()) 437 { 438 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ()); 439 payload_->insertRange (buff.data (), buff.size ()); 440 // payload_->insertRange ( buff ); 441 442 } 443 return (buff.size ()); 444 } 445 446 if (this->hasUnloadedChildren ()) 447 { 448 loadChildren (false); 449 } 450 451 std::vector < std::vector<const PointT*> > c; 452 c.resize (8); 453 for (std::size_t i = 0; i < 8; i++) 454 { 455 c[i].reserve (p.size () / 8); 456 } 457 458 const std::size_t len = p.size (); 459 for (std::size_t i = 0; i < len; i++) 460 { 461 //const PointT& pt = p[i]; 462 if (!skip_bb_check) 463 { 464 if (!this->pointInBoundingBox (*p[i])) 465 { 466 // std::cerr << "failed to place point!!!" << std::endl; 467 continue; 468 } 469 } 470 471 std::uint8_t box = 00; 472 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 473 //hash each coordinate to the appropriate octant 474 box = static_cast<std::uint8_t> (((p[i]->z >= mid_xyz[2]) << 2) | ((p[i]->y >= mid_xyz[1]) << 1) | ((p[i]->x >= mid_xyz[0] ))); 475 //3 bit, 8 octants 476 c[box].push_back (p[i]); 477 } 478 479 std::uint64_t points_added = 0; 480 for (std::size_t i = 0; i < 8; i++) 481 { 482 if (c[i].empty ()) 483 continue; 484 if (!children_[i]) 485 createChild (i); 486 points_added += children_[i]->addDataToLeaf (c[i], true); 487 c[i].clear (); 488 } 489 return (points_added); 490 } 491 //////////////////////////////////////////////////////////////////////////////// 492 493 494 template<typename ContainerT, typename PointT> std::uint64_t addPointCloud(const typename pcl::PCLPointCloud2::Ptr & input_cloud,const bool skip_bb_check)495 OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud (const typename pcl::PCLPointCloud2::Ptr& input_cloud, const bool skip_bb_check) 496 { 497 assert (this->root_node_->m_tree_ != NULL); 498 499 if (input_cloud->height*input_cloud->width == 0) 500 return (0); 501 502 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 503 return (addDataAtMaxDepth (input_cloud, true)); 504 505 if( num_children_ < 8 ) 506 if(hasUnloadedChildren ()) 507 loadChildren (false); 508 509 if( !skip_bb_check ) 510 { 511 512 //indices to store the points for each bin 513 //these lists will be used to copy data to new point clouds and pass down recursively 514 std::vector < pcl::Indices > indices; 515 indices.resize (8); 516 517 this->sortOctantIndices (input_cloud, indices, node_metadata_->getVoxelCenter ()); 518 519 for(std::size_t k=0; k<indices.size (); k++) 520 { 521 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Computed %d indices in octact %d\n", __FUNCTION__, indices[k].size (), k); 522 } 523 524 std::uint64_t points_added = 0; 525 526 for(std::size_t i=0; i<8; i++) 527 { 528 if ( indices[i].empty () ) 529 continue; 530 531 if (children_[i] == nullptr) 532 { 533 createChild (i); 534 } 535 536 pcl::PCLPointCloud2::Ptr dst_cloud (new pcl::PCLPointCloud2 () ); 537 538 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Extracting indices to bins\n", __FUNCTION__); 539 540 //copy the points from extracted indices from input cloud to destination cloud 541 pcl::copyPointCloud ( *input_cloud, indices[i], *dst_cloud ) ; 542 543 //recursively add the new cloud to the data 544 points_added += children_[i]->addPointCloud (dst_cloud, false); 545 indices[i].clear (); 546 } 547 548 return (points_added); 549 } 550 551 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Skipped bounding box check. Points not inserted\n"); 552 553 return 0; 554 } 555 556 557 //////////////////////////////////////////////////////////////////////////////// 558 template<typename ContainerT, typename PointT> void randomSample(const AlignedPointTVector & p,AlignedPointTVector & insertBuff,const bool skip_bb_check)559 OutofcoreOctreeBaseNode<ContainerT, PointT>::randomSample(const AlignedPointTVector& p, AlignedPointTVector& insertBuff, const bool skip_bb_check) 560 { 561 assert (this->root_node_->m_tree_ != NULL); 562 563 AlignedPointTVector sampleBuff; 564 if (!skip_bb_check) 565 { 566 for (const PointT& pt: p) 567 { 568 if (pointInBoundingBox(pt)) 569 { 570 sampleBuff.push_back(pt); 571 } 572 } 573 } 574 else 575 { 576 sampleBuff = p; 577 } 578 579 // Derive percentage from specified sample_percent and tree depth 580 const double percent = pow(sample_percent_, double((this->root_node_->m_tree_->getDepth () - depth_))); 581 const std::uint64_t samplesize = static_cast<std::uint64_t>(percent * static_cast<double>(sampleBuff.size())); 582 const std::uint64_t inputsize = sampleBuff.size(); 583 584 if(samplesize > 0) 585 { 586 // Resize buffer to sample size 587 insertBuff.resize(samplesize); 588 589 // Create random number generator 590 std::lock_guard<std::mutex> lock(rng_mutex_); 591 std::uniform_int_distribution<std::uint64_t> buffdist(0, inputsize-1); 592 593 // Randomly pick sampled points 594 for(std::uint64_t i = 0; i < samplesize; ++i) 595 { 596 std::uint64_t buffstart = buffdist(rng_); 597 insertBuff[i] = ( sampleBuff[buffstart] ); 598 } 599 } 600 // Have to do it the slow way 601 else 602 { 603 std::lock_guard<std::mutex> lock(rng_mutex_); 604 std::bernoulli_distribution buffdist(percent); 605 606 for(std::uint64_t i = 0; i < inputsize; ++i) 607 if(buffdist(rng_)) 608 insertBuff.push_back( p[i] ); 609 } 610 } 611 //////////////////////////////////////////////////////////////////////////////// 612 613 template<typename ContainerT, typename PointT> std::uint64_t addDataAtMaxDepth(const AlignedPointTVector & p,const bool skip_bb_check)614 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const AlignedPointTVector& p, const bool skip_bb_check) 615 { 616 assert (this->root_node_->m_tree_ != NULL); 617 618 // Trust me, just add the points 619 if (skip_bb_check) 620 { 621 // Increment point count for node 622 root_node_->m_tree_->incrementPointsInLOD (this->depth_, p.size ()); 623 624 // Insert point data 625 payload_->insertRange ( p ); 626 627 return (p.size ()); 628 } 629 630 // Add points found within the current node's bounding box 631 AlignedPointTVector buff; 632 const std::size_t len = p.size (); 633 634 for (std::size_t i = 0; i < len; i++) 635 { 636 if (pointInBoundingBox (p[i])) 637 { 638 buff.push_back (p[i]); 639 } 640 } 641 642 if (!buff.empty ()) 643 { 644 root_node_->m_tree_->incrementPointsInLOD (this->depth_, buff.size ()); 645 payload_->insertRange ( buff ); 646 } 647 return (buff.size ()); 648 } 649 //////////////////////////////////////////////////////////////////////////////// 650 template<typename ContainerT, typename PointT> std::uint64_t addDataAtMaxDepth(const pcl::PCLPointCloud2::Ptr input_cloud,const bool skip_bb_check)651 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check) 652 { 653 //this assumes data is already in the correct bin 654 if(skip_bb_check) 655 { 656 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Adding %u points at max depth, %u\n",__FUNCTION__, input_cloud->width*input_cloud->height, this->depth_); 657 658 this->root_node_->m_tree_->incrementPointsInLOD (this->depth_, input_cloud->width*input_cloud->height ); 659 payload_->insertRange (input_cloud); 660 661 return (input_cloud->width*input_cloud->height); 662 } 663 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Not implemented\n"); 664 return (0); 665 } 666 667 668 //////////////////////////////////////////////////////////////////////////////// 669 template<typename ContainerT, typename PointT> void subdividePoints(const AlignedPointTVector & p,std::vector<AlignedPointTVector> & c,const bool skip_bb_check)670 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check) 671 { 672 // Reserve space for children nodes 673 c.resize(8); 674 for(std::size_t i = 0; i < 8; i++) 675 c[i].reserve(p.size() / 8); 676 677 const std::size_t len = p.size(); 678 for(std::size_t i = 0; i < len; i++) 679 { 680 const PointT& pt = p[i]; 681 682 if(!skip_bb_check) 683 if(!this->pointInBoundingBox(pt)) 684 continue; 685 686 subdividePoint (pt, c); 687 } 688 } 689 //////////////////////////////////////////////////////////////////////////////// 690 691 template<typename ContainerT, typename PointT> void subdividePoint(const PointT & point,std::vector<AlignedPointTVector> & c)692 OutofcoreOctreeBaseNode<ContainerT, PointT>::subdividePoint (const PointT& point, std::vector< AlignedPointTVector >& c) 693 { 694 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 695 std::size_t octant = 0; 696 octant = ((point.z >= mid_xyz[2]) << 2) | ((point.y >= mid_xyz[1]) << 1) | ((point.x >= mid_xyz[0]) << 0); 697 c[octant].push_back (point); 698 } 699 700 //////////////////////////////////////////////////////////////////////////////// 701 template<typename ContainerT, typename PointT> std::uint64_t addPointCloud_and_genLOD(const pcl::PCLPointCloud2::Ptr input_cloud)702 OutofcoreOctreeBaseNode<ContainerT, PointT>::addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud) //, const bool skip_bb_check = false ) 703 { 704 std::uint64_t points_added = 0; 705 706 if ( input_cloud->width * input_cloud->height == 0 ) 707 { 708 return (0); 709 } 710 711 if ( this->depth_ == this->root_node_->m_tree_->getDepth () || input_cloud->width*input_cloud->height < 8 ) 712 { 713 std::uint64_t points_added = addDataAtMaxDepth (input_cloud, true); 714 assert (points_added > 0); 715 return (points_added); 716 } 717 718 if (num_children_ < 8 ) 719 { 720 if ( hasUnloadedChildren () ) 721 { 722 loadChildren (false); 723 } 724 } 725 726 //------------------------------------------------------------ 727 //subsample data: 728 // 1. Get indices from a random sample 729 // 2. Extract those indices with the extract indices class (in order to also get the complement) 730 //------------------------------------------------------------ 731 pcl::RandomSample<pcl::PCLPointCloud2> random_sampler; 732 random_sampler.setInputCloud (input_cloud); 733 734 //set sample size to 1/8 of total points (12.5%) 735 std::uint64_t sample_size = input_cloud->width*input_cloud->height / 8; 736 random_sampler.setSample (static_cast<unsigned int> (sample_size)); 737 738 //create our destination 739 pcl::PCLPointCloud2::Ptr downsampled_cloud ( new pcl::PCLPointCloud2 () ); 740 741 //create destination for indices 742 pcl::IndicesPtr downsampled_cloud_indices ( new pcl::Indices () ); 743 random_sampler.filter (*downsampled_cloud_indices); 744 745 //extract the "random subset", size by setSampleSize 746 pcl::ExtractIndices<pcl::PCLPointCloud2> extractor; 747 extractor.setInputCloud (input_cloud); 748 extractor.setIndices (downsampled_cloud_indices); 749 extractor.filter (*downsampled_cloud); 750 751 //extract the complement of those points (i.e. everything remaining) 752 pcl::PCLPointCloud2::Ptr remaining_points ( new pcl::PCLPointCloud2 () ); 753 extractor.setNegative (true); 754 extractor.filter (*remaining_points); 755 756 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 ); 757 758 //insert subsampled data to the node's disk container payload 759 if ( downsampled_cloud->width * downsampled_cloud->height != 0 ) 760 { 761 root_node_->m_tree_->incrementPointsInLOD ( this->depth_, downsampled_cloud->width * downsampled_cloud->height ); 762 payload_->insertRange (downsampled_cloud); 763 points_added += downsampled_cloud->width*downsampled_cloud->height ; 764 } 765 766 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Remaining points are %u\n",__FUNCTION__, remaining_points->width*remaining_points->height); 767 768 //subdivide remaining data by destination octant 769 std::vector<pcl::Indices> indices; 770 indices.resize (8); 771 772 this->sortOctantIndices (remaining_points, indices, node_metadata_->getVoxelCenter ()); 773 774 //pass each set of points to the appropriate child octant 775 for(std::size_t i=0; i<8; i++) 776 { 777 778 if(indices[i].empty ()) 779 continue; 780 781 if (children_[i] == nullptr) 782 { 783 assert (i < 8); 784 createChild (i); 785 } 786 787 //copy correct indices into a temporary cloud 788 pcl::PCLPointCloud2::Ptr tmp_local_point_cloud (new pcl::PCLPointCloud2 ()); 789 pcl::copyPointCloud (*remaining_points, indices[i], *tmp_local_point_cloud); 790 791 //recursively add points and keep track of how many were successfully added to the tree 792 points_added += children_[i]->addPointCloud_and_genLOD (tmp_local_point_cloud); 793 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); 794 795 } 796 assert (points_added == input_cloud->width*input_cloud->height); 797 return (points_added); 798 } 799 //////////////////////////////////////////////////////////////////////////////// 800 801 template<typename ContainerT, typename PointT> std::uint64_t addDataToLeaf_and_genLOD(const AlignedPointTVector & p,const bool skip_bb_check)802 OutofcoreOctreeBaseNode<ContainerT, PointT>::addDataToLeaf_and_genLOD (const AlignedPointTVector& p, const bool skip_bb_check) 803 { 804 // If there are no points return 805 if (p.empty ()) 806 return (0); 807 808 // when adding data and generating sampled LOD 809 // If the max depth has been reached 810 assert (this->root_node_->m_tree_ != NULL ); 811 812 if (this->depth_ == this->root_node_->m_tree_->getDepth ()) 813 { 814 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::addDataToLeaf_and_genLOD] Adding data to the leaves\n"); 815 return (addDataAtMaxDepth(p, false)); 816 } 817 818 // Create child nodes of the current node but not grand children+ 819 if (this->hasUnloadedChildren ()) 820 loadChildren (false /*no recursive loading*/); 821 822 // Randomly sample data 823 AlignedPointTVector insertBuff; 824 randomSample(p, insertBuff, skip_bb_check); 825 826 if(!insertBuff.empty()) 827 { 828 // Increment point count for node 829 root_node_->m_tree_->incrementPointsInLOD (this->depth_, insertBuff.size()); 830 // Insert sampled point data 831 payload_->insertRange (insertBuff); 832 833 } 834 835 //subdivide vec to pass data down lower 836 std::vector<AlignedPointTVector> c; 837 subdividePoints(p, c, skip_bb_check); 838 839 std::uint64_t points_added = 0; 840 for(std::size_t i = 0; i < 8; i++) 841 { 842 // If child doesn't have points 843 if(c[i].empty()) 844 continue; 845 846 // If child doesn't exist 847 if(!children_[i]) 848 createChild(i); 849 850 // Recursively build children 851 points_added += children_[i]->addDataToLeaf_and_genLOD(c[i], true); 852 c[i].clear(); 853 } 854 855 return (points_added); 856 } 857 //////////////////////////////////////////////////////////////////////////////// 858 859 template<typename ContainerT, typename PointT> void createChild(const std::size_t idx)860 OutofcoreOctreeBaseNode<ContainerT, PointT>::createChild (const std::size_t idx) 861 { 862 assert (idx < 8); 863 864 //if already has 8 children, return 865 if (children_[idx] || (num_children_ == 8)) 866 { 867 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode::createChild] Not allowed to create a 9th child of %s\n",this->node_metadata_->getMetadataFilename ().c_str ()); 868 return; 869 } 870 871 Eigen::Vector3d start = node_metadata_->getBoundingBoxMin (); 872 Eigen::Vector3d step = (node_metadata_->getBoundingBoxMax () - start)/static_cast<double>(2.0); 873 874 Eigen::Vector3d childbb_min; 875 Eigen::Vector3d childbb_max; 876 877 int x, y, z; 878 if (idx > 3) 879 { 880 x = ((idx == 5) || (idx == 7)) ? 1 : 0; 881 y = ((idx == 6) || (idx == 7)) ? 1 : 0; 882 z = 1; 883 } 884 else 885 { 886 x = ((idx == 1) || (idx == 3)) ? 1 : 0; 887 y = ((idx == 2) || (idx == 3)) ? 1 : 0; 888 z = 0; 889 } 890 891 childbb_min[2] = start[2] + static_cast<double> (z) * step[2]; 892 childbb_max[2] = start[2] + static_cast<double> (z + 1) * step[2]; 893 894 childbb_min[1] = start[1] + static_cast<double> (y) * step[1]; 895 childbb_max[1] = start[1] + static_cast<double> (y + 1) * step[1]; 896 897 childbb_min[0] = start[0] + static_cast<double> (x) * step[0]; 898 childbb_max[0] = start[0] + static_cast<double> (x + 1) * step[0]; 899 900 boost::filesystem::path childdir = node_metadata_->getDirectoryPathname () / boost::filesystem::path (std::to_string(idx)); 901 children_[idx] = new OutofcoreOctreeBaseNode<ContainerT, PointT> (childbb_min, childbb_max, childdir.string ().c_str (), this); 902 903 num_children_++; 904 } 905 //////////////////////////////////////////////////////////////////////////////// 906 907 template<typename ContainerT, typename PointT> bool pointInBoundingBox(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,const Eigen::Vector3d & point)908 pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const Eigen::Vector3d& point) 909 { 910 if (((min_bb[0] <= point[0]) && (point[0] < max_bb[0])) && 911 ((min_bb[1] <= point[1]) && (point[1] < max_bb[1])) && 912 ((min_bb[2] <= point[2]) && (point[2] < max_bb[2]))) 913 { 914 return (true); 915 916 } 917 return (false); 918 } 919 920 921 //////////////////////////////////////////////////////////////////////////////// 922 template<typename ContainerT, typename PointT> bool pointInBoundingBox(const PointT & p) const923 OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const PointT& p) const 924 { 925 const Eigen::Vector3d& min = node_metadata_->getBoundingBoxMin (); 926 const Eigen::Vector3d& max = node_metadata_->getBoundingBoxMax (); 927 928 if (((min[0] <= p.x) && (p.x < max[0])) && 929 ((min[1] <= p.y) && (p.y < max[1])) && 930 ((min[2] <= p.z) && (p.z < max[2]))) 931 { 932 return (true); 933 934 } 935 return (false); 936 } 937 938 //////////////////////////////////////////////////////////////////////////////// 939 template<typename ContainerT, typename PointT> void printBoundingBox(const std::size_t query_depth) const940 OutofcoreOctreeBaseNode<ContainerT, PointT>::printBoundingBox (const std::size_t query_depth) const 941 { 942 Eigen::Vector3d min; 943 Eigen::Vector3d max; 944 node_metadata_->getBoundingBox (min, max); 945 946 if (this->depth_ < query_depth){ 947 for (std::size_t i = 0; i < this->depth_; i++) 948 std::cout << " "; 949 950 std::cout << "[" << min[0] << ", " << min[1] << ", " << min[2] << "] - "; 951 std::cout << "[" << max[0] << ", " << max[1] << ", " << max[2] << "] - "; 952 std::cout << "[" << max[0] - min[0] << ", " << max[1] - min[1]; 953 std::cout << ", " << max[2] - min[2] << "]" << std::endl; 954 955 if (num_children_ > 0) 956 { 957 for (std::size_t i = 0; i < 8; i++) 958 { 959 if (children_[i]) 960 children_[i]->printBoundingBox (query_depth); 961 } 962 } 963 } 964 } 965 966 //////////////////////////////////////////////////////////////////////////////// 967 template<typename ContainerT, typename PointT> void getOccupiedVoxelCentersRecursive(AlignedPointTVector & voxel_centers,const std::size_t query_depth)968 OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (AlignedPointTVector &voxel_centers, const std::size_t query_depth) 969 { 970 if (this->depth_ < query_depth){ 971 if (num_children_ > 0) 972 { 973 for (std::size_t i = 0; i < 8; i++) 974 { 975 if (children_[i]) 976 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth); 977 } 978 } 979 } 980 else 981 { 982 PointT voxel_center; 983 Eigen::Vector3d mid_xyz = node_metadata_->getVoxelCenter (); 984 voxel_center.x = static_cast<float>(mid_xyz[0]); 985 voxel_center.y = static_cast<float>(mid_xyz[1]); 986 voxel_center.z = static_cast<float>(mid_xyz[2]); 987 988 voxel_centers.push_back(voxel_center); 989 } 990 } 991 992 //////////////////////////////////////////////////////////////////////////////// 993 // Eigen::Vector3d cornerOffsets[] = 994 // { 995 // Eigen::Vector3d(-1.0, -1.0, -1.0), // - - - 996 // Eigen::Vector3d( 1.0, -1.0, -1.0), // - - + 997 // Eigen::Vector3d(-1.0, 1.0, -1.0), // - + - 998 // Eigen::Vector3d( 1.0, 1.0, -1.0), // - + + 999 // Eigen::Vector3d(-1.0, -1.0, 1.0), // + - - 1000 // Eigen::Vector3d( 1.0, -1.0, 1.0), // + - + 1001 // Eigen::Vector3d(-1.0, 1.0, 1.0), // + + - 1002 // Eigen::Vector3d( 1.0, 1.0, 1.0) // + + + 1003 // }; 1004 // 1005 // // Note that the input vector must already be negated when using this code for halfplane tests 1006 // int 1007 // vectorToIndex(Eigen::Vector3d normal) 1008 // { 1009 // int index = 0; 1010 // 1011 // if (normal.z () >= 0) index |= 1; 1012 // if (normal.y () >= 0) index |= 2; 1013 // if (normal.x () >= 0) index |= 4; 1014 // 1015 // return index; 1016 // } 1017 // 1018 // void 1019 // get_np_vertices(Eigen::Vector3d normal, Eigen::Vector3d &p_vertex, Eigen::Vector3d &n_vertex, Eigen::Vector3d min_bb, Eigen::Vector3d max_bb) 1020 // { 1021 // 1022 // p_vertex = min_bb; 1023 // n_vertex = max_bb; 1024 // 1025 // if (normal.x () >= 0) 1026 // { 1027 // n_vertex.x () = min_bb.x (); 1028 // p_vertex.x () = max_bb.x (); 1029 // } 1030 // 1031 // if (normal.y () >= 0) 1032 // { 1033 // n_vertex.y () = min_bb.y (); 1034 // p_vertex.y () = max_bb.y (); 1035 // } 1036 // 1037 // if (normal.z () >= 0) 1038 // { 1039 // p_vertex.z () = max_bb.z (); 1040 // n_vertex.z () = min_bb.z (); 1041 // } 1042 // } 1043 1044 template<typename Container, typename PointT> void queryFrustum(const double planes[24],std::list<std::string> & file_names)1045 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names) 1046 { 1047 queryFrustum(planes, file_names, this->m_tree_->getTreeDepth()); 1048 } 1049 1050 template<typename Container, typename PointT> void queryFrustum(const double planes[24],std::list<std::string> & file_names,const std::uint32_t query_depth,const bool skip_vfc_check)1051 OutofcoreOctreeBaseNode<Container, PointT>::queryFrustum (const double planes[24], std::list<std::string>& file_names, const std::uint32_t query_depth, const bool skip_vfc_check) 1052 { 1053 1054 enum {INSIDE, INTERSECT, OUTSIDE}; 1055 1056 int result = INSIDE; 1057 1058 if (this->depth_ > query_depth) 1059 { 1060 return; 1061 } 1062 1063 // if (this->depth_ > query_depth) 1064 // return; 1065 1066 if (!skip_vfc_check) 1067 { 1068 for(int i =0; i < 6; i++){ 1069 double a = planes[(i*4)]; 1070 double b = planes[(i*4)+1]; 1071 double c = planes[(i*4)+2]; 1072 double d = planes[(i*4)+3]; 1073 1074 //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl; 1075 1076 Eigen::Vector3d normal(a, b, c); 1077 1078 Eigen::Vector3d min_bb; 1079 Eigen::Vector3d max_bb; 1080 node_metadata_->getBoundingBox(min_bb, max_bb); 1081 1082 // Basic VFC algorithm 1083 Eigen::Vector3d center = node_metadata_->getVoxelCenter(); 1084 Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())), 1085 std::abs (static_cast<double> (max_bb.y () - center.y ())), 1086 std::abs (static_cast<double> (max_bb.z () - center.z ()))); 1087 1088 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d; 1089 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c)); 1090 1091 if (m + n < 0){ 1092 result = OUTSIDE; 1093 break; 1094 } 1095 1096 if (m - n < 0) result = INTERSECT; 1097 1098 // // n-p implementation 1099 // Eigen::Vector3d p_vertex; //pos vertex 1100 // Eigen::Vector3d n_vertex; //neg vertex 1101 // get_np_vertices(normal, p_vertex, n_vertex, min_bb, max_bb); 1102 // 1103 // std::cout << "n_vertex: " << n_vertex.x () << ", " << n_vertex.y () << ", " << n_vertex.z () << std::endl; 1104 // std::cout << "p_vertex: " << p_vertex.x () << ", " << p_vertex.y () << ", " << p_vertex.z () << std::endl; 1105 1106 // is the positive vertex outside? 1107 // if (pl[i].distance(b.getVertexP(pl[i].normal)) < 0) 1108 // { 1109 // result = OUTSIDE; 1110 // } 1111 // // is the negative vertex outside? 1112 // else if (pl[i].distance(b.getVertexN(pl[i].normal)) < 0) 1113 // { 1114 // result = INTERSECT; 1115 // } 1116 1117 // 1118 // 1119 // // This should be the same as below 1120 // if (normal.dot(n_vertex) + d > 0) 1121 // { 1122 // result = OUTSIDE; 1123 // } 1124 // 1125 // if (normal.dot(p_vertex) + d >= 0) 1126 // { 1127 // result = INTERSECT; 1128 // } 1129 1130 // This should be the same as above 1131 // double m = (a * n_vertex.x ()) + (b * n_vertex.y ()) + (c * n_vertex.z ()); 1132 // std::cout << "m = " << m << std::endl; 1133 // if (m > -d) 1134 // { 1135 // result = OUTSIDE; 1136 // } 1137 // 1138 // double n = (a * p_vertex.x ()) + (b * p_vertex.y ()) + (c * p_vertex.z ()); 1139 // std::cout << "n = " << n << std::endl; 1140 // if (n > -d) 1141 // { 1142 // result = INTERSECT; 1143 // } 1144 } 1145 } 1146 1147 if (result == OUTSIDE) 1148 { 1149 return; 1150 } 1151 1152 // switch(result){ 1153 // case OUTSIDE: 1154 // //std::cout << this->depth_ << " [OUTSIDE]: " << node_metadata_->getPCDFilename() << std::endl; 1155 // return; 1156 // case INTERSECT: 1157 // //std::cout << this->depth_ << " [INTERSECT]: " << node_metadata_->getPCDFilename() << std::endl; 1158 // break; 1159 // case INSIDE: 1160 // //std::cout << this->depth_ << " [INSIDE]: " << node_metadata_->getPCDFilename() << std::endl; 1161 // break; 1162 // } 1163 1164 // Add files breadth first 1165 if (this->depth_ == query_depth && payload_->getDataSize () > 0) 1166 //if (payload_->getDataSize () > 0) 1167 { 1168 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 1169 } 1170 1171 if (hasUnloadedChildren ()) 1172 { 1173 loadChildren (false); 1174 } 1175 1176 if (this->getNumChildren () > 0) 1177 { 1178 for (std::size_t i = 0; i < 8; i++) 1179 { 1180 if (children_[i]) 1181 children_[i]->queryFrustum (planes, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/); 1182 } 1183 } 1184 // else if (hasUnloadedChildren ()) 1185 // { 1186 // loadChildren (false); 1187 // 1188 // for (std::size_t i = 0; i < 8; i++) 1189 // { 1190 // if (children_[i]) 1191 // children_[i]->queryFrustum (planes, file_names, query_depth); 1192 // } 1193 // } 1194 //} 1195 } 1196 1197 //////////////////////////////////////////////////////////////////////////////// 1198 1199 template<typename Container, typename PointT> void queryFrustum(const double planes[24],const Eigen::Vector3d & eye,const Eigen::Matrix4d & view_projection_matrix,std::list<std::string> & file_names,const std::uint32_t query_depth,const bool skip_vfc_check)1200 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 std::uint32_t query_depth, const bool skip_vfc_check) 1201 { 1202 1203 // If we're above our query depth 1204 if (this->depth_ > query_depth) 1205 { 1206 return; 1207 } 1208 1209 // Bounding Box 1210 Eigen::Vector3d min_bb; 1211 Eigen::Vector3d max_bb; 1212 node_metadata_->getBoundingBox(min_bb, max_bb); 1213 1214 // Frustum Culling 1215 enum {INSIDE, INTERSECT, OUTSIDE}; 1216 1217 int result = INSIDE; 1218 1219 if (!skip_vfc_check) 1220 { 1221 for(int i =0; i < 6; i++){ 1222 double a = planes[(i*4)]; 1223 double b = planes[(i*4)+1]; 1224 double c = planes[(i*4)+2]; 1225 double d = planes[(i*4)+3]; 1226 1227 //std::cout << i << ": " << a << "x + " << b << "y + " << c << "z + " << d << std::endl; 1228 1229 Eigen::Vector3d normal(a, b, c); 1230 1231 // Basic VFC algorithm 1232 Eigen::Vector3d center = node_metadata_->getVoxelCenter(); 1233 Eigen::Vector3d radius (std::abs (static_cast<double> (max_bb.x () - center.x ())), 1234 std::abs (static_cast<double> (max_bb.y () - center.y ())), 1235 std::abs (static_cast<double> (max_bb.z () - center.z ()))); 1236 1237 double m = (center.x () * a) + (center.y () * b) + (center.z () * c) + d; 1238 double n = (radius.x () * std::abs(a)) + (radius.y () * std::abs(b)) + (radius.z () * std::abs(c)); 1239 1240 if (m + n < 0){ 1241 result = OUTSIDE; 1242 break; 1243 } 1244 1245 if (m - n < 0) result = INTERSECT; 1246 1247 } 1248 } 1249 1250 if (result == OUTSIDE) 1251 { 1252 return; 1253 } 1254 1255 // Bounding box projection 1256 // 3--------2 1257 // /| /| Y 0 = xmin, ymin, zmin 1258 // / | / | | 6 = xmax, ymax. zmax 1259 // 7--------6 | | 1260 // | | | | | 1261 // | 0-----|--1 +------X 1262 // | / | / / 1263 // |/ |/ / 1264 // 4--------5 Z 1265 1266 // bounding_box[0] = Eigen::Vector4d(min_bb.x (), min_bb.y (), min_bb.z (), 1.0); 1267 // bounding_box[1] = Eigen::Vector4d(max_bb.x (), min_bb.y (), min_bb.z (), 1.0); 1268 // bounding_box[2] = Eigen::Vector4d(max_bb.x (), max_bb.y (), min_bb.z (), 1.0); 1269 // bounding_box[3] = Eigen::Vector4d(min_bb.x (), max_bb.y (), min_bb.z (), 1.0); 1270 // bounding_box[4] = Eigen::Vector4d(min_bb.x (), min_bb.y (), max_bb.z (), 1.0); 1271 // bounding_box[5] = Eigen::Vector4d(max_bb.x (), min_bb.y (), max_bb.z (), 1.0); 1272 // bounding_box[6] = Eigen::Vector4d(max_bb.x (), max_bb.y (), max_bb.z (), 1.0); 1273 // bounding_box[7] = Eigen::Vector4d(min_bb.x (), max_bb.y (), max_bb.z (), 1.0); 1274 1275 int width = 500; 1276 int height = 500; 1277 1278 float coverage = pcl::visualization::viewScreenArea(eye, min_bb, max_bb, view_projection_matrix, width, height); 1279 //float coverage = pcl::visualization::viewScreenArea(eye, bounding_box, view_projection_matrix); 1280 1281 // for (int i=0; i < this->depth_; i++) std::cout << " "; 1282 // std::cout << this->depth_ << ": " << coverage << std::endl; 1283 1284 // Add files breadth first 1285 if (this->depth_ <= query_depth && payload_->getDataSize () > 0) 1286 //if (payload_->getDataSize () > 0) 1287 { 1288 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 1289 } 1290 1291 //if (coverage <= 0.075) 1292 if (coverage <= 10000) 1293 return; 1294 1295 if (hasUnloadedChildren ()) 1296 { 1297 loadChildren (false); 1298 } 1299 1300 if (this->getNumChildren () > 0) 1301 { 1302 for (std::size_t i = 0; i < 8; i++) 1303 { 1304 if (children_[i]) 1305 children_[i]->queryFrustum (planes, eye, view_projection_matrix, file_names, query_depth, (result == INSIDE) /*skip_vfc_check*/); 1306 } 1307 } 1308 } 1309 1310 //////////////////////////////////////////////////////////////////////////////// 1311 template<typename ContainerT, typename PointT> void getOccupiedVoxelCentersRecursive(std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d>> & voxel_centers,const std::size_t query_depth)1312 OutofcoreOctreeBaseNode<ContainerT, PointT>::getOccupiedVoxelCentersRecursive (std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, const std::size_t query_depth) 1313 { 1314 if (this->depth_ < query_depth){ 1315 if (num_children_ > 0) 1316 { 1317 for (std::size_t i = 0; i < 8; i++) 1318 { 1319 if (children_[i]) 1320 children_[i]->getOccupiedVoxelCentersRecursive (voxel_centers, query_depth); 1321 } 1322 } 1323 } 1324 else 1325 { 1326 Eigen::Vector3d voxel_center = node_metadata_->getVoxelCenter (); 1327 voxel_centers.push_back(voxel_center); 1328 } 1329 } 1330 1331 1332 //////////////////////////////////////////////////////////////////////////////// 1333 1334 template<typename ContainerT, typename PointT> void queryBBIntersects(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,const std::uint32_t query_depth,std::list<std::string> & file_names)1335 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIntersects (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const std::uint32_t query_depth, std::list<std::string>& file_names) 1336 { 1337 1338 Eigen::Vector3d my_min = min_bb; 1339 Eigen::Vector3d my_max = max_bb; 1340 1341 if (intersectsWithBoundingBox (my_min, my_max)) 1342 { 1343 if (this->depth_ < query_depth) 1344 { 1345 if (this->getNumChildren () > 0) 1346 { 1347 for (std::size_t i = 0; i < 8; i++) 1348 { 1349 if (children_[i]) 1350 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names); 1351 } 1352 } 1353 else if (hasUnloadedChildren ()) 1354 { 1355 loadChildren (false); 1356 1357 for (std::size_t i = 0; i < 8; i++) 1358 { 1359 if (children_[i]) 1360 children_[i]->queryBBIntersects (my_min, my_max, query_depth, file_names); 1361 } 1362 } 1363 return; 1364 } 1365 1366 if (payload_->getDataSize () > 0) 1367 { 1368 file_names.push_back (this->node_metadata_->getMetadataFilename ().string ()); 1369 } 1370 } 1371 } 1372 //////////////////////////////////////////////////////////////////////////////// 1373 1374 template<typename ContainerT, typename PointT> void queryBBIncludes(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,std::size_t query_depth,const pcl::PCLPointCloud2::Ptr & dst_blob)1375 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::size_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob) 1376 { 1377 std::uint64_t startingSize = dst_blob->width*dst_blob->height; 1378 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Starting points in destination blob: %ul\n", __FUNCTION__, startingSize ); 1379 1380 // If the queried bounding box has any intersection with this node's bounding box 1381 if (intersectsWithBoundingBox (min_bb, max_bb)) 1382 { 1383 // If we aren't at the max desired depth 1384 if (this->depth_ < query_depth) 1385 { 1386 //if this node doesn't have any children, we are at the max depth for this query 1387 if ((num_children_ == 0) && (hasUnloadedChildren ())) 1388 loadChildren (false); 1389 1390 //if this node has children 1391 if (num_children_ > 0) 1392 { 1393 //recursively store any points that fall into the queried bounding box into v and return 1394 for (std::size_t i = 0; i < 8; i++) 1395 { 1396 if (children_[i]) 1397 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, dst_blob); 1398 } 1399 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 1400 return; 1401 } 1402 } 1403 else //otherwise if we are at the max depth 1404 { 1405 //get all the points from the payload and return (easy with PCLPointCloud2) 1406 pcl::PCLPointCloud2::Ptr tmp_blob (new pcl::PCLPointCloud2 ()); 1407 pcl::PCLPointCloud2::Ptr tmp_dst_blob (new pcl::PCLPointCloud2 ()); 1408 //load all the data in this node from disk 1409 payload_->readRange (0, payload_->size (), tmp_blob); 1410 1411 if( tmp_blob->width*tmp_blob->height == 0 ) 1412 return; 1413 1414 //if this node's bounding box falls completely within the queried bounding box, keep all the points 1415 if (inBoundingBox (min_bb, max_bb)) 1416 { 1417 //concatenate all of what was just read into the main dst_blob 1418 //(is it safe to do in place?) 1419 1420 //if there is already something in the destination blob (remember this method is recursive) 1421 if( dst_blob->width*dst_blob->height != 0 ) 1422 { 1423 PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud before: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 1424 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud\n", __FUNCTION__); 1425 int res = pcl::concatenate (*dst_blob, *tmp_blob, *dst_blob); 1426 pcl::utils::ignore(res); 1427 assert (res == 1); 1428 1429 PCL_DEBUG ("[pcl::outofocre::OutofcoreOctreeBaseNode::%s] Size of cloud after: %lu\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 1430 } 1431 //otherwise, just copy the tmp_blob into the dst_blob 1432 else 1433 { 1434 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode] Copying point cloud into the destination blob\n"); 1435 pcl::copyPointCloud (*tmp_blob, *dst_blob); 1436 assert (tmp_blob->width*tmp_blob->height == dst_blob->width*dst_blob->height); 1437 } 1438 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in dst_blob: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height ); 1439 return; 1440 } 1441 //otherwise queried bounding box only partially intersects this 1442 //node's bounding box, so we have to check all the points in 1443 //this box for intersection with queried bounding box 1444 1445 // 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] ); 1446 //put the ros message into a pointxyz point cloud (just to get the indices by using getPointsInBox) 1447 typename pcl::PointCloud<PointT>::Ptr tmp_cloud ( new pcl::PointCloud<PointT> () ); 1448 pcl::fromPCLPointCloud2 ( *tmp_blob, *tmp_cloud ); 1449 assert (tmp_blob->width*tmp_blob->height == tmp_cloud->width*tmp_cloud->height ); 1450 1451 Eigen::Vector4f min_pt ( static_cast<float> ( min_bb[0] ), static_cast<float> ( min_bb[1] ), static_cast<float> ( min_bb[2] ), 1.0f); 1452 Eigen::Vector4f max_pt ( static_cast<float> ( max_bb[0] ), static_cast<float> ( max_bb[1] ) , static_cast<float>( max_bb[2] ), 1.0f ); 1453 1454 pcl::Indices indices; 1455 1456 pcl::getPointsInBox ( *tmp_cloud, min_pt, max_pt, indices ); 1457 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points in box: %d\n", __FUNCTION__, indices.size () ); 1458 PCL_DEBUG ( "[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points remaining: %d\n", __FUNCTION__, tmp_cloud->width*tmp_cloud->height - indices.size () ); 1459 1460 if ( !indices.empty () ) 1461 { 1462 if( dst_blob->width*dst_blob->height > 0 ) 1463 { 1464 //need a new tmp destination with extracted points within BB 1465 pcl::PCLPointCloud2::Ptr tmp_blob_within_bb (new pcl::PCLPointCloud2 ()); 1466 1467 //copy just the points marked in indices 1468 pcl::copyPointCloud ( *tmp_blob, indices, *tmp_blob_within_bb ); 1469 assert ( tmp_blob_within_bb->width*tmp_blob_within_bb->height == indices.size () ); 1470 assert ( tmp_blob->fields.size () == tmp_blob_within_bb->fields.size () ); 1471 //concatenate those points into the returned dst_blob 1472 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Concatenating point cloud in place\n", __FUNCTION__); 1473 std::uint64_t orig_points_in_destination = dst_blob->width*dst_blob->height; 1474 int res = pcl::concatenate (*dst_blob, *tmp_blob_within_bb, *dst_blob); 1475 pcl::utils::ignore(orig_points_in_destination, res); 1476 assert (res == 1); 1477 assert (dst_blob->width*dst_blob->height == indices.size () + orig_points_in_destination); 1478 1479 } 1480 else 1481 { 1482 pcl::copyPointCloud ( *tmp_blob, indices, *dst_blob ); 1483 assert ( dst_blob->width*dst_blob->height == indices.size () ); 1484 } 1485 } 1486 } 1487 } 1488 1489 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] Points added by function call: %ul\n", __FUNCTION__, dst_blob->width*dst_blob->height - startingSize ); 1490 } 1491 1492 template<typename ContainerT, typename PointT> void queryBBIncludes(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,std::size_t query_depth,AlignedPointTVector & v)1493 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::size_t query_depth, AlignedPointTVector& v) 1494 { 1495 1496 //if the queried bounding box has any intersection with this node's bounding box 1497 if (intersectsWithBoundingBox (min_bb, max_bb)) 1498 { 1499 //if we aren't at the max desired depth 1500 if (this->depth_ < query_depth) 1501 { 1502 //if this node doesn't have any children, we are at the max depth for this query 1503 if (this->hasUnloadedChildren ()) 1504 { 1505 this->loadChildren (false); 1506 } 1507 1508 //if this node has children 1509 if (getNumChildren () > 0) 1510 { 1511 if(hasUnloadedChildren ()) 1512 loadChildren (false); 1513 1514 //recursively store any points that fall into the queried bounding box into v and return 1515 for (std::size_t i = 0; i < 8; i++) 1516 { 1517 if (children_[i]) 1518 children_[i]->queryBBIncludes (min_bb, max_bb, query_depth, v); 1519 } 1520 return; 1521 } 1522 } 1523 //otherwise if we are at the max depth 1524 else 1525 { 1526 //if this node's bounding box falls completely within the queried bounding box 1527 if (inBoundingBox (min_bb, max_bb)) 1528 { 1529 //get all the points from the payload and return 1530 AlignedPointTVector payload_cache; 1531 payload_->readRange (0, payload_->size (), payload_cache); 1532 v.insert (v.end (), payload_cache.begin (), payload_cache.end ()); 1533 return; 1534 } 1535 //otherwise queried bounding box only partially intersects this 1536 //node's bounding box, so we have to check all the points in 1537 //this box for intersection with queried bounding box 1538 //read _all_ the points in from the disk container 1539 AlignedPointTVector payload_cache; 1540 payload_->readRange (0, payload_->size (), payload_cache); 1541 1542 std::uint64_t len = payload_->size (); 1543 //iterate through each of them 1544 for (std::uint64_t i = 0; i < len; i++) 1545 { 1546 const PointT& p = payload_cache[i]; 1547 //if it falls within this bounding box 1548 if (pointInBoundingBox (min_bb, max_bb, p)) 1549 { 1550 //store it in the list 1551 v.push_back (p); 1552 } 1553 else 1554 { 1555 PCL_DEBUG ("[pcl::outofcore::queryBBIncludes] Point %.2lf %.2lf %.2lf not in bounding box %.2lf %.2lf %.2lf", p.x, p.y, p.z, min_bb[0], min_bb[1], min_bb[2], max_bb[0], max_bb[1], max_bb[2]); 1556 } 1557 } 1558 } 1559 } 1560 } 1561 1562 //////////////////////////////////////////////////////////////////////////////// 1563 template<typename ContainerT, typename PointT> void queryBBIncludes_subsample(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,std::uint64_t query_depth,const pcl::PCLPointCloud2::Ptr & dst_blob,double percent)1564 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent) 1565 { 1566 if (intersectsWithBoundingBox (min_bb, max_bb)) 1567 { 1568 if (this->depth_ < query_depth) 1569 { 1570 if (this->hasUnloadedChildren ()) 1571 this->loadChildren (false); 1572 1573 if (this->getNumChildren () > 0) 1574 { 1575 for (std::size_t i=0; i<8; i++) 1576 { 1577 //recursively traverse (depth first) 1578 if (children_[i]!=nullptr) 1579 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, dst_blob, percent); 1580 } 1581 return; 1582 } 1583 } 1584 //otherwise, at max depth --> read from disk, subsample, concatenate 1585 else 1586 { 1587 1588 if (inBoundingBox (min_bb, max_bb)) 1589 { 1590 pcl::PCLPointCloud2::Ptr tmp_blob; 1591 this->payload_->read (tmp_blob); 1592 std::uint64_t num_pts = tmp_blob->width*tmp_blob->height; 1593 1594 double sample_points = static_cast<double>(num_pts) * percent; 1595 if (num_pts > 0) 1596 { 1597 //always sample at least one point 1598 sample_points = sample_points > 1 ? sample_points : 1; 1599 } 1600 else 1601 { 1602 return; 1603 } 1604 1605 1606 pcl::RandomSample<pcl::PCLPointCloud2> random_sampler; 1607 random_sampler.setInputCloud (tmp_blob); 1608 1609 pcl::PCLPointCloud2::Ptr downsampled_points (new pcl::PCLPointCloud2 ()); 1610 1611 //set sample size as percent * number of points read 1612 random_sampler.setSample (static_cast<unsigned int> (sample_points)); 1613 1614 pcl::ExtractIndices<pcl::PCLPointCloud2> extractor; 1615 extractor.setInputCloud (tmp_blob); 1616 1617 pcl::IndicesPtr downsampled_cloud_indices (new pcl::Indices ()); 1618 random_sampler.filter (*downsampled_cloud_indices); 1619 extractor.setIndices (downsampled_cloud_indices); 1620 extractor.filter (*downsampled_points); 1621 1622 //concatenate the result into the destination cloud 1623 pcl::concatenate (*dst_blob, *downsampled_points, *dst_blob); 1624 } 1625 } 1626 } 1627 } 1628 1629 1630 //////////////////////////////////////////////////////////////////////////////// 1631 template<typename ContainerT, typename PointT> void queryBBIncludes_subsample(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,std::uint64_t query_depth,const double percent,AlignedPointTVector & dst)1632 OutofcoreOctreeBaseNode<ContainerT, PointT>::queryBBIncludes_subsample (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, std::uint64_t query_depth, const double percent, AlignedPointTVector& dst) 1633 { 1634 //check if the queried bounding box has any intersection with this node's bounding box 1635 if (intersectsWithBoundingBox (min_bb, max_bb)) 1636 { 1637 //if we are not at the max depth for queried nodes 1638 if (this->depth_ < query_depth) 1639 { 1640 //check if we don't have children 1641 if ((num_children_ == 0) && (hasUnloadedChildren ())) 1642 { 1643 loadChildren (false); 1644 } 1645 //if we do have children 1646 if (num_children_ > 0) 1647 { 1648 //recursively add their valid points within the queried bounding box to the list v 1649 for (std::size_t i = 0; i < 8; i++) 1650 { 1651 if (children_[i]) 1652 children_[i]->queryBBIncludes_subsample (min_bb, max_bb, query_depth, percent, dst); 1653 } 1654 return; 1655 } 1656 } 1657 //otherwise we are at the max depth, so we add all our points or some of our points 1658 else 1659 { 1660 //if this node's bounding box falls completely within the queried bounding box 1661 if (inBoundingBox (min_bb, max_bb)) 1662 { 1663 //add a random sample of all the points 1664 AlignedPointTVector payload_cache; 1665 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache); 1666 dst.insert (dst.end (), payload_cache.begin (), payload_cache.end ()); 1667 return; 1668 } 1669 //otherwise the queried bounding box only partially intersects with this node's bounding box 1670 //brute force selection of all valid points 1671 AlignedPointTVector payload_cache_within_region; 1672 { 1673 AlignedPointTVector payload_cache; 1674 payload_->readRange (0, payload_->size (), payload_cache); 1675 for (std::size_t i = 0; i < payload_->size (); i++) 1676 { 1677 const PointT& p = payload_cache[i]; 1678 if (pointInBoundingBox (min_bb, max_bb, p)) 1679 { 1680 payload_cache_within_region.push_back (p); 1681 } 1682 } 1683 }//force the payload cache to deconstruct here 1684 1685 //use STL random_shuffle and push back a random selection of the points onto our list 1686 std::shuffle (payload_cache_within_region.begin (), payload_cache_within_region.end (), std::mt19937(std::random_device()())); 1687 std::size_t numpick = static_cast<std::size_t> (percent * static_cast<double> (payload_cache_within_region.size ()));; 1688 1689 for (std::size_t i = 0; i < numpick; i++) 1690 { 1691 dst.push_back (payload_cache_within_region[i]); 1692 } 1693 } 1694 } 1695 } 1696 //////////////////////////////////////////////////////////////////////////////// 1697 1698 //dir is current level. we put this nodes files into it 1699 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode(const Eigen::Vector3d & bb_min,const Eigen::Vector3d & bb_max,const char * dir,OutofcoreOctreeBaseNode<ContainerT,PointT> * super)1700 OutofcoreOctreeBaseNode<ContainerT, PointT>::OutofcoreOctreeBaseNode (const Eigen::Vector3d& bb_min, const Eigen::Vector3d& bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT,PointT>* super) 1701 : m_tree_ () 1702 , root_node_ () 1703 , parent_ () 1704 , depth_ () 1705 , children_ (8, nullptr) 1706 , num_children_ () 1707 , num_loaded_children_ (0) 1708 , payload_ () 1709 , node_metadata_ (new OutofcoreOctreeNodeMetadata) 1710 { 1711 node_metadata_->setOutofcoreVersion (3); 1712 1713 if (super == nullptr) 1714 { 1715 PCL_ERROR ( "[pc::outofcore::OutofcoreOctreeBaseNode] Super is null - don't make a root node this way!\n" ); 1716 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Exception: Bad parent"); 1717 } 1718 1719 this->parent_ = super; 1720 root_node_ = super->root_node_; 1721 m_tree_ = super->root_node_->m_tree_; 1722 assert (m_tree_ != NULL); 1723 1724 depth_ = super->depth_ + 1; 1725 num_children_ = 0; 1726 1727 node_metadata_->setBoundingBox (bb_min, bb_max); 1728 1729 std::string uuid_idx; 1730 std::string uuid_cont; 1731 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_idx); 1732 OutofcoreOctreeDiskContainer<PointT>::getRandomUUIDString (uuid_cont); 1733 1734 std::string node_index_name = uuid_idx + std::string ("_") + node_index_basename + node_index_extension; 1735 1736 std::string node_container_name; 1737 node_container_name = uuid_cont + std::string ("_") + node_container_basename + pcd_extension; 1738 1739 node_metadata_->setDirectoryPathname (boost::filesystem::path (dir)); 1740 node_metadata_->setPCDFilename (node_metadata_->getDirectoryPathname () / boost::filesystem::path (node_container_name)); 1741 node_metadata_->setMetadataFilename ( node_metadata_->getDirectoryPathname ()/boost::filesystem::path (node_index_name)); 1742 1743 boost::filesystem::create_directory (node_metadata_->getDirectoryPathname ()); 1744 1745 payload_.reset (new ContainerT (node_metadata_->getPCDFilename ())); 1746 this->saveIdx (false); 1747 } 1748 1749 //////////////////////////////////////////////////////////////////////////////// 1750 1751 template<typename ContainerT, typename PointT> void copyAllCurrentAndChildPointsRec(std::list<PointT> & v)1752 OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec (std::list<PointT>& v) 1753 { 1754 if ((num_children_ == 0) && (hasUnloadedChildren ())) 1755 { 1756 loadChildren (false); 1757 } 1758 1759 for (std::size_t i = 0; i < num_children_; i++) 1760 { 1761 children_[i]->copyAllCurrentAndChildPointsRec (v); 1762 } 1763 1764 AlignedPointTVector payload_cache; 1765 payload_->readRange (0, payload_->size (), payload_cache); 1766 1767 { 1768 v.insert (v.end (), payload_cache.begin (), payload_cache.end ()); 1769 } 1770 } 1771 1772 //////////////////////////////////////////////////////////////////////////////// 1773 1774 template<typename ContainerT, typename PointT> void copyAllCurrentAndChildPointsRec_sub(std::list<PointT> & v,const double percent)1775 OutofcoreOctreeBaseNode<ContainerT, PointT>::copyAllCurrentAndChildPointsRec_sub (std::list<PointT>& v, const double percent) 1776 { 1777 if ((num_children_ == 0) && (hasUnloadedChildren ())) 1778 { 1779 loadChildren (false); 1780 } 1781 1782 for (std::size_t i = 0; i < 8; i++) 1783 { 1784 if (children_[i]) 1785 children_[i]->copyAllCurrentAndChildPointsRec_sub (v, percent); 1786 } 1787 1788 std::vector<PointT> payload_cache; 1789 payload_->readRangeSubSample (0, payload_->size (), percent, payload_cache); 1790 1791 for (std::size_t i = 0; i < payload_cache.size (); i++) 1792 { 1793 v.push_back (payload_cache[i]); 1794 } 1795 } 1796 1797 //////////////////////////////////////////////////////////////////////////////// 1798 1799 template<typename ContainerT, typename PointT> inline bool intersectsWithBoundingBox(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb) const1800 OutofcoreOctreeBaseNode<ContainerT, PointT>::intersectsWithBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const 1801 { 1802 Eigen::Vector3d min, max; 1803 node_metadata_->getBoundingBox (min, max); 1804 1805 //Check whether any portion of min_bb, max_bb falls within min,max 1806 if (((min[0] <= min_bb[0]) && (min_bb[0] <= max[0])) || ((min_bb[0] <= min[0]) && (min[0] <= max_bb[0]))) 1807 { 1808 if (((min[1] <= min_bb[1]) && (min_bb[1] <= max[1])) || ((min_bb[1] <= min[1]) && (min[1] <= max_bb[1]))) 1809 { 1810 if (((min[2] <= min_bb[2]) && (min_bb[2] <= max[2])) || ((min_bb[2] <= min[2]) && (min[2] <= max_bb[2]))) 1811 { 1812 return (true); 1813 } 1814 } 1815 } 1816 1817 return (false); 1818 } 1819 //////////////////////////////////////////////////////////////////////////////// 1820 1821 template<typename ContainerT, typename PointT> inline bool inBoundingBox(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb) const1822 OutofcoreOctreeBaseNode<ContainerT, PointT>::inBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb) const 1823 { 1824 Eigen::Vector3d min, max; 1825 1826 node_metadata_->getBoundingBox (min, max); 1827 1828 if ((min_bb[0] <= min[0]) && (max[0] <= max_bb[0])) 1829 { 1830 if ((min_bb[1] <= min[1]) && (max[1] <= max_bb[1])) 1831 { 1832 if ((min_bb[2] <= min[2]) && (max[2] <= max_bb[2])) 1833 { 1834 return (true); 1835 } 1836 } 1837 } 1838 1839 return (false); 1840 } 1841 //////////////////////////////////////////////////////////////////////////////// 1842 1843 template<typename ContainerT, typename PointT> inline bool pointInBoundingBox(const Eigen::Vector3d & min_bb,const Eigen::Vector3d & max_bb,const PointT & p)1844 OutofcoreOctreeBaseNode<ContainerT, PointT>::pointInBoundingBox (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, 1845 const PointT& p) 1846 { 1847 //by convention, minimum boundary is included; maximum boundary is not 1848 if ((min_bb[0] <= p.x) && (p.x < max_bb[0])) 1849 { 1850 if ((min_bb[1] <= p.y) && (p.y < max_bb[1])) 1851 { 1852 if ((min_bb[2] <= p.z) && (p.z < max_bb[2])) 1853 { 1854 return (true); 1855 } 1856 } 1857 } 1858 return (false); 1859 } 1860 1861 //////////////////////////////////////////////////////////////////////////////// 1862 1863 template<typename ContainerT, typename PointT> void writeVPythonVisual(std::ofstream & file)1864 OutofcoreOctreeBaseNode<ContainerT, PointT>::writeVPythonVisual (std::ofstream& file) 1865 { 1866 Eigen::Vector3d min; 1867 Eigen::Vector3d max; 1868 node_metadata_->getBoundingBox (min, max); 1869 1870 double l = max[0] - min[0]; 1871 double h = max[1] - min[1]; 1872 double w = max[2] - min[2]; 1873 file << "box( pos=(" << min[0] << ", " << min[1] << ", " << min[2] << "), length=" << l << ", height=" << h 1874 << ", width=" << w << " )\n"; 1875 1876 for (std::size_t i = 0; i < num_children_; i++) 1877 { 1878 children_[i]->writeVPythonVisual (file); 1879 } 1880 } 1881 1882 //////////////////////////////////////////////////////////////////////////////// 1883 1884 template<typename ContainerT, typename PointT> int read(pcl::PCLPointCloud2::Ptr & output_cloud)1885 OutofcoreOctreeBaseNode<ContainerT, PointT>::read (pcl::PCLPointCloud2::Ptr &output_cloud) 1886 { 1887 return (this->payload_->read (output_cloud)); 1888 } 1889 1890 //////////////////////////////////////////////////////////////////////////////// 1891 1892 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>* getChildPtr(std::size_t index_arg) const1893 OutofcoreOctreeBaseNode<ContainerT, PointT>::getChildPtr (std::size_t index_arg) const 1894 { 1895 PCL_DEBUG ("[pcl::outofcore::OutofcoreOctreeBaseNode::%s] %d", __FUNCTION__, index_arg); 1896 return (children_[index_arg]); 1897 } 1898 1899 //////////////////////////////////////////////////////////////////////////////// 1900 template<typename ContainerT, typename PointT> std::uint64_t getDataSize() const1901 OutofcoreOctreeBaseNode<ContainerT, PointT>::getDataSize () const 1902 { 1903 return (this->payload_->getDataSize ()); 1904 } 1905 1906 //////////////////////////////////////////////////////////////////////////////// 1907 1908 template<typename ContainerT, typename PointT> std::size_t countNumLoadedChildren() const1909 OutofcoreOctreeBaseNode<ContainerT, PointT>::countNumLoadedChildren () const 1910 { 1911 std::size_t loaded_children_count = 0; 1912 1913 for (std::size_t i=0; i<8; i++) 1914 { 1915 if (children_[i] != nullptr) 1916 loaded_children_count++; 1917 } 1918 1919 return (loaded_children_count); 1920 } 1921 1922 //////////////////////////////////////////////////////////////////////////////// 1923 1924 template<typename ContainerT, typename PointT> void loadFromFile(const boost::filesystem::path & path,OutofcoreOctreeBaseNode<ContainerT,PointT> * super)1925 OutofcoreOctreeBaseNode<ContainerT, PointT>::loadFromFile (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super) 1926 { 1927 PCL_DEBUG ("[pcl:outofcore::OutofcoreOctreeBaseNode] Loading metadata from %s\n", path.filename ().c_str ()); 1928 node_metadata_->loadMetadataFromDisk (path); 1929 1930 //this shouldn't be part of 'loadFromFile' 1931 this->parent_ = super; 1932 1933 if (num_children_ > 0) 1934 recFreeChildren (); 1935 1936 this->num_children_ = 0; 1937 this->payload_.reset (new ContainerT (node_metadata_->getPCDFilename ())); 1938 } 1939 1940 //////////////////////////////////////////////////////////////////////////////// 1941 1942 template<typename ContainerT, typename PointT> void convertToXYZRecursive()1943 OutofcoreOctreeBaseNode<ContainerT, PointT>::convertToXYZRecursive () 1944 { 1945 std::string fname = boost::filesystem::basename (node_metadata_->getPCDFilename ()) + std::string (".dat.xyz"); 1946 boost::filesystem::path xyzfile = node_metadata_->getDirectoryPathname () / fname; 1947 payload_->convertToXYZ (xyzfile); 1948 1949 if (hasUnloadedChildren ()) 1950 { 1951 loadChildren (false); 1952 } 1953 1954 for (std::size_t i = 0; i < 8; i++) 1955 { 1956 if (children_[i]) 1957 children_[i]->convertToXYZ (); 1958 } 1959 } 1960 1961 //////////////////////////////////////////////////////////////////////////////// 1962 1963 template<typename ContainerT, typename PointT> void flushToDiskRecursive()1964 OutofcoreOctreeBaseNode<ContainerT, PointT>::flushToDiskRecursive () 1965 { 1966 for (std::size_t i = 0; i < 8; i++) 1967 { 1968 if (children_[i]) 1969 children_[i]->flushToDiskRecursive (); 1970 } 1971 } 1972 1973 //////////////////////////////////////////////////////////////////////////////// 1974 1975 template<typename ContainerT, typename PointT> void sortOctantIndices(const pcl::PCLPointCloud2::Ptr & input_cloud,std::vector<pcl::Indices> & indices,const Eigen::Vector3d & mid_xyz)1976 OutofcoreOctreeBaseNode<ContainerT, PointT>::sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< pcl::Indices > &indices, const Eigen::Vector3d &mid_xyz) 1977 { 1978 if (indices.size () < 8) 1979 indices.resize (8); 1980 1981 int x_idx = pcl::getFieldIndex (*input_cloud , std::string ("x") ); 1982 int y_idx = pcl::getFieldIndex (*input_cloud, std::string ("y") ); 1983 int z_idx = pcl::getFieldIndex (*input_cloud, std::string ("z") ); 1984 1985 int x_offset = input_cloud->fields[x_idx].offset; 1986 int y_offset = input_cloud->fields[y_idx].offset; 1987 int z_offset = input_cloud->fields[z_idx].offset; 1988 1989 for ( std::size_t point_idx =0; point_idx < input_cloud->data.size (); point_idx +=input_cloud->point_step ) 1990 { 1991 PointT local_pt; 1992 1993 local_pt.x = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + x_offset])); 1994 local_pt.y = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + y_offset])); 1995 local_pt.z = * (reinterpret_cast<float*>(&input_cloud->data[point_idx + z_offset])); 1996 1997 if (!std::isfinite (local_pt.x) || !std::isfinite (local_pt.y) || !std::isfinite (local_pt.z)) 1998 continue; 1999 2000 if(!this->pointInBoundingBox (local_pt)) 2001 { 2002 PCL_ERROR ("pcl::outofcore::OutofcoreOctreeBaseNode::%s] Point %2.lf %.2lf %.2lf not in bounding box\n", __FUNCTION__, local_pt.x, local_pt.y, local_pt.z); 2003 } 2004 2005 assert (this->pointInBoundingBox (local_pt) == true); 2006 2007 //compute the box we are in 2008 std::size_t box = 0; 2009 box = ((local_pt.z >= mid_xyz[2]) << 2) | ((local_pt.y >= mid_xyz[1]) << 1) | ((local_pt.x >= mid_xyz[0]) << 0); 2010 assert (box < 8); 2011 2012 //insert to the vector of indices 2013 indices[box].push_back (static_cast<int> (point_idx/input_cloud->point_step)); 2014 } 2015 } 2016 //////////////////////////////////////////////////////////////////////////////// 2017 2018 #if 0 //A bunch of non-class methods left from the Urban Robotics code that has been deactivated 2019 template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>* 2020 makenode_norec (const boost::filesystem::path& path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super) 2021 { 2022 OutofcoreOctreeBaseNode<ContainerT, PointT>* thisnode = new OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer < PointT > , PointT > (); 2023 //octree_disk_node (); 2024 2025 if (super == NULL) 2026 { 2027 thisnode->thisdir_ = path.parent_path (); 2028 2029 if (!boost::filesystem::exists (thisnode->thisdir_)) 2030 { 2031 PCL_ERROR ( "[pcl::outofcore::OutofcoreOctreeBaseNode] could not find dir %s\n",thisnode->thisdir_.c_str () ); 2032 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Outofcore Octree Exception: Could not find directory"); 2033 } 2034 2035 thisnode->thisnodeindex_ = path; 2036 2037 thisnode->depth_ = 0; 2038 thisnode->root_node_ = thisnode; 2039 } 2040 else 2041 { 2042 thisnode->thisdir_ = path; 2043 thisnode->depth_ = super->depth_ + 1; 2044 thisnode->root_node_ = super->root_node_; 2045 2046 if (thisnode->depth_ > thisnode->root->max_depth_) 2047 { 2048 thisnode->root->max_depth_ = thisnode->depth_; 2049 } 2050 2051 boost::filesystem::directory_iterator diterend; 2052 bool loaded = false; 2053 for (boost::filesystem::directory_iterator diter (thisnode->thisdir_); diter != diterend; ++diter) 2054 { 2055 const boost::filesystem::path& file = *diter; 2056 if (!boost::filesystem::is_directory (file)) 2057 { 2058 if (boost::filesystem::extension (file) == OutofcoreOctreeBaseNode<ContainerT, PointT>::node_index_extension) 2059 { 2060 thisnode->thisnodeindex_ = file; 2061 loaded = true; 2062 break; 2063 } 2064 } 2065 } 2066 2067 if (!loaded) 2068 { 2069 PCL_ERROR ("[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find index!\n"); 2070 PCL_THROW_EXCEPTION (PCLException, "[pcl::outofcore::OutofcoreOctreeBaseNode] Could not find node metadata index file"); 2071 } 2072 2073 } 2074 thisnode->max_depth_ = 0; 2075 2076 { 2077 std::ifstream f (thisnode->thisnodeindex_.string ().c_str (), std::ios::in); 2078 2079 f >> thisnode->min_[0]; 2080 f >> thisnode->min_[1]; 2081 f >> thisnode->min_[2]; 2082 f >> thisnode->max_[0]; 2083 f >> thisnode->max_[1]; 2084 f >> thisnode->max_[2]; 2085 2086 std::string filename; 2087 f >> filename; 2088 thisnode->thisnodestorage_ = thisnode->thisdir_ / filename; 2089 2090 f.close (); 2091 2092 thisnode->payload_.reset (new ContainerT (thisnode->thisnodestorage_)); 2093 } 2094 2095 thisnode->parent_ = super; 2096 children_.clear (); 2097 children_.resize (8, static_cast<OutofcoreOctreeBaseNode<ContainerT, PointT>* > (0)); 2098 thisnode->num_children_ = 0; 2099 2100 return (thisnode); 2101 } 2102 2103 //////////////////////////////////////////////////////////////////////////////// 2104 2105 //accelerate search 2106 template<typename ContainerT, typename PointT> void 2107 queryBBIntersects_noload (const boost::filesystem::path& root_node, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name) 2108 { 2109 OutofcoreOctreeBaseNode<ContainerT, PointT>* root = makenode_norec<ContainerT, PointT> (root_node, NULL); 2110 if (root == NULL) 2111 { 2112 std::cout << "test"; 2113 } 2114 if (root->intersectsWithBoundingBox (min, max)) 2115 { 2116 if (query_depth == root->max_depth_) 2117 { 2118 if (!root->payload_->empty ()) 2119 { 2120 bin_name.push_back (root->thisnodestorage_.string ()); 2121 } 2122 return; 2123 } 2124 2125 for (int i = 0; i < 8; i++) 2126 { 2127 boost::filesystem::path child_dir = root->thisdir_ 2128 / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 2129 if (boost::filesystem::exists (child_dir)) 2130 { 2131 root->children_[i] = makenode_norec (child_dir, root); 2132 root->num_children_++; 2133 queryBBIntersects_noload (root->children_[i], min, max, root->max_depth_ - query_depth, bin_name); 2134 } 2135 } 2136 } 2137 delete root; 2138 } 2139 2140 //////////////////////////////////////////////////////////////////////////////// 2141 2142 template<typename ContainerT, typename PointT> void 2143 queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const std::uint32_t query_depth, std::list<std::string>& bin_name) 2144 { 2145 if (current->intersectsWithBoundingBox (min, max)) 2146 { 2147 if (current->depth_ == query_depth) 2148 { 2149 if (!current->payload_->empty ()) 2150 { 2151 bin_name.push_back (current->thisnodestorage_.string ()); 2152 } 2153 } 2154 else 2155 { 2156 for (int i = 0; i < 8; i++) 2157 { 2158 boost::filesystem::path child_dir = current->thisdir_ / boost::filesystem::path (boost::lexical_cast<std::string> (i)); 2159 if (boost::filesystem::exists (child_dir)) 2160 { 2161 current->children_[i] = makenode_norec<ContainerT, PointT> (child_dir, current); 2162 current->num_children_++; 2163 queryBBIntersects_noload (current->children_[i], min, max, query_depth, bin_name); 2164 } 2165 } 2166 } 2167 } 2168 } 2169 #endif 2170 //////////////////////////////////////////////////////////////////////////////// 2171 2172 }//namespace outofcore 2173 }//namespace pcl 2174 2175 //#define PCL_INSTANTIATE.... 2176 2177 #endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_IMPL_H_ 2178