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);
}
/*! @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;
}