octree_base.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *  Copyright (c) 2012, Urban Robotics, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  $Id$
 */

#ifndef PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_
#define PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_

#include <pcl/outofcore/boost.h>
#include <pcl/common/io.h>

//outofcore classes
#include <pcl/outofcore/octree_base_node.h>
#include <pcl/outofcore/octree_disk_container.h>
#include <pcl/outofcore/octree_ram_container.h>

//outofcore iterators
#include <pcl/outofcore/outofcore_iterator_base.h>
#include <pcl/outofcore/outofcore_breadth_first_iterator.h>
#include <pcl/outofcore/outofcore_depth_first_iterator.h>
#include <pcl/outofcore/impl/outofcore_breadth_first_iterator.hpp>
#include <pcl/outofcore/impl/outofcore_depth_first_iterator.hpp>

//outofcore metadata
#include <pcl/outofcore/metadata.h>
#include <pcl/outofcore/outofcore_base_data.h>

#include <pcl/filters/filter.h>
#include <pcl/filters/random_sample.h>

#include <pcl/PCLPointCloud2.h>

namespace pcl
{
  namespace outofcore
  {
    struct OutofcoreParams
    {
      std::string node_index_basename_;
      std::string node_container_basename_;
      std::string node_index_extension_;
      std::string node_container_extension_;
      double sample_percent;
    };
    
    template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ>
    class OutofcoreOctreeBase
    {
      friend class OutofcoreOctreeBaseNode<ContainerT, PointT>;
      friend class pcl::outofcore::OutofcoreIteratorBase<PointT, ContainerT>;

      public:

        // public typedefs
        typedef OutofcoreOctreeBase<OutofcoreOctreeDiskContainer<PointT>, PointT > octree_disk;
        typedef OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer<PointT>, PointT > octree_disk_node;

        typedef OutofcoreOctreeBase<OutofcoreOctreeRamContainer<PointT>, PointT> octree_ram;
        typedef OutofcoreOctreeBaseNode<OutofcoreOctreeRamContainer<PointT>, PointT> octree_ram_node;

        typedef OutofcoreOctreeBaseNode<ContainerT, PointT> OutofcoreNodeType;

        typedef OutofcoreOctreeBaseNode<ContainerT, PointT> BranchNode;
        typedef OutofcoreOctreeBaseNode<ContainerT, PointT> LeafNode;

        typedef OutofcoreDepthFirstIterator<PointT, ContainerT> Iterator;
        typedef const OutofcoreDepthFirstIterator<PointT, ContainerT> ConstIterator;

        typedef OutofcoreBreadthFirstIterator<PointT, ContainerT> BreadthFirstIterator;
        typedef const OutofcoreBreadthFirstIterator<PointT, ContainerT> BreadthFirstConstIterator;

        typedef OutofcoreDepthFirstIterator<PointT, ContainerT> DepthFirstIterator;
        typedef const OutofcoreDepthFirstIterator<PointT, ContainerT> DepthFirstConstIterator;

        typedef boost::shared_ptr<OutofcoreOctreeBase<ContainerT, PointT> > Ptr;
        typedef boost::shared_ptr<const OutofcoreOctreeBase<ContainerT, PointT> > ConstPtr;

        typedef pcl::PointCloud<PointT> PointCloud;

        typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
        typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;

        typedef boost::shared_ptr<PointCloud> PointCloudPtr;
        typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;

        typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector;

        // Constructors
        // -----------------------------------------------------------------------

        OutofcoreOctreeBase (const boost::filesystem::path &root_node_name, const bool load_all);

        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);

        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);

        virtual
        ~OutofcoreOctreeBase ();

        // Point/Region INSERTION methods
        // --------------------------------------------------------------------------------
        boost::uint64_t
        addDataToLeaf (const AlignedPointTVector &p);

        boost::uint64_t
        addPointCloud (PointCloudConstPtr point_cloud);

