Point Cloud Library (PCL)
1.3.1
|
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_