void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int k, double radius)
{
// Convert data to PointCloud<T>
PointCloud<PointNormal>::Ptr xyznormals (new PointCloud<PointNormal>);
fromROSMsg (*input, *xyznormals);
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
FPFHEstimation<PointNormal, PointNormal, FPFHSignature33> ne;
ne.setInputCloud (xyznormals);
ne.setInputNormals (xyznormals);
ne.setSearchMethod (search::KdTree<PointNormal>::Ptr (new search::KdTree<PointNormal>));
ne.setKSearch (k);
ne.setRadiusSearch (radius);
PointCloud<FPFHSignature33> fpfhs;
ne.compute (fpfhs);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", fpfhs.width * fpfhs.height); print_info (" points]\n");
// Convert data back
sensor_msgs::PointCloud2 output_fpfhs;
toROSMsg (fpfhs, output_fpfhs);
concatenateFields (*input, output_fpfhs, output);
}
TEST (PCL, FPFHEstimation)
{
// Estimate normals first
NormalEstimation<PointXYZ, Normal> n;
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
// set parameters
n.setInputCloud (cloud.makeShared ());
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
n.setSearchMethod (tree);
n.setKSearch (10); // Use 10 nearest neighbors to estimate the normals
// estimate
n.compute (*normals);
FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh;
fpfh.setInputNormals (normals);
EXPECT_EQ (fpfh.getInputNormals (), normals);
// computePointSPFHSignature
int nr_subdiv = 11; // use the same number of bins for all three angular features
Eigen::MatrixXf hist_f1 (indices.size (), nr_subdiv), hist_f2 (indices.size (), nr_subdiv), hist_f3 (indices.size (), nr_subdiv);
hist_f1.setZero (); hist_f2.setZero (); hist_f3.setZero ();
for (int i = 0; i < static_cast<int> (indices.size ()); ++i)
fpfh.computePointSPFHSignature (cloud, *normals, i, i, indices, hist_f1, hist_f2, hist_f3);
EXPECT_NEAR (hist_f1 (0, 0), 0.757576, 1e-4);
EXPECT_NEAR (hist_f1 (0, 1), 0.757576, 1e-4);
EXPECT_NEAR (hist_f1 (0, 2), 4.54545, 1e-4);
EXPECT_NEAR (hist_f1 (0, 3), 19.697, 1e-4);
EXPECT_NEAR (hist_f1 (0, 4), 40.6566, 1e-4);
EXPECT_NEAR (hist_f1 (0, 5), 21.4647, 1e-4);
EXPECT_NEAR (hist_f1 (0, 6), 7.575759, 1e-4);
EXPECT_NEAR (hist_f1 (0, 7), 0.000000, 1e-4);
EXPECT_NEAR (hist_f1 (0, 8), 0.000000, 1e-4);
EXPECT_NEAR (hist_f1 (0, 9), 0.50505, 1e-4);
EXPECT_NEAR (hist_f1 (0, 10), 4.0404, 1e-4);
EXPECT_NEAR (hist_f2 (0, 0), 0.757576, 1e-4);
EXPECT_NEAR (hist_f2 (0, 1), 1.51515, 1e-4);
EXPECT_NEAR (hist_f2 (0, 2), 6.31313, 1e-4);
EXPECT_NEAR (hist_f2 (0, 3), 9.59596, 1e-4);
EXPECT_NEAR (hist_f2 (0, 4), 20.7071, 1e-4);
EXPECT_NEAR (hist_f2 (0, 5), 18.9394, 1e-4);
EXPECT_NEAR (hist_f2 (0, 6), 15.9091, 1e-4);
EXPECT_NEAR (hist_f2 (0, 7), 12.8788, 1e-4);
EXPECT_NEAR (hist_f2 (0, 8), 6.56566, 1e-4);
EXPECT_NEAR (hist_f2 (0, 9), 4.29293, 1e-4);
EXPECT_NEAR (hist_f2 (0, 10), 2.52525, 1e-4);
EXPECT_NEAR (hist_f3 (0, 0), 0.000000, 1e-4);
EXPECT_NEAR (hist_f3 (0, 1), 5.05051, 1e-4);
EXPECT_NEAR (hist_f3 (0, 2), 4.54545, 1e-4);
EXPECT_NEAR (hist_f3 (0, 3), 5.05051, 1e-4);
EXPECT_NEAR (hist_f3 (0, 4), 1.76768, 1e-4);
EXPECT_NEAR (hist_f3 (0, 5), 3.0303, 1e-4);
EXPECT_NEAR (hist_f3 (0, 6), 9.09091, 1e-4);
EXPECT_NEAR (hist_f3 (0, 7), 31.8182, 1e-4);
EXPECT_NEAR (hist_f3 (0, 8), 22.2222, 1e-4);
EXPECT_NEAR (hist_f3 (0, 9), 11.8687, 1e-4);
EXPECT_NEAR (hist_f3 (0, 10), 5.55556, 1e-4);
// weightPointSPFHSignature
Eigen::VectorXf fpfh_histogram (nr_subdiv + nr_subdiv + nr_subdiv);
fpfh_histogram.setZero ();
vector<float> dists (indices.size ());
for (size_t i = 0; i < dists.size (); ++i) dists[i] = static_cast<float> (i);
fpfh.weightPointSPFHSignature (hist_f1, hist_f2, hist_f3, indices, dists, fpfh_histogram);
EXPECT_NEAR (fpfh_histogram[0], 1.9798 , 1e-2);
EXPECT_NEAR (fpfh_histogram[1], 2.86927, 1e-2);
EXPECT_NEAR (fpfh_histogram[2], 8.47911, 1e-2);
EXPECT_NEAR (fpfh_histogram[3], 22.8784, 1e-2);
EXPECT_NEAR (fpfh_histogram[4], 29.8597, 1e-2);
EXPECT_NEAR (fpfh_histogram[5], 19.6877, 1e-2);
EXPECT_NEAR (fpfh_histogram[6], 7.38611, 1e-2);
EXPECT_NEAR (fpfh_histogram[7], 1.44265, 1e-2);
EXPECT_NEAR (fpfh_histogram[8], 0.69677, 1e-2);
EXPECT_NEAR (fpfh_histogram[9], 1.72609, 1e-2);
EXPECT_NEAR (fpfh_histogram[10], 2.99435, 1e-2);
EXPECT_NEAR (fpfh_histogram[11], 2.26313, 1e-2);
EXPECT_NEAR (fpfh_histogram[12], 5.16573, 1e-2);
EXPECT_NEAR (fpfh_histogram[13], 8.3263 , 1e-2);
EXPECT_NEAR (fpfh_histogram[14], 9.92427, 1e-2);
EXPECT_NEAR (fpfh_histogram[15], 16.8062, 1e-2);
EXPECT_NEAR (fpfh_histogram[16], 16.2767, 1e-2);
EXPECT_NEAR (fpfh_histogram[17], 12.251 , 1e-2);
//EXPECT_NEAR (fpfh_histogram[18], 10.354, 1e-1);
//EXPECT_NEAR (fpfh_histogram[19], 6.65578, 1e-2);
EXPECT_NEAR (fpfh_histogram[20], 6.1437 , 1e-2);
EXPECT_NEAR (fpfh_histogram[21], 5.83341, 1e-2);
EXPECT_NEAR (fpfh_histogram[22], 1.08809, 1e-2);
EXPECT_NEAR (fpfh_histogram[23], 3.34133, 1e-2);
EXPECT_NEAR (fpfh_histogram[24], 5.59236, 1e-2);
EXPECT_NEAR (fpfh_histogram[25], 5.6355 , 1e-2);
EXPECT_NEAR (fpfh_histogram[26], 3.03257, 1e-2);
EXPECT_NEAR (fpfh_histogram[27], 1.37437, 1e-2);
EXPECT_NEAR (fpfh_histogram[28], 7.99746, 1e-2);
EXPECT_NEAR (fpfh_histogram[29], 18.0343, 1e-2);
EXPECT_NEAR (fpfh_histogram[30], 23.691 , 1e-2);
EXPECT_NEAR (fpfh_histogram[31], 19.8475, 1e-2);
EXPECT_NEAR (fpfh_histogram[32], 10.3655, 1e-2);
// Object
PointCloud<FPFHSignature33>::Ptr fpfhs (new PointCloud<FPFHSignature33> ());
// set parameters
fpfh.setInputCloud (cloud.makeShared ());
fpfh.setNrSubdivisions (11, 11, 11);
fpfh.setIndices (indicesptr);
fpfh.setSearchMethod (tree);
fpfh.setKSearch (static_cast<int> (indices.size ()));
// estimate
fpfh.compute (*fpfhs);
EXPECT_EQ (fpfhs->points.size (), indices.size ());
EXPECT_NEAR (fpfhs->points[0].histogram[0], 1.58591, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[1], 1.68365, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[2], 6.71 , 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[3], 23.0717, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[4], 33.3844, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[5], 20.4002, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[6], 7.31067, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[7], 1.02635, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[8], 0.48591, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[9], 1.47069, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[10], 2.87061, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[11], 1.78321, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[12], 4.30795, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[13], 7.05514, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[14], 9.37615, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[15], 17.963 , 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[16], 18.2801, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[17], 14.2766, 1e-2);
//EXPECT_NEAR (fpfhs->points[0].histogram[18], 10.8542, 1e-2);
//EXPECT_NEAR (fpfhs->points[0].histogram[19], 6.07925, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[20], 5.28565, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[21], 4.73887, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[22], 0.56984, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[23], 3.29826, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[24], 5.28156, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[25], 5.26939, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[26], 3.13191, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[27], 1.74453, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[28], 9.41971, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[29], 21.5894, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[30], 24.6302, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[31], 17.7764, 1e-2);
EXPECT_NEAR (fpfhs->points[0].histogram[32], 7.28878, 1e-2);
// Test results when setIndices and/or setSearchSurface are used
boost::shared_ptr<vector<int> > test_indices (new vector<int> (0));
for (size_t i = 0; i < cloud.size (); i+=3)
test_indices->push_back (static_cast<int> (i));
testIndicesAndSearchSurface<FPFHEstimation<PointXYZ, Normal, FPFHSignature33>, PointXYZ, Normal, FPFHSignature33>
(cloud.makeShared (), normals, test_indices, 33);
}