std::shared_ptr<Feature> ComputeFPFHFeature(const PointCloud &input,
const KDTreeSearchParam &search_param/* = KDTreeSearchParamKNN()*/)
{
auto feature = std::make_shared<Feature>();
feature->Resize(33, (int)input.points_.size());
if (input.HasNormals() == false) {
PrintDebug("[ComputeFPFHFeature] Failed because input point cloud has no normal.\n");
return feature;
}
KDTreeFlann kdtree(input);
auto spfh = ComputeSPFHFeature(input, kdtree, search_param);
#ifdef _OPENMP
#pragma omp parallel for schedule(static)
#endif
for (int i = 0; i < (int)input.points_.size(); i++) {
const auto &point = input.points_[i];
std::vector<int> indices;
std::vector<double> distance2;
if (kdtree.Search(point, search_param, indices, distance2) > 1) {
double sum[3] = {0.0, 0.0, 0.0};
for (size_t k = 1; k < indices.size(); k++) {
// skip the point itself
double dist = distance2[k];
if (dist == 0.0)
continue;
for (int j = 0; j < 33; j++) {
double val = spfh->data_(j, indices[k]) / dist;
sum[j / 11] += val;
feature->data_(j, i) += val;
}
}
for (int j = 0; j < 3; j++)
if (sum[j] != 0.0) sum[j] = 100.0 / sum[j];
for (int j = 0; j < 33; j++) {
feature->data_(j, i) *= sum[j / 11];
// The commented line is the fpfh function in the paper.
// But according to PCL implementation, it is skipped.
// Our initial test shows that the full fpfh function in the
// paper seems to be better than PCL implementation. Further
// test required.
feature->data_(j, i) += spfh->data_(j, i);
}
}
}
return feature;
}
