Point Cloud Library (PCL)
1.7.0
|
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 the copyright holder(s) 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: voxel_grid.h 1374 2011-06-19 02:29:56Z bouffa $ 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/boost.h> 00042 #include <pcl/filters/filter.h> 00043 00044 namespace pcl 00045 { 00046 /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */ 00047 template <typename PointT> 00048 struct xNdCopyEigenPointFunctor 00049 { 00050 typedef typename traits::POD<PointT>::type Pod; 00051 00052 xNdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2) 00053 : p1_ (p1), 00054 p2_ (reinterpret_cast<Pod&>(p2)), 00055 f_idx_ (0) { } 00056 00057 template<typename Key> inline void operator() () 00058 { 00059 //boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++]; 00060 typedef typename pcl::traits::datatype<PointT, Key>::type T; 00061 uint8_t* data_ptr = reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointT, Key>::value; 00062 *reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]); 00063 } 00064 00065 private: 00066 const Eigen::VectorXf &p1_; 00067 Pod &p2_; 00068 int f_idx_; 00069 }; 00070 00071 /** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */ 00072 template <typename PointT> 00073 struct xNdCopyPointEigenFunctor 00074 { 00075 typedef typename traits::POD<PointT>::type Pod; 00076 00077 xNdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2) 00078 : p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2), f_idx_ (0) { } 00079 00080 template<typename Key> inline void operator() () 00081 { 00082 //p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_); 00083 typedef typename pcl::traits::datatype<PointT, Key>::type T; 00084 const uint8_t* data_ptr = reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointT, Key>::value; 00085 p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr)); 00086 } 00087 00088 private: 00089 const Pod &p1_; 00090 Eigen::VectorXf &p2_; 00091 int f_idx_; 00092 }; 00093 00094 /** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. 00095 * 00096 * \author James Bowman, Radu B. Rusu 00097 * \ingroup filters 00098 */ 00099 template <typename PointT> 00100 class ApproximateVoxelGrid: public Filter<PointT> 00101 { 00102 using Filter<PointT>::filter_name_; 00103 using Filter<PointT>::getClassName; 00104 using Filter<PointT>::input_; 00105 using Filter<PointT>::indices_; 00106 00107 typedef typename Filter<PointT>::PointCloud PointCloud; 00108 typedef typename PointCloud::Ptr PointCloudPtr; 00109 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00110 00111 private: 00112 struct he 00113 { 00114 he () : ix (), iy (), iz (), count (0), centroid () {} 00115 int ix, iy, iz; 00116 int count; 00117 Eigen::VectorXf centroid; 00118 }; 00119 00120 public: 00121 00122 typedef boost::shared_ptr< ApproximateVoxelGrid<PointT> > Ptr; 00123 typedef boost::shared_ptr< const ApproximateVoxelGrid<PointT> > ConstPtr; 00124 00125 00126 /** \brief Empty constructor. */ 00127 ApproximateVoxelGrid () : 00128 pcl::Filter<PointT> (), 00129 leaf_size_ (Eigen::Vector3f::Ones ()), 00130 inverse_leaf_size_ (Eigen::Array3f::Ones ()), 00131 downsample_all_data_ (true), histsize_ (512), 00132 history_ (new he[histsize_]) 00133 { 00134 filter_name_ = "ApproximateVoxelGrid"; 00135 } 00136 00137 /** \brief Copy constructor. 00138 * \param[in] src the approximate voxel grid to copy into this. 00139 */ 00140 ApproximateVoxelGrid (const ApproximateVoxelGrid &src) : 00141 pcl::Filter<PointT> (), 00142 leaf_size_ (src.leaf_size_), 00143 inverse_leaf_size_ (src.inverse_leaf_size_), 00144 downsample_all_data_ (src.downsample_all_data_), 00145 histsize_ (src.histsize_), 00146 history_ () 00147 { 00148 history_ = new he[histsize_]; 00149 for (size_t i = 0; i < histsize_; i++) 00150 history_[i] = src.history_[i]; 00151 } 00152 00153 00154 /** \brief Destructor. 00155 */ 00156 ~ApproximateVoxelGrid () 00157 { 00158 delete [] history_; 00159 } 00160 00161 00162 /** \brief Copy operator. 00163 * \param[in] src the approximate voxel grid to copy into this. 00164 */ 00165 inline ApproximateVoxelGrid& 00166 operator = (const ApproximateVoxelGrid &src) 00167 { 00168 leaf_size_ = src.leaf_size_; 00169 inverse_leaf_size_ = src.inverse_leaf_size_; 00170 downsample_all_data_ = src.downsample_all_data_; 00171 histsize_ = src.histsize_; 00172 history_ = new he[histsize_]; 00173 for (size_t i = 0; i < histsize_; i++) 00174 history_[i] = src.history_[i]; 00175 return (*this); 00176 } 00177 00178 /** \brief Set the voxel grid leaf size. 00179 * \param[in] leaf_size the voxel grid leaf size 00180 */ 00181 inline void 00182 setLeafSize (const Eigen::Vector3f &leaf_size) 00183 { 00184 leaf_size_ = leaf_size; 00185 inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array (); 00186 } 00187 00188 /** \brief Set the voxel grid leaf size. 00189 * \param[in] lx the leaf size for X 00190 * \param[in] ly the leaf size for Y 00191 * \param[in] lz the leaf size for Z 00192 */ 00193 inline void 00194 setLeafSize (float lx, float ly, float lz) 00195 { 00196 setLeafSize (Eigen::Vector3f (lx, ly, lz)); 00197 } 00198 00199 /** \brief Get the voxel grid leaf size. */ 00200 inline Eigen::Vector3f 00201 getLeafSize () const { return (leaf_size_); } 00202 00203 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. 00204 * \param downsample the new value (true/false) 00205 */ 00206 inline void 00207 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 00208 00209 /** \brief Get the state of the internal downsampling parameter (true if 00210 * all fields need to be downsampled, false if just XYZ). 00211 */ 00212 inline bool 00213 getDownsampleAllData () const { return (downsample_all_data_); } 00214 00215 protected: 00216 /** \brief The size of a leaf. */ 00217 Eigen::Vector3f leaf_size_; 00218 00219 /** \brief Compute 1/leaf_size_ to avoid division later */ 00220 Eigen::Array3f inverse_leaf_size_; 00221 00222 /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ 00223 bool downsample_all_data_; 00224 00225 /** \brief history buffer size, power of 2 */ 00226 size_t histsize_; 00227 00228 /** \brief history buffer */ 00229 struct he* history_; 00230 00231 typedef typename pcl::traits::fieldList<PointT>::type FieldList; 00232 00233 /** \brief Downsample a Point Cloud using a voxelized grid approach 00234 * \param output the resultant point cloud message 00235 */ 00236 void 00237 applyFilter (PointCloud &output); 00238 00239 /** \brief Write a single point from the hash to the output cloud 00240 */ 00241 void 00242 flush (PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size); 00243 }; 00244 } 00245 00246 #ifdef PCL_NO_PRECOMPILE 00247 #include <pcl/filters/impl/approximate_voxel_grid.hpp> 00248 #endif 00249 00250 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_