Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/keypoints/include/pcl/keypoints/uniform_sampling.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  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
00041 #define PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
00042 
00043 #include <pcl/keypoints/keypoint.h>
00044 #include <boost/unordered_map.hpp>
00045 
00046 namespace pcl
00047 {
00048   /** \brief @b UniformSampling assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
00049     *
00050     * The @b UniformSampling class creates a *3D voxel grid* (think about a voxel
00051     * grid as a set of tiny 3D boxes in space) over the input point cloud data.
00052     * Then, in each *voxel* (i.e., 3D box), all the points present will be
00053     * approximated (i.e., *downsampled*) with their centroid. This approach is
00054     * a bit slower than approximating them with the center of the voxel, but it
00055     * represents the underlying surface more accurately.
00056     *
00057     * \author Radu Bogdan Rusu
00058     * \ingroup keypoints
00059     */
00060   template <typename PointInT>
00061   class UniformSampling: public Keypoint<PointInT, int>
00062   {
00063     typedef typename Keypoint<PointInT, int>::PointCloudIn PointCloudIn;
00064     typedef typename Keypoint<PointInT, int>::PointCloudOut PointCloudOut;
00065 
00066     using Keypoint<PointInT, int>::name_;
00067     using Keypoint<PointInT, int>::input_;
00068     using Keypoint<PointInT, int>::indices_;
00069     using Keypoint<PointInT, int>::search_radius_;
00070     using Keypoint<PointInT, int>::getClassName;
00071 
00072     public:
00073       typedef boost::shared_ptr<UniformSampling<PointInT> > Ptr;
00074       typedef boost::shared_ptr<const UniformSampling<PointInT> > ConstPtr;
00075 
00076       /** \brief Empty constructor. */
00077       UniformSampling () :
00078         leaves_ (),
00079         leaf_size_ (Eigen::Vector4f::Zero ()),
00080         inverse_leaf_size_ (Eigen::Vector4f::Zero ()),
00081         min_b_ (Eigen::Vector4i::Zero ()),
00082         max_b_ (Eigen::Vector4i::Zero ()),
00083         div_b_ (Eigen::Vector4i::Zero ()),
00084         divb_mul_ (Eigen::Vector4i::Zero ())
00085       {
00086         name_ = "UniformSampling";
00087       }
00088 
00089       /** \brief Destructor. */
00090       virtual ~UniformSampling ()
00091       {
00092         leaves_.clear();
00093       }
00094 
00095       /** \brief Set the 3D grid leaf size.
00096         * \param radius the 3D grid leaf size
00097         */
00098       virtual inline void 
00099       setRadiusSearch (double radius) 
00100       { 
00101         leaf_size_[0] = leaf_size_[1] = leaf_size_[2] = static_cast<float> (radius);
00102         // Avoid division errors
00103         if (leaf_size_[3] == 0)
00104           leaf_size_[3] = 1;
00105         // Use multiplications instead of divisions
00106         inverse_leaf_size_ = Eigen::Array4f::Ones () / leaf_size_.array ();
00107         search_radius_ = radius;
00108       }
00109 
00110     protected:
00111       /** \brief Simple structure to hold an nD centroid and the number of points in a leaf. */
00112       struct Leaf
00113       {
00114         Leaf () : idx (-1) { }
00115         int idx;
00116       };
00117 
00118       /** \brief The 3D grid leaves. */
00119       boost::unordered_map<size_t, Leaf> leaves_;
00120 
00121       /** \brief The size of a leaf. */
00122       Eigen::Vector4f leaf_size_;
00123 
00124       /** \brief Internal leaf sizes stored as 1/leaf_size_ for efficiency reasons. */ 
00125       Eigen::Array4f inverse_leaf_size_;
00126 
00127       /** \brief The minimum and maximum bin coordinates, the number of divisions, and the division multiplier. */
00128       Eigen::Vector4i min_b_, max_b_, div_b_, divb_mul_;
00129 
00130       /** \brief Downsample a Point Cloud using a voxelized grid approach
00131         * \param output the resultant point cloud message
00132         */
00133       void 
00134       detectKeypoints (PointCloudOut &output);
00135   };
00136 }
00137 
00138 #ifdef PCL_NO_PRECOMPILE
00139 #include <pcl/keypoints/impl/uniform_sampling.hpp>
00140 #endif
00141 
00142 #endif  //#ifndef PCL_KEYPOINTS_UNIFORM_SAMPLING_H_
00143