49 hhe->centroid /=
static_cast<float> (hhe->count);
50 pcl::for_each_type <FieldList> (pcl::xNdCopyEigenPointFunctor <PointT> (hhe->centroid, output[op]));
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[op]) + rgba_index, &rgb,
sizeof (
float));
67 int centroid_size = 4;
68 if (downsample_all_data_)
69 centroid_size = boost::mpl::size<FieldList>::value;
72 std::vector<pcl::PCLPointField> fields;
74 rgba_index = pcl::getFieldIndex<PointT> (
"rgb", fields);
76 rgba_index = pcl::getFieldIndex<PointT> (
"rgba", fields);
79 rgba_index = fields[rgba_index].offset;
83 for (std::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.resize (input_->size ());
92 for (
const auto& point: *input_)
94 int ix =
static_cast<int> (std::floor (point.x * inverse_leaf_size_[0]));
95 int iy =
static_cast<int> (std::floor (point.y * inverse_leaf_size_[1]));
96 int iz =
static_cast<int> (std::floor (point.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 *
> (&point)) + 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;
121 pcl::for_each_type <FieldList> (xNdCopyPointEigenFunctor <PointT> (point, scratch));
122 hhe->centroid += scratch;
124 for (std::size_t i = 0; i < histsize_; i++)
126 he *hhe = &history_[i];
128 flush (output, op++, hhe, rgba_index, centroid_size);
131 output.width = output.size ();
133 output.is_dense =
false;
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.