octree_base_node.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_OCTREE_BASE_NODE_H_
#define PCL_OUTOFCORE_OCTREE_BASE_NODE_H_

#include <pcl/common/io.h>
#include <pcl/PCLPointCloud2.h>

#include <pcl/outofcore/boost.h>
#include <pcl/outofcore/octree_base.h>
#include <pcl/outofcore/octree_disk_container.h>
#include <pcl/outofcore/outofcore_node_data.h>

#include <pcl/octree/octree_nodes.h>

namespace pcl
{
  namespace outofcore
  {
    // Forward Declarations
    template<typename ContainerT, typename PointT>
    class OutofcoreOctreeBaseNode;

    template<typename ContainerT, typename PointT>
    class OutofcoreOctreeBase;

    template<typename ContainerT, typename PointT> OutofcoreOctreeBaseNode<ContainerT, PointT>*
    makenode_norec (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);

    template<typename ContainerT, typename PointT> void
    queryBBIntersects_noload (const boost::filesystem::path &root_node, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);

    template<typename ContainerT, typename PointT> void
    queryBBIntersects_noload (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d&, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);

    template<typename ContainerT = OutofcoreOctreeDiskContainer<pcl::PointXYZ>, typename PointT = pcl::PointXYZ>
    class OutofcoreOctreeBaseNode : public pcl::octree::OctreeNode
    {
      friend class OutofcoreOctreeBase<ContainerT, PointT> ;

      //these methods can be rewritten with the iterators. 
      friend OutofcoreOctreeBaseNode<ContainerT, PointT>*
      makenode_norec<ContainerT, PointT> (const boost::filesystem::path &path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);
  
      friend void
      queryBBIntersects_noload<ContainerT, PointT> (const boost::filesystem::path &rootnode, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);

      friend void
      queryBBIntersects_noload<ContainerT, PointT> (OutofcoreOctreeBaseNode<ContainerT, PointT>* current, const Eigen::Vector3d &min, const Eigen::Vector3d &max, const boost::uint32_t query_depth, std::list<std::string> &bin_name);
  
      public:
        typedef OutofcoreOctreeBase<OutofcoreOctreeDiskContainer < PointT > , PointT > octree_disk;
        typedef OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer < PointT > , PointT > octree_disk_node;

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

        typedef pcl::octree::node_type_t node_type_t;

        const static std::string node_index_basename;
        const static std::string node_container_basename;
        const static std::string node_index_extension;
        const static std::string node_container_extension;
        const static double sample_percent_;

        OutofcoreOctreeBaseNode ();

        OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &root_name);

        virtual
        ~OutofcoreOctreeBaseNode ();

        //query
        virtual inline void
        getBoundingBox (Eigen::Vector3d &min_bb, Eigen::Vector3d &max_bb) const
        {
          node_metadata_->getBoundingBox (min_bb, max_bb);
        }


        const boost::filesystem::path&
        getPCDFilename () const
        {
          return node_metadata_->getPCDFilename ();
        }

        const boost::filesystem::path&
        getMetadataFilename () const
        {
          return node_metadata_->getMetadataFilename ();
        }

        void
        queryFrustum (const double planes[24], std::list<std::string>& file_names);

        void
        queryFrustum (const double planes[24], std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check = false);

        void
        queryFrustum (const double planes[24], const Eigen::Vector3d &eye, const Eigen::Matrix4d &view_projection_matrix, std::list<std::string>& file_names, const boost::uint32_t query_depth, const bool skip_vfc_check = false);

        //point extraction
        virtual void
        queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, AlignedPointTVector &dst);

        virtual void
        queryBBIncludes (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, size_t query_depth, const pcl::PCLPointCloud2::Ptr &dst_blob);

