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); }