Point Cloud Library (PCL)  1.3.1
approximate_voxel_grid.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: approximate_voxel_grid.h 3028 2011-11-01 04:12:17Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FILTERS_APPROXIMATE_VOXEL_GRID_MAP_H_
00039 #define PCL_FILTERS_APPROXIMATE_VOXEL_GRID_MAP_H_
00040 
00041 #include "pcl/filters/filter.h"
00042 #include <boost/mpl/size.hpp>
00043 
00044 namespace pcl
00045 {
00046   struct he {
00047     int ix, iy, iz;
00048     int count;
00049     Eigen::VectorXf centroid;
00050   };
00051 
00053   template <typename PointT>
00054   struct xNdCopyEigenPointFunctor
00055   {
00056     typedef typename traits::POD<PointT>::type Pod;
00057     
00058     xNdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2)
00059       : p1_ (p1),
00060         p2_ (reinterpret_cast<Pod&>(p2)),
00061         f_idx_ (0) { }
00062 
00063     template<typename Key> inline void operator() ()
00064     {
00065       //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
00066       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00067       uint8_t* data_ptr = reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointT, Key>::value;
00068       *reinterpret_cast<T*>(data_ptr) = p1_[f_idx_++];
00069     }
00070 
00071     private:
00072       const Eigen::VectorXf &p1_;
00073       Pod &p2_;
00074       int f_idx_;
00075   };
00076 
00078   template <typename PointT>
00079   struct xNdCopyPointEigenFunctor
00080   {
00081     typedef typename traits::POD<PointT>::type Pod;
00082     
00083     xNdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2)
00084       : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { }
00085 
00086     template<typename Key> inline void operator() ()
00087     {
00088       //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
00089       typedef typename pcl::traits::datatype<PointT, Key>::type T;
00090       const uint8_t* data_ptr = reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointT, Key>::value;
00091       p2_[f_idx_++] = *reinterpret_cast<const T*>(data_ptr);
00092     }
00093 
00094     private:
00095       const Pod &p1_;
00096       Eigen::VectorXf &p2_;
00097       int f_idx_;
00098   };
00099 
00112   template <typename PointT>
00113   class ApproximateVoxelGrid: public Filter<PointT>
00114   {
00115     using Filter<PointT>::filter_name_;
00116     using Filter<PointT>::getClassName;
00117     using Filter<PointT>::input_;
00118     using Filter<PointT>::indices_;
00119     using Filter<PointT>::filter_limit_negative_;
00120     using Filter<PointT>::filter_limit_min_;
00121     using Filter<PointT>::filter_limit_max_;
00122     using Filter<PointT>::filter_field_name_;
00123 
00124     typedef typename Filter<PointT>::PointCloud PointCloud;
00125     typedef typename PointCloud::Ptr PointCloudPtr;
00126     typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00127 
00128     public:
00130       ApproximateVoxelGrid () : downsample_all_data_ (true), histsize(512)
00131       {
00132         setLeafSize(1, 1, 1);
00133         filter_name_ = "ApproximateVoxelGrid";
00134         history = new he[histsize];
00135       }
00136 
00138       virtual ~ApproximateVoxelGrid ()
00139       {
00140       }
00141 
00145       inline void 
00146       setLeafSize (const Eigen::Vector3f &leaf_size) 
00147       { 
00148         leaf_size_ = leaf_size; 
00149         inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array ();
00150       }
00151 
00157       inline void
00158       setLeafSize (float lx, float ly, float lz)
00159       {
00160         setLeafSize(Eigen::Vector3f(lx, ly, lz));
00161       }
00162 
00164       inline Eigen::Vector3f 
00165       getLeafSize () { return leaf_size_; }
00166 
00170       inline void 
00171       setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
00172 
00176       inline bool 
00177       getDownsampleAllData () { return (downsample_all_data_); }
00178 
00179     protected:
00181       Eigen::Vector3f leaf_size_;
00182 
00184       Eigen::Array3f inverse_leaf_size_;
00185 
00187       bool downsample_all_data_;
00188 
00190       size_t histsize;
00191 
00193       struct he *history;
00194 
00195       typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00196 
00200       void 
00201       applyFilter (PointCloud &output);
00202 
00205       void flush(PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size);
00206   };
00207 }
00208 
00209 #endif  //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines