Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/octree/include/pcl/octree/octree_pointcloud_voxelcentroid.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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_OCTREE_VOXELCENTROID_H
00041 #define PCL_OCTREE_VOXELCENTROID_H
00042 
00043 #include "octree_pointcloud.h"
00044 
00045 #include <pcl/common/point_operators.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/register_point_struct.h>
00048 
00049 namespace pcl
00050 {
00051   namespace octree
00052   {
00053     /** \brief @b Octree pointcloud voxel centroid leaf node class
00054       * \note This class implements a leaf node that calculates the mean centroid of all points added this octree container.
00055       * \author Julius Kammerl (julius@kammerl.de)
00056       */
00057     template<typename PointT>
00058     class OctreePointCloudVoxelCentroidContainer : public OctreeContainerBase
00059     {
00060       public:
00061         /** \brief Class initialization. */
00062         OctreePointCloudVoxelCentroidContainer ()
00063         {
00064           this->reset();
00065         }
00066 
00067         /** \brief Empty class deconstructor. */
00068         virtual ~OctreePointCloudVoxelCentroidContainer ()
00069         {
00070         }
00071 
00072         /** \brief deep copy function */
00073         virtual OctreePointCloudVoxelCentroidContainer *
00074         deepCopy () const
00075         {
00076           return (new OctreePointCloudVoxelCentroidContainer (*this));
00077         }
00078 
00079         /** \brief Equal comparison operator - set to false
00080          * \param[in] OctreePointCloudVoxelCentroidContainer to compare with
00081          */
00082         virtual bool operator==(const OctreeContainerBase&) const
00083         {
00084           return ( false );
00085         }
00086 
00087         /** \brief Add new point to voxel.
00088           * \param[in] new_point the new point to add  
00089           */
00090         void 
00091         addPoint (const PointT& new_point)
00092         {
00093           using namespace pcl::common;
00094 
00095           ++point_counter_;
00096 
00097           point_sum_ += new_point;
00098         }
00099 
00100         /** \brief Calculate centroid of voxel.
00101           * \param[out] centroid_arg the resultant centroid of the voxel 
00102           */
00103         void 
00104         getCentroid (PointT& centroid_arg) const
00105         {
00106           using namespace pcl::common;
00107 
00108           if (point_counter_)
00109           {
00110             centroid_arg = point_sum_;
00111             centroid_arg /= static_cast<float> (point_counter_);
00112           }
00113           else
00114           {
00115             centroid_arg *= 0.0f;
00116           }
00117         }
00118 
00119         /** \brief Reset leaf container. */
00120         virtual void 
00121         reset ()
00122         {
00123           using namespace pcl::common;
00124 
00125           point_counter_ = 0;
00126           point_sum_ *= 0.0f;
00127         }
00128 
00129       private:
00130         unsigned int point_counter_;
00131         PointT point_sum_;
00132     };
00133 
00134     /** \brief @b Octree pointcloud voxel centroid class
00135       * \note This class generate an octrees from a point cloud (zero-copy). It provides a vector of centroids for all occupied voxels.
00136       * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
00137       * \note
00138       * \note typename: PointT: type of point used in pointcloud
00139       *
00140       * \ingroup octree
00141       * \author Julius Kammerl (julius@kammerl.de)
00142       */
00143     template<typename PointT,
00144              typename LeafContainerT = OctreePointCloudVoxelCentroidContainer<PointT> ,
00145              typename BranchContainerT = OctreeContainerEmpty >
00146     class OctreePointCloudVoxelCentroid : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
00147     {
00148       public:
00149         typedef boost::shared_ptr<OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > Ptr;
00150         typedef boost::shared_ptr<const OctreePointCloudVoxelCentroid<PointT, LeafContainerT> > ConstPtr;
00151 
00152         typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT> OctreeT;
00153         typedef typename OctreeT::LeafNode LeafNode;
00154         typedef typename OctreeT::BranchNode BranchNode;
00155 
00156         /** \brief OctreePointCloudVoxelCentroids class constructor.
00157           * \param[in] resolution_arg octree resolution at lowest octree level
00158           */
00159         OctreePointCloudVoxelCentroid (const double resolution_arg) :
00160           OctreePointCloud<PointT, LeafContainerT, BranchContainerT> (resolution_arg)
00161         {
00162         }
00163 
00164         /** \brief Empty class deconstructor. */
00165         virtual
00166         ~OctreePointCloudVoxelCentroid ()
00167         {
00168         }
00169 
00170         /** \brief Add DataT object to leaf node at octree key.
00171           * \param[in] key_arg octree key addressing a leaf node.
00172           * \param[in] data_arg DataT object to be added.
00173           */
00174         virtual void 
00175         addPointIdx (const int pointIdx_arg)
00176         {
00177           OctreeKey key;
00178 
00179           assert (pointIdx_arg < static_cast<int> (this->input_->points.size ()));
00180 
00181           const PointT& point = this->input_->points[pointIdx_arg];
00182 
00183           // make sure bounding box is big enough
00184           this->adoptBoundingBoxToPoint (point);
00185 
00186           // generate key
00187           this->genOctreeKeyforPoint (point, key);
00188 
00189           // add point to octree at key
00190           LeafContainerT* container = this->createLeaf(key);
00191           container->addPoint (point);
00192 
00193         }
00194 
00195         /** \brief Get centroid for a single voxel addressed by a PointT point.
00196           * \param[in] point_arg point addressing a voxel in octree
00197           * \param[out] voxel_centroid_arg centroid is written to this PointT reference
00198           * \return "true" if voxel is found; "false" otherwise
00199           */
00200         bool
00201         getVoxelCentroidAtPoint (const PointT& point_arg, PointT& voxel_centroid_arg) const;
00202 
00203         /** \brief Get centroid for a single voxel addressed by a PointT point from input cloud.
00204           * \param[in] point_idx_arg point index from input cloud addressing a voxel in octree
00205           * \param[out] voxel_centroid_arg centroid is written to this PointT reference
00206           * \return "true" if voxel is found; "false" otherwise
00207           */
00208         inline bool
00209         getVoxelCentroidAtPoint (const int& point_idx_arg, PointT& voxel_centroid_arg) const
00210         {
00211           // get centroid at point
00212           return (this->getVoxelCentroidAtPoint (this->input_->points[point_idx_arg], voxel_centroid_arg));
00213         }
00214 
00215         /** \brief Get PointT vector of centroids for all occupied voxels.
00216           * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
00217           * \return number of occupied voxels
00218           */
00219         size_t
00220         getVoxelCentroids (typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const;
00221 
00222         /** \brief Recursively explore the octree and output a PointT vector of centroids for all occupied voxels.
00223           * \param[in] branch_arg: current branch node
00224           * \param[in] key_arg: current key
00225           * \param[out] voxel_centroid_list_arg results are written to this vector of PointT elements
00226           */
00227         void
00228         getVoxelCentroidsRecursive (const BranchNode* branch_arg, 
00229                                     OctreeKey& key_arg, 
00230                                     typename OctreePointCloud<PointT, LeafContainerT, BranchContainerT>::AlignedPointTVector &voxel_centroid_list_arg) const;
00231 
00232     };
00233   }
00234 }
00235 
00236 #include <pcl/octree/impl/octree_pointcloud_voxelcentroid.hpp>
00237 
00238 #endif
00239