octree_pointcloud.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, 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: octree_pointcloud.h 4702 2012-02-23 09:39:33Z gedikli $
 */

#ifndef OCTREE_POINTCLOUD_H
#define OCTREE_POINTCLOUD_H

#include "octree_base.h"
#include "octree2buf_base.h"
#include "octree_lowmemory_base.h"

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "octree_nodes.h"
#include "octree_iterator.h"

#include <queue>
#include <vector>
#include <algorithm>
#include <iostream>

namespace pcl
{
  namespace octree
  {



    template<typename PointT, typename LeafT = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> >
    class OctreePointCloud : public OctreeT
    {
      // iterators are friends
      friend class OctreeIteratorBase<int, LeafT, OctreeT> ;
      friend class OctreeDepthFirstIterator<int, LeafT, OctreeT> ;
      friend class OctreeBreadthFirstIterator<int, LeafT, OctreeT> ;
      friend class OctreeLeafNodeIterator<int, LeafT, OctreeT> ;

      public:
        typedef OctreeT Base;
        typedef typename OctreeT::OctreeLeaf OctreeLeaf;

        // Octree iterators
        typedef OctreeDepthFirstIterator<int, LeafT, OctreeT> Iterator;
        typedef const OctreeDepthFirstIterator<int, LeafT, OctreeT> ConstIterator;

        typedef OctreeLeafNodeIterator<int, LeafT, OctreeT> LeafNodeIterator;
        typedef const OctreeLeafNodeIterator<int, LeafT, OctreeT> ConstLeafNodeIterator;

        typedef OctreeDepthFirstIterator<int, LeafT, OctreeT> DepthFirstIterator;
        typedef const OctreeDepthFirstIterator<int, LeafT, OctreeT> ConstDepthFirstIterator;
        typedef OctreeBreadthFirstIterator<int, LeafT, OctreeT> BreadthFirstIterator;
        typedef const OctreeBreadthFirstIterator<int, LeafT, OctreeT> ConstBreadthFirstIterator;

        OctreePointCloud (const double resolution_arg);

        virtual
        ~OctreePointCloud ();

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

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

        // public typedefs for single/double buffering
        typedef OctreePointCloud<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer;
        typedef OctreePointCloud<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer;
        typedef OctreePointCloud<PointT, LeafT, OctreeLowMemBase<int, LeafT> > LowMem;

        // Boost shared pointers
        typedef boost::shared_ptr<OctreePointCloud<PointT, LeafT, OctreeT> > Ptr;
        typedef boost::shared_ptr<const OctreePointCloud<PointT, LeafT, OctreeT> > ConstPtr;

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

        inline void
        setInputCloud (const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg = IndicesConstPtr ())
        {
          assert (this->leafCount_==0);

          if ((input_ != cloud_arg) && (this->leafCount_ == 0))
          {
            input_ = cloud_arg;
            indices_ = indices_arg;
          }
        }

        inline IndicesConstPtr const
        getIndices () const
        {
          return (indices_);
        }

        inline PointCloudConstPtr
        getInputCloud () const
        {
          return (input_);
        }

        inline void
        setEpsilon (double eps)
        {
          epsilon_ = eps;
        }

        inline double
        getEpsilon () const
        {
          return (epsilon_);
        }

        inline void
        setResolution (double resolution_arg)
        {
          // octree needs to be empty to change its resolution
          assert( this->leafCount_ == 0 );

          resolution_ = resolution_arg;

          getKeyBitSize();
        }

        inline double
        getResolution () const
        {
          return (resolution_);
        }

        inline unsigned int
        getTreeDepth () const
        {
          return this->octreeDepth_;
        }

        void
        addPointsFromInputCloud ();

        void
        addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg);

        void
        addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg);

        void
        addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg);

        bool
        isVoxelOccupiedAtPoint (const PointT& point_arg) const;

        void
        deleteTree ()
        {
          // reset bounding box
          minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0;
          maxKeys_ = 1;
          this->boundingBoxDefined_ = false;

          OctreeT::deleteTree ();
        }

        bool
        isVoxelOccupiedAtPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg) const;

        bool
        isVoxelOccupiedAtPoint (const int& pointIdx_arg) const;

        int
        getOccupiedVoxelCenters (AlignedPointTVector &voxelCenterList_arg) const;


        int
        getApproxIntersectedVoxelCentersBySegment (const Eigen::Vector3f& origin,
                                                   const Eigen::Vector3f& end,
                                                   AlignedPointTVector &voxel_center_list,
                                                   float precision = 0.2);

        void
        deleteVoxelAtPoint (const PointT& point_arg);

        void
        deleteVoxelAtPoint (const int& pointIdx_arg);

        // Bounding box methods


        void
        defineBoundingBox ();

        void
        defineBoundingBox (const double minX_arg, const double minY_arg, const double minZ_arg, const double maxX_arg,
                           const double maxY_arg, const double maxZ_arg);

        void
        defineBoundingBox (const double maxX_arg, const double maxY_arg, const double maxZ_arg);

        void
        defineBoundingBox (const double cubeLen_arg);

        void
        getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, double& maxX_arg, double& maxY_arg,
                        double& maxZ_arg) const ;

        double
        getVoxelSquaredDiameter (unsigned int treeDepth_arg) const ;

        inline double
        getVoxelSquaredDiameter () const
        {
          return getVoxelSquaredDiameter (this->octreeDepth_);
        }

        double
        getVoxelSquaredSideLen (unsigned int treeDepth_arg) const ;

        inline double
        getVoxelSquaredSideLen () const
        {
          return getVoxelSquaredSideLen (this->octreeDepth_);
        }


        inline void
        getVoxelBounds (OctreeIteratorBase<int,LeafT,OctreeT>& iterator, Eigen::Vector3f &min_pt, Eigen::Vector3f &max_pt)
        {
          this->genVoxelBoundsFromOctreeKey (iterator.getCurrentOctreeKey(), iterator.getCurrentOctreeDepth(), min_pt, max_pt);
        }


      protected:

        void
        addPointIdx (const int pointIdx_arg);

        const PointT&
        getPointByIndex (const unsigned int index_arg) const;

        LeafT*
        findLeafAtPoint (const PointT& point_arg) const ;

        // Protected octree methods based on octree keys

        typedef typename OctreeT::OctreeKey OctreeKey;
        typedef typename OctreeT::OctreeBranch OctreeBranch;

        void
        getKeyBitSize ();

        void
        adoptBoundingBoxToPoint (const PointT& pointIdx_arg);

        void
        genOctreeKeyforPoint (const PointT & point_arg, OctreeKey &key_arg) const ;

        void
        genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg,
                              OctreeKey & key_arg) const;

        virtual bool
        genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const;

        void
        genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg, PointT& point_arg) const ;

        void
        genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, PointT& point_arg) const ;

        void
        genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, Eigen::Vector3f &min_pt,
                                     Eigen::Vector3f &max_pt) const;

        int
        getOccupiedVoxelCentersRecursive (const OctreeBranch* node_arg,
                                          const OctreeKey& key_arg,
                                          std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const;

        // Globals

        PointCloudConstPtr input_;

        IndicesConstPtr indices_;

        double epsilon_;

        double resolution_;

        // Octree bounding box coordinates
        double minX_;
        double maxX_;

        double minY_;
        double maxY_;

        double minZ_;
        double maxZ_;

        unsigned int maxKeys_;

        bool boundingBoxDefined_;
    };
  }
}

#endif