static void passThroughFilter(PointCloudPtr cloudPCLInput, PointCloudPtr cloudPCLOutput, string strAxis, double dMinRange, double dMaxRange)
{
PassThrough<PointT> passThroughFilter;
passThroughFilter.setInputCloud(cloudPCLInput);
passThroughFilter.setFilterFieldName(strAxis);
passThroughFilter.setFilterLimits(dMinRange, dMaxRange);
passThroughFilter.filter(*cloudPCLOutput);
}
// Subsample cloud for faster matching and processing, while filling in normals.
void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const
{
PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced;
PointCloud<Normal> normals;
PointCloud<PointXYZRGBNormal> cloud_normals;
std::vector<int> indices;
// Filter out nans.
removeNaNFromPointCloud(input, cloud_nan_filtered, indices);
indices.clear();
// Filter out everything outside a [200x200x200] box.
Eigen::Vector4f min_pt(-100, -100, -100, -100);
Eigen::Vector4f max_pt(100, 100, 100, 100);
getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices);
ExtractIndices<PointXYZRGB> boxfilter;
boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered));
boxfilter.setIndices (boost::make_shared<vector<int> > (indices));
boxfilter.filter(cloud_box_filtered);
// Reduce pointcloud
VoxelGrid<PointXYZRGB> voxelfilter;
voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered));
voxelfilter.setLeafSize (0.05, 0.05, 0.05);
// voxelfilter.setLeafSize (0.1, 0.1, 0.1);
voxelfilter.filter (cloud_voxel_reduced);
// Compute normals
NormalEstimation<PointXYZRGB, Normal> normalest;
normalest.setViewPoint(0, 0, 0);
normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ());
//normalest.setKSearch (10);
normalest.setRadiusSearch (0.25);
// normalest.setRadiusSearch (0.4);
normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced));
normalest.compute(normals);
pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals);
// Filter based on curvature
PassThrough<PointXYZRGBNormal> normalfilter;
normalfilter.setFilterFieldName("curvature");
// normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setFilterLimits(0.0, 0.2);
normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals));
normalfilter.filter(output);
}
/**
* Implements the PassThrough filter.
* Gets as arguments - output path, field to filter, from limit and to limit.
* Creating the output on the disk and returns filtered point cloud.
*/
PointCloud<pointType>::Ptr FilterHandler::passThroughFilter(string fieldName,
double fromFilterLimit,
double toFilterLimit,
bool negativeLimits)
{
PassThrough<pointType> pass;
pass.setInputCloud(_cloud);
pass.setFilterFieldName(fieldName);
pass.setFilterLimits(fromFilterLimit, toFilterLimit);
pass.setFilterLimitsNegative(negativeLimits);
pass.filter(*_cloud);
io::savePCDFile(_output, *_cloud, true);
return _cloud;
}
int main(int argc, char** argv)
{
readOptions(argc, argv);
PointCloud<PointXYZRGB>::Ptr in(new PointCloud<PointXYZRGB>);
PointCloud<Normal>::Ptr n(new PointCloud<Normal>);
PointCloud<Normal>::Ptr n_mls(new PointCloud<Normal>);
PointCloud<PointXYZRGB>::Ptr p_mls(new PointCloud<PointXYZRGB>);
PointCloud<FPFHSignature33>::Ptr fpfh(new PointCloud<FPFHSignature33>);
PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>);
//PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>);
for (size_t i = 0; i < scenes_.size(); i++)
{
io::loadPCDFile<PointXYZRGB>(folder_ + "raw_labeled/" + scenes_[i] + ".pcd", *in);
PassThrough<PointXYZRGB> pass;
pass.setInputCloud(in);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0f, limit_);
pass.filter(*in);
#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
tree->setInputCloud(in);
for (float r_n = r_min_; r_n <= r_max_; r_n += r_step_)
{
string str_rn = fl2label(r_n,3) + "rn";
NormalEstimationOMP<PointXYZRGB, Normal> norm;
norm.setNumberOfThreads(1);
norm.setInputCloud(in);
norm.setSearchMethod(tree);
norm.setRadiusSearch(r_n);
norm.compute(*n);
#ifdef PCL_MINOR_VERSION
#if PCL_MINOR_VERSION >=6
std::cerr << "current implementation of mls doesn't work with pcl1.7" << std::endl;
exit(-1);
#else
MovingLeastSquares<PointXYZRGB, Normal> mls;
mls.setInputCloud(in);
mls.setOutputNormals(n_mls);
mls.setPolynomialFit(true);
mls.setPolynomialOrder(2);
mls.setSearchMethod(tree);
mls.setSearchRadius(r_n);
mls.reconstruct(*p_mls);
#endif
#endif
for (size_t p = 0; p < n_mls->points.size(); ++p)
{
flipNormalTowardsViewpoint(p_mls->points[p], 0.0f, 0.0f, 0.0f,
n_mls->points[p].normal[0],
n_mls->points[p].normal[1],
n_mls->points[p].normal[2]);
}
io::savePCDFileASCII<PointXYZRGB>(folder_+"normals/"+scenes_[i]+
"/mls_"+scenes_[i]+"_"+str_rn+".pcd",*p_mls);
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree_mls (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
pcl::KdTreeFLANN<PointXYZRGB>::Ptr tree_mls (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree_mls->setInputCloud(p_mls);
for (float r_f = r_n; r_f <= r_max_; r_f += r_step_)
{
cout << "scene: " << scenes_[i] << "\tnormals: " << r_n << "\tfeatures: " << r_f << endl;
string str_rf = fl2label(r_f,3) + "rf";
FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est;
fpfh_est.setNumberOfThreads(1);
fpfh_est.setInputCloud(in);
fpfh_est.setInputNormals(n);
fpfh_est.setSearchMethod(tree);
fpfh_est.setRadiusSearch(r_f);
fpfh_est.compute(*fpfh);
// filename example:
// <folder_>/fpfh/kitchen01/fpfh_kitchen01_020rn_025rf.pcd
// <folder_>/fpfh/kitchen02/fpfh_kitchen02_030rnmls_060rf.pcd
io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] +
"/fpfh_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*fpfh);
FPFHEstimationOMP<PointXYZRGB, Normal, FPFHSignature33> fpfh_est_mls;
fpfh_est_mls.setNumberOfThreads(1);
fpfh_est_mls.setInputCloud(p_mls);
fpfh_est_mls.setInputNormals(n_mls);
fpfh_est_mls.setSearchMethod(tree_mls);
fpfh_est_mls.setRadiusSearch(r_f);
fpfh_est_mls.compute(*fpfh);
io::savePCDFileASCII<FPFHSignature33>(folder_ + "0_fpfh/" + scenes_[i] +
"/fpfh_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*fpfh);
PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est;
pc_est.setInputCloud(in);
pc_est.setInputNormals(n);
pc_est.setSearchMethod(tree);
pc_est.setRadiusSearch(r_f);
pc_est.compute(*pc);
io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] +
"/pc_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*pc);
PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pc_est_mls;
pc_est_mls.setInputCloud(p_mls);
pc_est_mls.setInputNormals(n_mls);
pc_est_mls.setSearchMethod(tree_mls);
pc_est_mls.setRadiusSearch(r_f);
pc_est_mls.compute(*pc);
io::savePCDFileASCII<PrincipalCurvatures>(folder_ + "0_pc/" + scenes_[i] +
"/pc_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*pc);
/*RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est;
rsd_est.setInputCloud(in);
rsd_est.setInputNormals(n);
rsd_est.setSearchMethod(tree);
rsd_est.setPlaneRadius(0.5);
rsd_est.setRadiusSearch(r_f);
rsd_est.compute(*rsd);
io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] +
"/rsd_" + scenes_[i] +"_"+str_rn+"_"+str_rf+".pcd",
*rsd);
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsd_est_mls;
rsd_est_mls.setInputCloud(p_mls);
rsd_est_mls.setInputNormals(n_mls);
rsd_est_mls.setSearchMethod(tree_mls);
rsd_est_mls.setPlaneRadius(0.5);
rsd_est_mls.setRadiusSearch(r_f);
rsd_est_mls.compute(*rsd);
io::savePCDFileASCII<PrincipalRadiiRSD>(folder_ + "0_rsd/" + scenes_[i] +
"/rsd_" + scenes_[i] +"_"+str_rn+"mls_"+str_rf+".pcd",
*rsd);*/
}
}
}
return 0;
}
void processRSD(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr rsd_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<PrincipalRadiiRSD>::Ptr rsd(new PointCloud<PrincipalRadiiRSD>());
// passthrough filtering (needed to remove NaNs)
cout << "RSD: 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 (rsd_vox_enable_)
{
cout << "RSD: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(rsd_vox_, rsd_vox_, rsd_vox_);
vox.filter(*ref_out);
}
#ifdef PCL_VERSION_COMPARE //fuerte
pcl::search::KdTree<PointXYZRGB>::Ptr tree (new pcl::search::KdTree<PointXYZRGB>());
#else //electric
KdTreeFLANN<PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<PointXYZRGB> ());
#endif
tree->setInputCloud(ref_out);
// optional surface smoothing
if(rsd_mls_enable_)
{
cout << "RSD: 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(rsd_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "RSD: 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 << "RSD: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(rsd_rn_);
norm.compute(*n);
}
tree->setInputCloud(ref_out);
// RSD estimation
cout << "RSD: estimation (with " << ref_out->points.size() << " points)" << endl;
RSDEstimation<PointXYZRGB, Normal, PrincipalRadiiRSD> rsdE;
rsdE.setInputCloud(ref_out);
rsdE.setInputNormals(n);
rsdE.setSearchMethod(tree);
rsdE.setPlaneRadius(r_limit_);
rsdE.setRadiusSearch(rsd_rf_);
rsdE.compute(*rsd);
cout << "RSD: classification " << endl;
*rsd_out = *ref_out;
// apply RSD rules for classification
int exp_rgb, pre_rgb;
float r_max,r_min;
cob_3d_mapping_common::LabelResults stats(fl2label(rsd_rn_),fl2label(rsd_rf_),rsd_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
r_max = rsd->points[idx].r_max;
r_min = rsd->points[idx].r_min;
if ( r_min > r_high )
{
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
}
else if (r_min < r_low)
{
if (r_max < r_r2 * r_min)
{
pre_rgb = LBL_COR;
if (exp_rgb != LBL_COR && exp_rgb != LBL_UNDEF) stats.fp[EVAL_COR]++;
}
else
{
pre_rgb = LBL_EDGE;
if (exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_EDGE]++;
}
// special case: combined class for corner and edge
if (exp_rgb != LBL_COR && exp_rgb != LBL_EDGE && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_EDGECORNER]++;
}
else
{
if (r_max < r_r1 * r_min)
{
pre_rgb = LBL_SPH;
if (exp_rgb != LBL_SPH && exp_rgb != LBL_UNDEF) stats.fp[EVAL_SPH]++;
}
else
{
pre_rgb = LBL_CYL;
if (exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF) stats.fp[EVAL_CYL]++;
}
// special case: combined class for sphere and cylinder
if (exp_rgb != LBL_SPH && exp_rgb != LBL_CYL && exp_rgb != LBL_UNDEF)
stats.fp[EVAL_CURVED]++;
}
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;
}
rsd_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "RSD:\n" << stats << endl << endl;
}
/*! @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;
}
