Vector<uint64_t,3> count_block_wins(const section_t section, const Vector<uint8_t,4> block, RawArray<const Vector<super_t,2>> flat_data) {
// Prepare
const auto base = block_size*Vector<int,4>(block);
const auto shape = block_shape(section.shape(),block);
const auto rmin0 = safe_rmin_slice(section.counts[0],base[0]+range(shape[0])),
rmin1 = safe_rmin_slice(section.counts[1],base[1]+range(shape[1])),
rmin2 = safe_rmin_slice(section.counts[2],base[2]+range(shape[2])),
rmin3 = safe_rmin_slice(section.counts[3],base[3]+range(shape[3]));
GEODE_ASSERT(shape.product()==flat_data.size());
const RawArray<const Vector<super_t,2>,4> block_data(shape,flat_data.data());
// Count
Vector<uint64_t,3> counts;
for (int i0=0;i0<shape[0];i0++)
for (int i1=0;i1<shape[1];i1++)
for (int i2=0;i2<shape[2];i2++)
for (int i3=0;i3<shape[3];i3++)
counts += Vector<uint64_t,3>(popcounts_over_stabilizers(quadrants(rmin0[i0],rmin1[i1],rmin2[i2],rmin3[i3]),block_data(i0,i1,i2,i3)));
return counts;
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut & output,
std::vector<pcl::PointIndices> & cluster_indices)
{
PointCloudOut ourcvfh_output;
for (size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
{
std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
PointInTPtr grid (new pcl::PointCloud<PointInT>);
sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
for (size_t t = 0; t < transformations.size (); t++)
{
pcl::transformPointCloud (*processed, *grid, transformations[t]);
transforms_.push_back (transformations[t]);
valid_transforms_.push_back (true);
std::vector < Eigen::VectorXf > quadrants (8);
int size_hists = 13;
int num_hists = 8;
for (int k = 0; k < num_hists; k++)
quadrants[k].setZero (size_hists);
Eigen::Vector4f centroid_p;
centroid_p.setZero ();
Eigen::Vector4f max_pt;
pcl::getMaxDistance (*grid, centroid_p, max_pt);
max_pt[3] = 0;
double distance_normalization_factor = (centroid_p - max_pt).norm ();
float hist_incr;
if (normalize_bins_)
hist_incr = 100.0f / static_cast<float> (grid->points.size () - 1);
else
hist_incr = 1.0f;
float * weights = new float[num_hists];
float sigma = 0.01f; //1cm
float sigma_sq = sigma * sigma;
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector4f p = grid->points[k].getVector4fMap ();
p[3] = 0.f;
float d = p.norm ();
//compute weight for all octants
float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes
float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
//distribute the weights using the x-coordinate
if (p[0] >= 0)
{
for (size_t ii = 0; ii <= 3; ii++)
weights[ii] = 0.5f - wx * 0.5f;
for (size_t ii = 4; ii <= 7; ii++)
weights[ii] = 0.5f + wx * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 3; ii++)
weights[ii] = 0.5f + wx * 0.5f;
for (size_t ii = 4; ii <= 7; ii++)
weights[ii] = 0.5f - wx * 0.5f;
}
//distribute the weights using the y-coordinate
if (p[1] >= 0)
{
for (size_t ii = 0; ii <= 1; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 4; ii <= 5; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 2; ii <= 3; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 6; ii <= 7; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 1; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 4; ii <= 5; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 2; ii <= 3; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 6; ii <= 7; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
}
//distribute the weights using the z-coordinate
if (p[2] >= 0)
{
for (size_t ii = 0; ii <= 7; ii += 2)
weights[ii] *= 0.5f - wz * 0.5f;
for (size_t ii = 1; ii <= 7; ii += 2)
weights[ii] *= 0.5f + wz * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 7; ii += 2)
weights[ii] *= 0.5f + wz * 0.5f;
for (size_t ii = 1; ii <= 7; ii += 2)
weights[ii] *= 0.5f - wz * 0.5f;
}
int h_index = static_cast<int> (std::floor (size_hists * (d / distance_normalization_factor)));
for (int j = 0; j < num_hists; j++)
quadrants[j][h_index] += hist_incr * weights[j];
}
//copy to the cvfh signature
PointCloudOut vfh_signature;
vfh_signature.points.resize (1);
vfh_signature.width = vfh_signature.height = 1;
for (int d = 0; d < 308; ++d)
vfh_signature.points[0].histogram[d] = output.points[i].histogram[d];
int pos = 45 * 3;
for (int k = 0; k < num_hists; k++)
{
for (int ii = 0; ii < size_hists; ii++, pos++)
{
vfh_signature.points[0].histogram[pos] = quadrants[k][ii];
}
}
ourcvfh_output.points.push_back (vfh_signature.points[0]);
delete[] weights;
}
}
output = ourcvfh_output;
}
