Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012, Urban Robotics, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of Willow Garage, Inc. nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 */ 00039 00040 #ifndef PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_ 00041 #define PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_ 00042 00043 #include <pcl/outofcore/boost.h> 00044 #include <pcl/common/io.h> 00045 00046 //outofcore classes 00047 #include <pcl/outofcore/octree_base_node.h> 00048 #include <pcl/outofcore/octree_disk_container.h> 00049 #include <pcl/outofcore/octree_ram_container.h> 00050 00051 //outofcore iterators 00052 #include <pcl/outofcore/outofcore_iterator_base.h> 00053 #include <pcl/outofcore/outofcore_breadth_first_iterator.h> 00054 #include <pcl/outofcore/outofcore_depth_first_iterator.h> 00055 #include <pcl/outofcore/impl/outofcore_breadth_first_iterator.hpp> 00056 #include <pcl/outofcore/impl/outofcore_depth_first_iterator.hpp> 00057 00058 //outofcore metadata 00059 #include <pcl/outofcore/metadata.h> 00060 #include <pcl/outofcore/outofcore_base_data.h> 00061 00062 #include <pcl/filters/filter.h> 00063 #include <pcl/filters/random_sample.h> 00064 00065 #include <pcl/PCLPointCloud2.h> 00066 00067 namespace pcl 00068 { 00069 namespace outofcore 00070 { 00071 struct OutofcoreParams 00072 { 00073 std::string node_index_basename_; 00074 std::string node_container_basename_; 00075 std::string node_index_extension_; 00076 std::string node_container_extension_; 00077 double sample_percent; 00078 }; 00079 00080 /** \class OutofcoreOctreeBase 00081 * \brief This code defines the octree used for point storage at Urban Robotics. 00082 * 00083 * \note Code was adapted from the Urban Robotics out of core octree implementation. 00084 * Contact Jacob Schloss <jacob.schloss@urbanrobotics.net> with any questions. 00085 * http://www.urbanrobotics.net/. This code was integrated for the Urban Robotics 00086 * Code Sprint (URCS) by Stephen Fox (foxstephend@gmail.com). Additional development notes can be found at 00087 * http://www.pointclouds.org/blog/urcs/. 00088 * 00089 * The primary purpose of this class is an interface to the 00090 * recursive traversal (recursion handled by \ref OutofcoreOctreeBaseNode) of the 00091 * in-memory/top-level octree structure. The metadata in each node 00092 * can be loaded entirely into main memory, from which the tree can be traversed 00093 * recursively in this state. This class provides an the interface 00094 * for: 00095 * -# Point/Region insertion methods 00096 * -# Frustrum/box/region queries 00097 * -# Parameterization of resolution, container type, etc... 00098 * 00099 * For lower-level node access, there is a Depth-First iterator 00100 * for traversing the trees with direct access to the nodes. This 00101 * can be used for implementing other algorithms, and other 00102 * iterators can be written in a similar fashion. 00103 * 00104 * The format of the octree is stored on disk in a hierarchical 00105 * octree structure, where .oct_idx are the JSON-based node 00106 * metadata files managed by \ref OutofcoreOctreeNodeMetadata, 00107 * and .octree is the JSON-based octree metadata file managed by 00108 * \ref OutofcoreOctreeBaseMetadata. Children of each node live 00109 * in up to eight subdirectories named from 0 to 7, where a 00110 * metadata and optionally a pcd file will exist. The PCD files 00111 * are stored in compressed binary PCD format, containing all of 00112 * the fields existing in the PCLPointCloud2 objects originally 00113 * inserted into the out of core object. 00114 * 00115 * A brief outline of the out of core octree can be seen 00116 * below. The files in [brackets] exist only when the LOD are 00117 * built. 00118 * 00119 * At this point in time, there is not support for multiple trees 00120 * existing in a single directory hierarchy. 00121 * 00122 * \verbatim 00123 tree_name/ 00124 tree_name.oct_idx 00125 tree_name.octree 00126 [tree_name-uuid.pcd] 00127 0/ 00128 tree_name.oct_idx 00129 [tree_name-uuid.pcd] 00130 0/ 00131 ... 00132 1/ 00133 ... 00134 ... 00135 0/ 00136 tree_name.oct_idx 00137 tree_name.pcd 00138 1/ 00139 ... 00140 7/ 00141 \endverbatim 00142 * 00143 * \ingroup outofcore 00144 * \author Jacob Schloss (jacob.schloss@urbanrobotics.net) 00145 * \author Stephen Fox, Urban Robotics Code Sprint (foxstephend@gmail.com) 00146 * 00147 */ 00148 template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ> 00149 class OutofcoreOctreeBase 00150 { 00151 friend class OutofcoreOctreeBaseNode<ContainerT, PointT>; 00152 friend class pcl::outofcore::OutofcoreIteratorBase<PointT, ContainerT>; 00153 00154 public: 00155 00156 // public typedefs 00157 typedef OutofcoreOctreeBase<OutofcoreOctreeDiskContainer<PointT>, PointT > octree_disk; 00158 typedef OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer<PointT>, PointT > octree_disk_node; 00159 00160 typedef OutofcoreOctreeBase<OutofcoreOctreeRamContainer<PointT>, PointT> octree_ram; 00161 typedef OutofcoreOctreeBaseNode<OutofcoreOctreeRamContainer<PointT>, PointT> octree_ram_node; 00162 00163 typedef OutofcoreOctreeBaseNode<ContainerT, PointT> OutofcoreNodeType; 00164 00165 typedef OutofcoreOctreeBaseNode<ContainerT, PointT> BranchNode; 00166 typedef OutofcoreOctreeBaseNode<ContainerT, PointT> LeafNode; 00167 00168 typedef OutofcoreDepthFirstIterator<PointT, ContainerT> Iterator; 00169 typedef const OutofcoreDepthFirstIterator<PointT, ContainerT> ConstIterator; 00170 00171 typedef OutofcoreBreadthFirstIterator<PointT, ContainerT> BreadthFirstIterator; 00172 typedef const OutofcoreBreadthFirstIterator<PointT, ContainerT> BreadthFirstConstIterator; 00173 00174 typedef OutofcoreDepthFirstIterator<PointT, ContainerT> DepthFirstIterator; 00175 typedef const OutofcoreDepthFirstIterator<PointT, ContainerT> DepthFirstConstIterator; 00176 00177 typedef boost::shared_ptr<OutofcoreOctreeBase<ContainerT, PointT> > Ptr; 00178 typedef boost::shared_ptr<const OutofcoreOctreeBase<ContainerT, PointT> > ConstPtr; 00179 00180 typedef pcl::PointCloud<PointT> PointCloud; 00181 00182 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00183 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00184 00185 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00186 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00187 00188 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector; 00189 00190 // Constructors 00191 // ----------------------------------------------------------------------- 00192 00193 /** \brief Load an existing tree 00194 * 00195 * If load_all is set, the BB and point count for every node is loaded, 00196 * otherwise only the root node is actually created, and the rest will be 00197 * generated on insertion or query. 00198 * 00199 * \param Path to the top-level tree/tree.oct_idx metadata file 00200 * \param load_all Load entire tree metadata (does not load any points from disk) 00201 * \throws PCLException for bad extension (root node metadata must be .oct_idx extension) 00202 */ 00203 OutofcoreOctreeBase (const boost::filesystem::path &root_node_name, const bool load_all); 00204 00205 /** \brief Create a new tree 00206 * 00207 * Create a new tree rootname with specified bounding box; will remove and overwrite existing tree with the same name 00208 * 00209 * Computes the depth of the tree based on desired leaf , then calls the other constructor. 00210 * 00211 * \param min Bounding box min 00212 * \param max Bounding box max 00213 * \param node_dim_meters Node dimension in meters (assuming your point data is in meters) 00214 * \param root_node_name must end in ".oct_idx" 00215 * \param coord_sys Coordinate system which is stored in the JSON metadata 00216 * \throws PCLException if root file extension does not match \ref OutofcoreOctreeBaseNode::node_index_extension 00217 */ 00218 OutofcoreOctreeBase (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const double resolution_arg, const boost::filesystem::path &root_node_name, const std::string &coord_sys); 00219 00220 /** \brief Create a new tree; will not overwrite existing tree of same name 00221 * 00222 * Create a new tree rootname with specified bounding box; will not overwrite an existing tree 00223 * 00224 * \param max_depth Specifies a fixed number of LODs to generate, which is the depth of the tree 00225 * \param min Bounding box min 00226 * \param max Bounding box max 00227 * \note Bounding box of the tree must be set before inserting any points. The tree \b cannot be resized at this time. 00228 * \param root_node_name must end in ".oct_idx" 00229 * \param coord_sys Coordinate system which is stored in the JSON metadata 00230 * \throws PCLException if the parent directory has existing children (detects an existing tree) 00231 * \throws PCLException if file extension is not ".oct_idx" 00232 */ 00233 OutofcoreOctreeBase (const boost::uint64_t max_depth, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::filesystem::path &root_node_name, const std::string &coord_sys); 00234 00235 virtual 00236 ~OutofcoreOctreeBase (); 00237 00238 // Point/Region INSERTION methods 00239 // -------------------------------------------------------------------------------- 00240 /** \brief Recursively add points to the tree 00241 * \note shared read_write_mutex lock occurs 00242 */ 00243 boost::uint64_t 00244 addDataToLeaf (const AlignedPointTVector &p); 00245 00246 /** \brief Copies the points from the point_cloud falling within the bounding box of the octree to the 00247 * out-of-core octree; this is an interface to addDataToLeaf and can be used multiple times. 00248 * \param point_cloud Pointer to the point cloud data to copy to the outofcore octree; Assumes templated 00249 * PointT matches for each. 00250 * \return Number of points successfully copied from the point cloud to the octree. 00251 */ 00252 boost::uint64_t 00253 addPointCloud (PointCloudConstPtr point_cloud); 00254 00255 /** \brief Recursively copies points from input_cloud into the leaf nodes of the out-of-core octree, and stores them to disk. 00256 * 00257 * \param[in] input_cloud The cloud of points to be inserted into the out-of-core octree. Note if multiple PCLPointCloud2 objects are added to the tree, this assumes that they all have exactly the same fields. 00258 * \param[in] skip_bb_check (default=false) whether to skip the bounding box check on insertion. Note the bounding box check is never skipped in the current implementation. 00259 * \return Number of poitns successfully copied from the point cloud to the octree 00260 */ 00261 boost::uint64_t 00262 addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false); 00263 00264 /** \brief Recursively add points to the tree. 00265 * 00266 * Recursively add points to the tree. 1/8 of the remaining 00267 * points at each LOD are stored at each internal node of the 00268 * octree until either (a) runs out of points, in which case 00269 * the leaf is not at the maximum depth of the tree, or (b) 00270 * a larger set of points falls in the leaf at the maximum depth. 00271 * Note unlike the old implementation, multiple 00272 * copies of the same point will \b not be added at multiple 00273 * LODs as it walks the tree. Once the point is added to the 00274 * octree, it is no longer propagated further down the tree. 00275 * 00276 *\param[in] input_cloud The input cloud of points which will 00277 * be copied into the sorted nodes of the out-of-core octree 00278 * \return The total number of points added to the out-of-core 00279 * octree. 00280 */ 00281 boost::uint64_t 00282 addPointCloud_and_genLOD (pcl::PCLPointCloud2::Ptr &input_cloud); 00283 00284 boost::uint64_t 00285 addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud); 00286 00287 boost::uint64_t 00288 addPointCloud_and_genLOD (PointCloudConstPtr point_cloud); 00289 00290 /** \brief Recursively add points to the tree subsampling LODs on the way. 00291 * 00292 * shared read_write_mutex lock occurs 00293 */ 00294 boost::uint64_t 00295 addDataToLeaf_and_genLOD (AlignedPointTVector &p); 00296 00297 // Frustrum/Box/Region REQUESTS/QUERIES: DB Accessors 00298 // ----------------------------------------------------------------------- 00299 void 00300 queryFrustum (const double *planes, std::list<std::string>& file_names) const; 00301 00302 void 00303 queryFrustum (const double *planes, std::list<std::string>& file_names, const boost::uint32_t query_depth) const; 00304 00305 void 00306 queryFrustum (const double *planes, const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, 00307 std::list<std::string>& file_names, const boost::uint32_t query_depth) const; 00308 00309 //-------------------------------------------------------------------------------- 00310 //templated PointT methods 00311 //-------------------------------------------------------------------------------- 00312 00313 /** \brief Get a list of file paths at query_depth that intersect with your bounding box specified by \ref min and \ref max. When querying with this method, you may be stuck with extra data (some outside of your query bounds) that reside in the files. 00314 * 00315 * \param[in] min The minimum corner of the bounding box 00316 * \param[in] max The maximum corner of the bounding box 00317 * \param[in] query_depth 0 is root, (this->depth) is full 00318 * \param[out] bin_name List of paths to point data files (PCD currently) which satisfy the query 00319 */ 00320 void 00321 queryBBIntersects (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name) const; 00322 00323 /** \brief Get Points in BB, only points inside BB. The query 00324 * processes the data at each node, filtering points that fall 00325 * out of the query bounds, and returns a single, concatenated 00326 * point cloud. 00327 * 00328 * \param[in] min The minimum corner of the bounding box for querying 00329 * \param[in] max The maximum corner of the bounding box for querying 00330 * \param[in] query_depth The depth from which point data will be taken 00331 * \note If the LODs of the tree have not been built, you must specify the maximum depth in order to retrieve any data 00332 * \param[out] dst The destination vector of points 00333 */ 00334 void 00335 queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, AlignedPointTVector &dst) const; 00336 00337 /** \brief Query all points falling within the input bounding box at \ref query_depth and return a PCLPointCloud2 object in \ref dst_blob. 00338 * 00339 * \param[in] min The minimum corner of the input bounding box. 00340 * \param[in] max The maximum corner of the input bounding box. 00341 * \param[in] query_depth The query depth at which to search for points; only points at this depth are returned 00342 * \param[out] dst_blob Storage location for the points satisfying the query. 00343 **/ 00344 void 00345 queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob) const; 00346 00347 /** \brief Returns a random subsample of points within the given bounding box at \ref query_depth. 00348 * 00349 * \param[in] min The minimum corner of the boudning box to query. 00350 * \param[out] max The maximum corner of the bounding box to query. 00351 * \param[in] query_depth The depth in the tree at which to look for the points. Only returns points within the given bounding box at the specified \ref query_depth. 00352 * \param[out] dst The destination in which to return the points. 00353 * 00354 */ 00355 void 00356 queryBBIncludes_subsample (const Eigen::Vector3d &min, const Eigen::Vector3d &max, uint64_t query_depth, const double percent, AlignedPointTVector &dst) const; 00357 00358 //-------------------------------------------------------------------------------- 00359 //PCLPointCloud2 methods 00360 //-------------------------------------------------------------------------------- 00361 00362 /** \brief Query all points falling within the input bounding box at \ref query_depth and return a PCLPointCloud2 object in \ref dst_blob. If the optional argument for filter is given, points are processed by that filter before returning. 00363 * \param[in] min The minimum corner of the input bounding box. 00364 * \param[in] max The maximum corner of the input bounding box. 00365 * \param[in] query_depth The depth of tree at which to query; only points at this depth are returned 00366 * \param[out] dst_blob The destination in which points within the bounding box are stored. 00367 * \param[in] percent optional sampling percentage which is applied after each time data are read from disk 00368 */ 00369 virtual void 00370 queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent = 1.0); 00371 00372 /** \brief Returns list of pcd files from nodes whose bounding boxes intersect with the input bounding box. 00373 * \param[in] min The minimum corner of the input bounding box. 00374 * \param[in] max The maximum corner of the input bounding box. 00375 * \param[out] filenames The list of paths to the PCD files which can be loaded and processed. 00376 */ 00377 inline virtual void 00378 queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, std::list<std::string> &filenames) const 00379 { 00380 boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_); 00381 filenames.clear (); 00382 this->root_node_->queryBBIntersects (min, max, query_depth, filenames); 00383 } 00384 00385 // Parameterization: getters and setters 00386 // -------------------------------------------------------------------------------- 00387 00388 /** \brief Get the overall bounding box of the outofcore 00389 * octree; this is the same as the bounding box of the \ref root_node_ node */ 00390 bool 00391 getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const; 00392 00393 /** \brief Get number of points at specified LOD 00394 * \param[in] depth the level of detail at which we want the number of points (0 is root, 1, 2,...) 00395 * \return number of points in the tree at \b depth 00396 */ 00397 inline boost::uint64_t 00398 getNumPointsAtDepth (const boost::uint64_t& depth_index) const 00399 { 00400 return (metadata_->getLODPoints (depth_index)); 00401 } 00402 00403 /** \brief Queries the number of points in a bounding box 00404 * 00405 * \param[in] min The minimum corner of the input bounding box 00406 * \param[out] max The maximum corner of the input bounding box 00407 * \param[in] query_depth The depth of the nodes to restrict the search to (only this depth is searched) 00408 * \param[in] load_from_disk (default true) Whether to load PCD files to count exactly the number of points within the bounding box; setting this to false will return an upper bound by just reading the number of points from the PCD header, even if there may be some points in that node do not fall within the query bounding box. 00409 * \return Number of points in the bounding box at depth \b query_depth 00410 **/ 00411 boost::uint64_t 00412 queryBoundingBoxNumPoints (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const int query_depth, bool load_from_disk = true); 00413 00414 00415 /** \brief Get number of points at each LOD 00416 * \return vector of number of points in each LOD indexed by each level of depth, 0 to the depth of the tree. 00417 */ 00418 inline const std::vector<boost::uint64_t>& 00419 getNumPointsVector () const 00420 { 00421 return (metadata_->getLODPoints ()); 00422 } 00423 00424 /** \brief Get number of LODs, which is the height of the tree 00425 */ 00426 inline boost::uint64_t 00427 getDepth () const 00428 { 00429 return (metadata_->getDepth ()); 00430 } 00431 00432 inline boost::uint64_t 00433 getTreeDepth () const 00434 { 00435 return (this->getDepth ()); 00436 } 00437 00438 /** \brief Computes the expected voxel dimensions at the leaves 00439 */ 00440 bool 00441 getBinDimension (double &x, double &y) const; 00442 00443 /** \brief gets the side length of an (assumed) perfect cubic voxel. 00444 * \note If the initial bounding box specified in constructing the octree is not square, then this method does not return a sensible value 00445 * \return the side length of the cubic voxel size at the specified depth 00446 */ 00447 double 00448 getVoxelSideLength (const boost::uint64_t& depth) const; 00449 00450 /** \brief Gets the smallest (assumed) cubic voxel side lengths. The smallest voxels are located at the max depth of the tree. 00451 * \return The side length of a the cubic voxel located at the leaves 00452 */ 00453 double 00454 getVoxelSideLength () const 00455 { 00456 return (this->getVoxelSideLength (metadata_->getDepth ())); 00457 } 00458 00459 /** \brief Get coordinate system tag from the JSON metadata file 00460 */ 00461 const std::string& 00462 getCoordSystem () const 00463 { 00464 return (metadata_->getCoordinateSystem ()); 00465 } 00466 00467 // Mutators 00468 // ----------------------------------------------------------------------- 00469 00470 /** \brief Generate multi-resolution LODs for the tree, which are a uniform random sampling all child leafs below the node. 00471 */ 00472 void 00473 buildLOD (); 00474 00475 /** \brief Prints size of BBox to stdout 00476 */ 00477 void 00478 printBoundingBox (const size_t query_depth) const; 00479 00480 /** \brief Prints the coordinates of the bounding box of the node to stdout */ 00481 void 00482 printBoundingBox (OutofcoreNodeType& node) const; 00483 00484 /** \brief Prints size of the bounding boxes to stdou 00485 */ 00486 inline void 00487 printBoundingBox() const 00488 { 00489 this->printBoundingBox (metadata_->getDepth ()); 00490 } 00491 00492 /** \brief Returns the voxel centers of all existing voxels at \ref query_depth 00493 \param[in] query_depth: the depth of the tree at which to retrieve occupied/existing voxels 00494 \param[out] vector of PointXYZ voxel centers for nodes that exist at that depth 00495 */ 00496 void 00497 getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, size_t query_depth) const; 00498 00499 /** \brief Returns the voxel centers of all existing voxels at \ref query_depth 00500 \param[in] query_depth: the depth of the tree at which to retrieve occupied/existing voxels 00501 \param[out] vector of PointXYZ voxel centers for nodes that exist at that depth 00502 */ 00503 void 00504 getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, size_t query_depth) const; 00505 00506 /** \brief Gets the voxel centers of all occupied/existing leaves of the tree */ 00507 void 00508 getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers) const 00509 { 00510 getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ()); 00511 } 00512 00513 /** \brief Returns the voxel centers of all occupied/existing leaves of the tree 00514 * \param[out] voxel_centers std::vector of the centers of all occupied leaves of the octree 00515 */ 00516 void 00517 getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers) const 00518 { 00519 getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ()); 00520 } 00521 00522 // Serializers 00523 // ----------------------------------------------------------------------- 00524 00525 /** \brief Save each .bin file as an XYZ file */ 00526 void 00527 convertToXYZ (); 00528 00529 /** \brief Write a python script using the vpython module containing all 00530 * the bounding boxes */ 00531 void 00532 writeVPythonVisual (const boost::filesystem::path filename); 00533 00534 OutofcoreNodeType* 00535 getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const; 00536 00537 pcl::Filter<pcl::PCLPointCloud2>::Ptr 00538 getLODFilter (); 00539 00540 const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr 00541 getLODFilter () const; 00542 00543 /** \brief Sets the filter to use when building the levels of depth. Recommended filters are pcl::RandomSample<pcl::PCLPointCloud2> or pcl::VoxelGrid */ 00544 void 00545 setLODFilter (const pcl::Filter<pcl::PCLPointCloud2>::Ptr& filter_arg); 00546 00547 /** \brief Returns the sample_percent_ used when constructing the LOD. */ 00548 double 00549 getSamplePercent () const 00550 { 00551 return (sample_percent_); 00552 } 00553 00554 /** \brief Sets the sampling percent for constructing LODs. Each LOD gets sample_percent^d points. 00555 * \param[in] sample_percent_arg Percentage between 0 and 1. */ 00556 inline void 00557 setSamplePercent (const double sample_percent_arg) 00558 { 00559 this->sample_percent_ = std::fabs (sample_percent_arg) > 1.0 ? 1.0 : std::fabs (sample_percent_arg); 00560 } 00561 00562 protected: 00563 void 00564 init (const boost::uint64_t& depth, const Eigen::Vector3d& min, const Eigen::Vector3d& max, const boost::filesystem::path& root_name, const std::string& coord_sys); 00565 00566 OutofcoreOctreeBase (OutofcoreOctreeBase &rval); 00567 00568 OutofcoreOctreeBase (const OutofcoreOctreeBase &rval); 00569 00570 OutofcoreOctreeBase& 00571 operator= (OutofcoreOctreeBase &rval); 00572 00573 OutofcoreOctreeBase& 00574 operator= (const OutofcoreOctreeBase &rval); 00575 00576 inline OutofcoreNodeType* 00577 getRootNode () 00578 { 00579 return (this->root_node_); 00580 } 00581 00582 /** \brief flush empty nodes only */ 00583 void 00584 DeAllocEmptyNodeCache (OutofcoreNodeType* current); 00585 00586 /** \brief Write octree definition ".octree" (defined by octree_extension_) to disk */ 00587 void 00588 saveToFile (); 00589 00590 /** \brief recursive portion of lod builder */ 00591 void 00592 buildLODRecursive (const std::vector<BranchNode*>& current_branch); 00593 00594 /** \brief Increment current depths (LOD for branch nodes) point count; called by addDataAtMaxDepth in OutofcoreOctreeBaseNode 00595 */ 00596 inline void 00597 incrementPointsInLOD (boost::uint64_t depth, boost::uint64_t inc); 00598 00599 /** \brief Auxiliary function to validate path_name extension is .octree 00600 * 00601 * \return 0 if bad; 1 if extension is .oct_idx 00602 */ 00603 bool 00604 checkExtension (const boost::filesystem::path& path_name); 00605 00606 00607 /** \brief DEPRECATED - Flush all nodes' cache 00608 * \deprecated this was moved to the octree_node class 00609 */ 00610 void 00611 flushToDisk (); 00612 00613 /** \brief DEPRECATED - Flush all non leaf nodes' cache 00614 * \deprecated 00615 */ 00616 void 00617 flushToDiskLazy (); 00618 00619 /** \brief DEPRECATED - Flush empty nodes only 00620 * \deprecated 00621 */ 00622 void 00623 DeAllocEmptyNodeCache (); 00624 00625 /** \brief Pointer to the root node of the octree data structure */ 00626 OutofcoreNodeType* root_node_; 00627 00628 /** \brief shared mutex for controlling read/write access to disk */ 00629 mutable boost::shared_mutex read_write_mutex_; 00630 00631 boost::shared_ptr<OutofcoreOctreeBaseMetadata> metadata_; 00632 00633 /** \brief defined as ".octree" to append to treepath files 00634 * \note this might change 00635 */ 00636 const static std::string TREE_EXTENSION_; 00637 const static int OUTOFCORE_VERSION_; 00638 00639 const static uint64_t LOAD_COUNT_ = static_cast<uint64_t>(2e9); 00640 00641 private: 00642 00643 /** \brief Auxiliary function to enlarge a bounding box to a cube. */ 00644 void 00645 enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max); 00646 00647 /** \brief Auxiliary function to compute the depth of the tree given the bounding box and the desired size of the leaf voxels */ 00648 boost::uint64_t 00649 calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution); 00650 00651 double sample_percent_; 00652 00653 pcl::RandomSample<pcl::PCLPointCloud2>::Ptr lod_filter_ptr_; 00654 00655 }; 00656 } 00657 } 00658 00659 00660 #endif // PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_