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); }
/*! @brief runs the whole processing pipeline for FPFH features * * @note At the moment the evaluation results will be printed to console. * * @param[in] in the labeled input point cloud * @param[out] ref_out the reference point cloud after the preprocessing steps * @param[out] fpfh_out the labeled point cloud after the classifing process */ void processFPFH(const PointCloud<PointXYZRGB>::Ptr in, PointCloud<PointXYZRGB>::Ptr ref_out, PointCloud<PointXYZRGB>::Ptr fpfh_out) { PointCloud<Normal>::Ptr n(new PointCloud<Normal>()); PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>()); // Passthrough filtering (needs to be done to remove NaNs) cout << "FPFH: Pass (with " << in->points.size() << " points)" << endl; PassThrough<PointXYZRGB> pass; pass.setInputCloud(in); pass.setFilterFieldName("z"); pass.setFilterLimits(0.0f, pass_depth_); pass.filter(*ref_out); // Optional voxelgrid filtering if (fpfh_vox_enable_) { cout << "FPFH: Voxel (with " << ref_out->points.size() << " points)" << endl; VoxelGrid<PointXYZRGB> vox; vox.setInputCloud(ref_out); vox.setLeafSize(fpfh_vox_, fpfh_vox_, fpfh_vox_); vox.filter(*ref_out); } #ifdef PCL_VERSION_COMPARE //fuerte pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>()); #else //electric pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ()); #endif //KdTree<PointXYZRGB>::Ptr tree(new KdTreeFLANN<PointXYZRGB>()); tree->setInputCloud(ref_out); // Optional surface smoothing if(fpfh_mls_enable_) { cout << "FPFH: MLS (with " << ref_out->points.size() << " points)" << endl; #ifdef PCL_VERSION_COMPARE std::cerr << "MLS has changed completely in PCL 1.7! Requires redesign of entire program" << std::endl; exit(0); #else MovingLeastSquares<PointXYZRGB, Normal> mls; mls.setInputCloud(ref_out); mls.setOutputNormals(n); mls.setPolynomialFit(true); mls.setPolynomialOrder(2); mls.setSearchMethod(tree); mls.setSearchRadius(fpfh_rn_); mls.reconstruct(*ref_out); #endif cout << "FPFH: flip normals (with " << ref_out->points.size() << " points)" << endl; for (size_t i = 0; i < ref_out->points.size(); ++i) { flipNormalTowardsViewpoint(ref_out->points[i], 0.0f, 0.0f, 0.0f, n->points[i].normal[0], n->points[i].normal[1], n->points[i].normal[2]); } } else { cout << "FPFH: Normals (with " << ref_out->points.size() << " points)" << endl; NormalEstimation<PointXYZRGB, Normal> norm; norm.setInputCloud(ref_out); norm.setSearchMethod(tree); norm.setRadiusSearch(fpfh_rn_); norm.compute(*n); } // FPFH estimation #ifdef PCL_VERSION_COMPARE //fuerte tree.reset(new pcl::search::KdTree<PointXYZRGB>()); #else //electric tree.reset(new KdTreeFLANN<PointXYZRGB> ()); #endif tree->setInputCloud(ref_out); cout << "FPFH: estimation (with " << ref_out->points.size() << " points)" << endl; FPFHEstimation<PointXYZRGB, Normal, FPFHSignature33> fpfhE; fpfhE.setInputCloud(ref_out); fpfhE.setInputNormals(n); fpfhE.setSearchMethod(tree); fpfhE.setRadiusSearch(fpfh_rf_); fpfhE.compute(*fpfh); cout << "FPFH: classification " << endl; *fpfh_out = *ref_out; CvSVM svm; svm.load(fpfh_svm_model_.c_str()); cv::Mat fpfh_histo(1, 33, CV_32FC1); int exp_rgb, pre_rgb, predict; cob_3d_mapping_common::LabelResults stats(fl2label(fpfh_rn_),fl2label(fpfh_rf_),fpfh_mls_enable_); for (size_t idx = 0; idx < ref_out->points.size(); idx++) { exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label memcpy(fpfh_histo.ptr<float>(0), fpfh->points[idx].histogram, sizeof(fpfh->points[idx].histogram)); predict = (int)svm.predict(fpfh_histo); //cout << predict << endl; switch(predict) { case SVM_PLANE: pre_rgb = LBL_PLANE; if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++; break; case SVM_EDGE: pre_rgb = LBL_EDGE; if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++; if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++; break; case SVM_COR: pre_rgb = LBL_COR; if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++; if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGECORNER]++; break; case SVM_SPH: pre_rgb = LBL_SPH; if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++; if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++; break; case SVM_CYL: pre_rgb = LBL_CYL; if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++; if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CURVED]++; break; default: pre_rgb = LBL_UNDEF; break; } switch(exp_rgb) { case LBL_PLANE: if (pre_rgb != exp_rgb) stats.fn[EVAL_PLANE]++; stats.exp[EVAL_PLANE]++; break; case LBL_EDGE: if (pre_rgb != exp_rgb) { stats.fn[EVAL_EDGE]++; if (pre_rgb != LBL_COR) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_EDGE]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_COR: if (pre_rgb != exp_rgb) { stats.fn[EVAL_COR]++; if (pre_rgb != LBL_EDGE) stats.fn[EVAL_EDGECORNER]++; } stats.exp[EVAL_COR]++; stats.exp[EVAL_EDGECORNER]++; break; case LBL_SPH: if (pre_rgb != exp_rgb) { stats.fn[EVAL_SPH]++; if (pre_rgb != LBL_CYL) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_SPH]++; stats.exp[EVAL_CURVED]++; break; case LBL_CYL: if (pre_rgb != exp_rgb) { stats.fn[EVAL_CYL]++; if (pre_rgb != LBL_SPH) stats.fn[EVAL_CURVED]++; } stats.exp[EVAL_CYL]++; stats.exp[EVAL_CURVED]++; break; default: stats.undef++; break; } fpfh_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb); } cout << "FPFH:\n" << stats << endl << endl; }