        boost::uint64_t
        addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud, const bool skip_bb_check = false);

        boost::uint64_t
        addPointCloud_and_genLOD (pcl::PCLPointCloud2::Ptr &input_cloud);

        boost::uint64_t
        addPointCloud (pcl::PCLPointCloud2::Ptr &input_cloud);
        
        boost::uint64_t
        addPointCloud_and_genLOD (PointCloudConstPtr point_cloud);

        boost::uint64_t
        addDataToLeaf_and_genLOD (AlignedPointTVector &p);

        // Frustrum/Box/Region REQUESTS/QUERIES: DB Accessors
        // -----------------------------------------------------------------------
        void
        queryFrustum (const double *planes, std::list<std::string>& file_names) const;

              void
        queryFrustum (const double *planes, std::list<std::string>& file_names, const boost::uint32_t query_depth) const;

              void
        queryFrustum (const double *planes, const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix,
                      std::list<std::string>& file_names, const boost::uint32_t query_depth) const;
        
        //--------------------------------------------------------------------------------
        //templated PointT methods
        //--------------------------------------------------------------------------------

        void
        queryBBIntersects (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name) const;

        void
        queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, AlignedPointTVector &dst) const;

        void
        queryBBIncludes (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob) const;
        
        void
        queryBBIncludes_subsample (const Eigen::Vector3d &min, const Eigen::Vector3d &max, uint64_t query_depth, const double percent, AlignedPointTVector &dst) const;

        //--------------------------------------------------------------------------------
        //PCLPointCloud2 methods
        //--------------------------------------------------------------------------------

        virtual void
        queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob, double percent = 1.0);
        
        inline virtual void
        queryBoundingBox (const Eigen::Vector3d &min, const Eigen::Vector3d &max, const int query_depth, std::list<std::string> &filenames) const
        {
          boost::shared_lock < boost::shared_mutex > lock (read_write_mutex_);
          filenames.clear ();
          this->root_node_->queryBBIntersects (min, max, query_depth, filenames);
        }

        // Parameterization: getters and setters
        // --------------------------------------------------------------------------------

        bool
        getBoundingBox (Eigen::Vector3d &min, Eigen::Vector3d &max) const;

        inline boost::uint64_t
        getNumPointsAtDepth (const boost::uint64_t& depth_index) const
        {
          return (metadata_->getLODPoints (depth_index));
        }

        boost::uint64_t
        queryBoundingBoxNumPoints (const Eigen::Vector3d& min, const Eigen::Vector3d& max, const int query_depth, bool load_from_disk = true);
        

        inline const std::vector<boost::uint64_t>&
        getNumPointsVector () const
        {
          return (metadata_->getLODPoints ());
        }

        inline boost::uint64_t
        getDepth () const
        {
          return (metadata_->getDepth ());
        }

        inline boost::uint64_t
        getTreeDepth () const
        {
          return (this->getDepth ());
        }

        bool
        getBinDimension (double &x, double &y) const;

        double
        getVoxelSideLength (const boost::uint64_t& depth) const;

        double
        getVoxelSideLength () const
        {
          return (this->getVoxelSideLength (metadata_->getDepth ()));
        }

        const std::string&
        getCoordSystem () const
        {
          return (metadata_->getCoordinateSystem ());
        }

        // Mutators
        // -----------------------------------------------------------------------

        void
        buildLOD ();

        void
        printBoundingBox (const size_t query_depth) const;

        void
        printBoundingBox (OutofcoreNodeType& node) const;

        inline void
        printBoundingBox() const
        {
          this->printBoundingBox (metadata_->getDepth ());
        }

        void
        getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers, size_t query_depth) const;

        void
        getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers, size_t query_depth) const;

        void
        getOccupiedVoxelCenters(AlignedPointTVector &voxel_centers) const
        {
          getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ());
        }

        void
        getOccupiedVoxelCenters(std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &voxel_centers) const
        {
          getOccupiedVoxelCenters(voxel_centers, metadata_->getDepth ());
        }

        // Serializers
        // -----------------------------------------------------------------------

        void
        convertToXYZ ();

        void
        writeVPythonVisual (const boost::filesystem::path filename);

        OutofcoreNodeType*
        getBranchChildPtr (const BranchNode& branch_arg, unsigned char childIdx_arg) const;

        pcl::Filter<pcl::PCLPointCloud2>::Ptr
        getLODFilter ();

        const pcl::Filter<pcl::PCLPointCloud2>::ConstPtr
        getLODFilter () const;

        void
        setLODFilter (const pcl::Filter<pcl::PCLPointCloud2>::Ptr& filter_arg);

        double 
        getSamplePercent () const
        {
          return (sample_percent_);
        }
        
        inline void 
        setSamplePercent (const double sample_percent_arg)
        {
          this->sample_percent_ = std::fabs (sample_percent_arg) > 1.0 ? 1.0 : std::fabs (sample_percent_arg);
        }
        
      protected:
        void
        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);

        OutofcoreOctreeBase (OutofcoreOctreeBase &rval);

        OutofcoreOctreeBase (const OutofcoreOctreeBase &rval);

        OutofcoreOctreeBase&
        operator= (OutofcoreOctreeBase &rval);

        OutofcoreOctreeBase&
        operator= (const OutofcoreOctreeBase &rval);

        inline OutofcoreNodeType*
        getRootNode ()
        {
          return (this->root_node_);
        }

        void
        DeAllocEmptyNodeCache (OutofcoreNodeType* current);

        void
        saveToFile ();

        void
        buildLODRecursive (const std::vector<BranchNode*>& current_branch);

        inline void
        incrementPointsInLOD (boost::uint64_t depth, boost::uint64_t inc);

        bool
        checkExtension (const boost::filesystem::path& path_name);


        void
        flushToDisk ();

        void
        flushToDiskLazy ();

        void
        DeAllocEmptyNodeCache ();

        OutofcoreNodeType* root_node_;

        mutable boost::shared_mutex read_write_mutex_;

        boost::shared_ptr<OutofcoreOctreeBaseMetadata> metadata_;
        
        const static std::string TREE_EXTENSION_;
        const static int OUTOFCORE_VERSION_;

        const static uint64_t LOAD_COUNT_ = static_cast<uint64_t>(2e9);

      private:    

        void
        enlargeToCube (Eigen::Vector3d &bb_min, Eigen::Vector3d &bb_max);

        boost::uint64_t
        calculateDepth (const Eigen::Vector3d& min_bb, const Eigen::Vector3d& max_bb, const double leaf_resolution);

        double sample_percent_;

        pcl::RandomSample<pcl::PCLPointCloud2>::Ptr lod_filter_ptr_;
        
    };
  }
}

  
#endif // PCL_OUTOFCORE_OUTOFCOREOCTREEBASE_H_