        virtual void
        queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::uint64_t query_depth, const double percent, AlignedPointTVector &v);

        virtual void
        queryBBIncludes_subsample (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, boost::uint64_t query_depth, const pcl::PCLPointCloud2::Ptr& dst_blob, double percent = 1.0);

        virtual void
        queryBBIntersects (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const boost::uint32_t query_depth, std::list<std::string> &file_names);

        virtual void
        printBoundingBox (const size_t query_depth) const;

        virtual boost::uint64_t
        addDataToLeaf (const AlignedPointTVector &p, const bool skip_bb_check = true);

        virtual boost::uint64_t
        addDataToLeaf (const std::vector<const PointT*> &p, const bool skip_bb_check = true);

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

        virtual boost::uint64_t
        addPointCloud_and_genLOD (const pcl::PCLPointCloud2::Ptr input_cloud); //, const bool skip_bb_check);
        
        virtual boost::uint64_t
        addDataToLeaf_and_genLOD (const AlignedPointTVector &p, const bool skip_bb_check);

        void
        writeVPythonVisual (std::ofstream &file);

        virtual int
        read (pcl::PCLPointCloud2::Ptr &output_cloud);

        virtual inline node_type_t
        getNodeType () const
        {
          if(this->getNumChildren () > 0)
          {
            return (pcl::octree::BRANCH_NODE);
          }
          else
          {
            return (pcl::octree::LEAF_NODE);
          }
        }
        
        virtual
        OutofcoreOctreeBaseNode* 
        deepCopy () const
        {
          OutofcoreOctreeBaseNode* res = NULL;
          PCL_THROW_EXCEPTION (PCLException, "Not implemented\n");
          return (res);
        }

        virtual inline size_t
        getDepth () const
        {
          return (this->depth_);
        }

        virtual size_t
        getNumChildren () const 
        {
          size_t res = this->countNumChildren ();
          return (res);
        }

        virtual size_t
        getNumLoadedChildren ()  const
        {
          size_t res = this->countNumLoadedChildren ();
          return (res);
        }        
        
        virtual OutofcoreOctreeBaseNode*
        getChildPtr (size_t index_arg) const;

        virtual boost::uint64_t
        getDataSize () const;

        inline virtual void
        clearData ()
        {
          //clears write cache and removes PCD file from disk
          this->payload_->clear ();
        }

      // PROTECTED METHODS
      protected:
        OutofcoreOctreeBaseNode (const boost::filesystem::path &directory_path, OutofcoreOctreeBaseNode<ContainerT, PointT>* super, bool load_all);

        void init_root_node (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, OutofcoreOctreeBase<ContainerT, PointT> * const tree, const boost::filesystem::path &rootname);

        OutofcoreOctreeBaseNode (const OutofcoreOctreeBaseNode &rval);

        OutofcoreOctreeBaseNode&
        operator= (const OutofcoreOctreeBaseNode &rval);

        virtual size_t
        countNumChildren () const;

        virtual size_t
        countNumLoadedChildren () const;
        
        void
        saveIdx (bool recursive);

        void
        randomSample (const AlignedPointTVector &p, AlignedPointTVector &insertBuff, const bool skip_bb_check);

        void
        subdividePoints (const AlignedPointTVector &p, std::vector< AlignedPointTVector > &c, const bool skip_bb_check);
        void
        subdividePoint (const PointT &point, std::vector< AlignedPointTVector > &c);

        boost::uint64_t
        addDataAtMaxDepth (const AlignedPointTVector &p, const bool skip_bb_check = true);

        boost::uint64_t
        addDataAtMaxDepth (const pcl::PCLPointCloud2::Ptr input_cloud, const bool skip_bb_check = true);
        
        inline bool
        intersectsWithBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;

        inline bool
        inBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb) const;

        bool
        pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const Eigen::Vector3d &point);

        static bool
        pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const PointT &p);

        static bool
        pointInBoundingBox (const Eigen::Vector3d &min_bb, const Eigen::Vector3d &max_bb, const double x, const double y, const double z);

        inline bool
        pointInBoundingBox (const PointT &p) const;

        void
        createChild (const std::size_t idx);

        void
        saveMetadataToFile (const boost::filesystem::path &path);

        void
        recFreeChildren ();

        inline boost::uint64_t
        size () const
        {
          return (payload_->size ());
        }

        void
        flushToDiskRecursive ();

        void
        loadFromFile (const boost::filesystem::path &path, OutofcoreOctreeBaseNode* super);

        void
        convertToXYZRecursive ();

        OutofcoreOctreeBaseNode (const Eigen::Vector3d &bb_min, const Eigen::Vector3d &bb_max, const char* dir, OutofcoreOctreeBaseNode<ContainerT, PointT>* super);

        void
        copyAllCurrentAndChildPointsRec (std::list<PointT> &v);

        void
        copyAllCurrentAndChildPointsRec_sub (std::list<PointT> &v, const double percent);

        bool
        hasUnloadedChildren () const;

        virtual void
        loadChildren (bool recursive);

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

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

        void
        sortOctantIndices (const pcl::PCLPointCloud2::Ptr &input_cloud, std::vector< std::vector<int> > &indices, const Eigen::Vector3d &mid_xyz);

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

        OutofcoreOctreeBase<ContainerT, PointT>* m_tree_;//
        OutofcoreOctreeBaseNode* root_node_;//
        OutofcoreOctreeBaseNode* parent_;
        size_t depth_;
        std::vector<OutofcoreOctreeBaseNode*> children_;

        uint64_t num_children_;

        uint64_t num_loaded_children_;

        boost::shared_ptr<ContainerT> payload_;

        static boost::mutex rng_mutex_;

        static boost::mt19937 rand_gen_;

        const static boost::uint32_t rngseed = 0xAABBCCDD;
        const static std::string pcd_extension;

        OutofcoreOctreeNodeMetadata::Ptr node_metadata_;
    };
  }//namespace outofcore
}//namespace pcl

#endif //PCL_OUTOFCORE_OCTREE_BASE_NODE_H_