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