38 #ifndef PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
39 #define PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_
41 #include <pcl/common/common.h>
42 #include <pcl/common/io.h>
43 #include <pcl/filters/approximate_voxel_grid.h>
46 template <
typename Po
intT>
void
49 hhe->centroid /=
static_cast<float> (hhe->count);
55 float r = hhe->centroid[centroid_size-3],
56 g = hhe->centroid[centroid_size-2],
57 b = hhe->centroid[centroid_size-1];
58 int rgb = (
static_cast<int> (r)) << 16 | (
static_cast<int> (g)) << 8 | (
static_cast<int> (b));
59 memcpy (reinterpret_cast<char*> (&output.
points[op]) + rgba_index, &rgb, sizeof (
float));
64 template <
typename Po
intT>
void
67 int centroid_size = 4;
68 if (downsample_all_data_)
69 centroid_size = boost::mpl::size<FieldList>::value;
72 std::vector<pcl::PCLPointField> fields;
79 rgba_index = fields[rgba_index].offset;
83 for (
size_t i = 0; i < histsize_; i++)
85 history_[i].count = 0;
86 history_[i].centroid = Eigen::VectorXf::Zero (centroid_size);
88 Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size);
90 output.
points.resize (input_->points.size ());
92 for (
size_t cp = 0; cp < input_->points.size (); ++cp)
94 int ix =
static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]));
95 int iy =
static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]));
96 int iz =
static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]));
97 unsigned int hash =
static_cast<unsigned int> ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1));
98 he *hhe = &history_[hash];
99 if (hhe->count && ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz)))
101 flush (output, op++, hhe, rgba_index, centroid_size);
103 hhe->centroid.setZero ();
116 memcpy (&rgb, (reinterpret_cast<const char *> (&input_->points[cp])) + rgba_index, sizeof (
RGB));
117 scratch[centroid_size-3] = rgb.r;
118 scratch[centroid_size-2] = rgb.g;
119 scratch[centroid_size-1] = rgb.b;
122 hhe->centroid += scratch;
124 for (
size_t i = 0; i < histsize_; i++)
126 he *hhe = &history_[i];
128 flush (output, op++, hhe, rgba_index, centroid_size);
130 output.
points.resize (op);
131 output.
width =
static_cast<uint32_t
> (output.
points.size ());
136 #define PCL_INSTANTIATE_ApproximateVoxelGrid(T) template class PCL_EXPORTS pcl::ApproximateVoxelGrid<T>;
138 #endif // PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_