Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/outofcore/include/pcl/outofcore/octree_base.h
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_