Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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 {
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
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
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
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
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:
00122 ApproximateVoxelGrid () :
00123 pcl::Filter<PointT> (),
00124 leaf_size_ (Eigen::Vector3f::Ones ()),
00125 inverse_leaf_size_ (Eigen::Array3f::Ones ()),
00126 downsample_all_data_ (true), histsize_ (512),
00127 history_ (new he[histsize_])
00128 {
00129 filter_name_ = "ApproximateVoxelGrid";
00130 }
00131
00135 ApproximateVoxelGrid (const ApproximateVoxelGrid &src) :
00136 pcl::Filter<PointT> (),
00137 leaf_size_ (src.leaf_size_),
00138 inverse_leaf_size_ (src.inverse_leaf_size_),
00139 downsample_all_data_ (src.downsample_all_data_),
00140 histsize_ (src.histsize_),
00141 history_ ()
00142 {
00143 history_ = new he[histsize_];
00144 for (size_t i = 0; i < histsize_; i++)
00145 history_[i] = src.history_[i];
00146 }
00147
00151 inline ApproximateVoxelGrid&
00152 operator = (const ApproximateVoxelGrid &src)
00153 {
00154 leaf_size_ = src.leaf_size_;
00155 inverse_leaf_size_ = src.inverse_leaf_size_;
00156 downsample_all_data_ = src.downsample_all_data_;
00157 histsize_ = src.histsize_;
00158 history_ = new he[histsize_];
00159 for (size_t i = 0; i < histsize_; i++)
00160 history_[i] = src.history_[i];
00161 return (*this);
00162 }
00163
00167 inline void
00168 setLeafSize (const Eigen::Vector3f &leaf_size)
00169 {
00170 leaf_size_ = leaf_size;
00171 inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array ();
00172 }
00173
00179 inline void
00180 setLeafSize (float lx, float ly, float lz)
00181 {
00182 setLeafSize (Eigen::Vector3f (lx, ly, lz));
00183 }
00184
00186 inline Eigen::Vector3f
00187 getLeafSize () const { return (leaf_size_); }
00188
00192 inline void
00193 setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
00194
00198 inline bool
00199 getDownsampleAllData () const { return (downsample_all_data_); }
00200
00201 protected:
00203 Eigen::Vector3f leaf_size_;
00204
00206 Eigen::Array3f inverse_leaf_size_;
00207
00209 bool downsample_all_data_;
00210
00212 size_t histsize_;
00213
00215 struct he* history_;
00216
00217 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00218
00222 void
00223 applyFilter (PointCloud &output);
00224
00227 void
00228 flush(PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size);
00229 };
00230 }
00231
00232 #endif //#ifndef PCL_FILTERS_VOXEL_GRID_MAP_H_