38 #ifndef PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ 39 #define PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ 41 #include <pcl/common/io.h> 42 #include <pcl/filters/approximate_voxel_grid.h> 45 template <
typename Po
intT>
void 48 hhe->centroid /=
static_cast<float> (hhe->count);
54 float r = hhe->centroid[centroid_size-3],
55 g = hhe->centroid[centroid_size-2],
56 b = hhe->centroid[centroid_size-1];
57 int rgb = (
static_cast<int> (r)) << 16 | (
static_cast<int> (g)) << 8 | (
static_cast<int> (b));
58 memcpy (reinterpret_cast<char*> (&output[op]) + rgba_index, &rgb,
sizeof (
float));
63 template <
typename Po
intT>
void 66 int centroid_size = 4;
67 if (downsample_all_data_)
68 centroid_size = boost::mpl::size<FieldList>::value;
71 std::vector<pcl::PCLPointField> fields;
73 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
75 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
78 rgba_index = fields[rgba_index].offset;
82 for (std::size_t i = 0; i < histsize_; i++)
84 history_[i].count = 0;
85 history_[i].centroid = Eigen::VectorXf::Zero (centroid_size);
87 Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size);
89 output.
points.resize (input_->size ());
91 for (
const auto& point: *input_)
93 int ix =
static_cast<int> (std::floor (point.x * inverse_leaf_size_[0]));
94 int iy =
static_cast<int> (std::floor (point.y * inverse_leaf_size_[1]));
95 int iz =
static_cast<int> (std::floor (point.z * inverse_leaf_size_[2]));
96 unsigned int hash =
static_cast<unsigned int> ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1));
97 he *hhe = &history_[hash];
98 if (hhe->count && ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz)))
100 flush (output, op++, hhe, rgba_index, centroid_size);
102 hhe->centroid.setZero ();
115 memcpy (&rgb, (reinterpret_cast<const char *> (&point)) + rgba_index,
sizeof (
RGB));
116 scratch[centroid_size-3] = rgb.r;
117 scratch[centroid_size-2] = rgb.g;
118 scratch[centroid_size-1] = rgb.b;
121 hhe->centroid += scratch;
123 for (std::size_t i = 0; i < histsize_; i++)
125 he *hhe = &history_[i];
127 flush (output, op++, hhe, rgba_index, centroid_size);
129 output.
points.resize (op);
135 #define PCL_INSTANTIATE_ApproximateVoxelGrid(T) template class PCL_EXPORTS pcl::ApproximateVoxelGrid<T>; 137 #endif // PCL_FILTERS_IMPL_FAST_VOXEL_GRID_H_ std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void flush(PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size)
Write a single point from the hash to the output cloud.
std::uint32_t width
The point cloud width (if organized as an image-structure).
A structure representing RGB color information.
std::uint32_t height
The point cloud height (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.