TEST (PCL, PrincipalCurvaturesEstimation)
{
float pcx, pcy, pcz, pc1, pc2;
// 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);
PrincipalCurvaturesEstimation<PointXYZ, Normal, PrincipalCurvatures> pc;
pc.setInputNormals (normals);
EXPECT_EQ (pc.getInputNormals (), normals);
// computePointPrincipalCurvatures (indices)
pc.computePointPrincipalCurvatures (*normals, 0, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (fabs (pcx), 0.98509, 1e-4);
EXPECT_NEAR (fabs (pcy), 0.10714, 1e-4);
EXPECT_NEAR (fabs (pcz), 0.13462, 1e-4);
EXPECT_NEAR (pc1, 0.23997423052787781, 1e-4);
EXPECT_NEAR (pc2, 0.19400238990783691, 1e-4);
pc.computePointPrincipalCurvatures (*normals, 2, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.98079, 1e-4);
EXPECT_NEAR (pcy, -0.04019, 1e-4);
EXPECT_NEAR (pcz, 0.19086, 1e-4);
EXPECT_NEAR (pc1, 0.27207490801811218, 1e-4);
EXPECT_NEAR (pc2, 0.19464978575706482, 1e-4);
int indices_size = static_cast<int> (indices.size ());
pc.computePointPrincipalCurvatures (*normals, indices_size - 3, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.86725, 1e-4);
EXPECT_NEAR (pcy, -0.37599, 1e-4);
EXPECT_NEAR (pcz, 0.32635, 1e-4);
EXPECT_NEAR (pc1, 0.25900053977966309, 1e-4);
EXPECT_NEAR (pc2, 0.17906945943832397, 1e-4);
pc.computePointPrincipalCurvatures (*normals, indices_size - 1, indices, pcx, pcy, pcz, pc1, pc2);
EXPECT_NEAR (pcx, 0.86725, 1e-4);
EXPECT_NEAR (pcy, -0.375851, 1e-3);
EXPECT_NEAR (pcz, 0.32636, 1e-4);
EXPECT_NEAR (pc1, 0.2590005099773407, 1e-4);
EXPECT_NEAR (pc2, 0.17906956374645233, 1e-4);
// Object
PointCloud<PrincipalCurvatures>::Ptr pcs (new PointCloud<PrincipalCurvatures> ());
// set parameters
pc.setInputCloud (cloud.makeShared ());
pc.setIndices (indicesptr);
pc.setSearchMethod (tree);
pc.setKSearch (indices_size);
// estimate
pc.compute (*pcs);
EXPECT_EQ (pcs->points.size (), indices.size ());
// Adjust for small numerical inconsitencies (due to nn_indices not being sorted)
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[0]), 0.98509, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[1]), 0.10713, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].principal_curvature[2]), 0.13462, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].pc1), 0.23997458815574646, 1e-4);
EXPECT_NEAR (fabs (pcs->points[0].pc2), 0.19400238990783691, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[0], 0.98079, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[1], -0.04019, 1e-4);
EXPECT_NEAR (pcs->points[2].principal_curvature[2], 0.19086, 1e-4);
EXPECT_NEAR (pcs->points[2].pc1, 0.27207502722740173, 1e-4);
EXPECT_NEAR (pcs->points[2].pc2, 0.1946497857570648, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[0], 0.86725, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[1], -0.37599, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].principal_curvature[2], 0.32636, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].pc1, 0.2590007483959198, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 3].pc2, 0.17906941473484039, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[0], 0.86725, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[1], -0.375851, 1e-3);
EXPECT_NEAR (pcs->points[indices.size () - 1].principal_curvature[2], 0.32636, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].pc1, 0.25900065898895264, 1e-4);
EXPECT_NEAR (pcs->points[indices.size () - 1].pc2, 0.17906941473484039, 1e-4);
}
void processPC(const PointCloud<PointXYZRGB>::Ptr in,
PointCloud<PointXYZRGB>::Ptr ref_out,
PointCloud<PointXYZRGB>::Ptr pc_out)
{
PointCloud<Normal>::Ptr n(new PointCloud<Normal>());
PointCloud<PrincipalCurvatures>::Ptr pc(new PointCloud<PrincipalCurvatures>());
// passthrough filtering (needed to remove NaNs)
cout << "PC: 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 (pc_vox_enable_)
{
cout << "PC: Voxel (with " << ref_out->points.size() << " points)" << endl;
VoxelGrid<PointXYZRGB> vox;
vox.setInputCloud(ref_out);
vox.setLeafSize(pc_vox_, pc_vox_, pc_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(pc_mls_enable_)
{
cout << "PC: 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(pc_rn_);
mls.reconstruct(*ref_out);
#endif
cout << "PC: 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 << "PC: Normals (with " << ref_out->points.size() << " points)" << endl;
NormalEstimation<PointXYZRGB, Normal> norm;
norm.setInputCloud(ref_out);
norm.setSearchMethod(tree);
norm.setRadiusSearch(pc_rn_);
norm.compute(*n);
}
// estimate PC
#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 << "PC: estimation (with " << ref_out->points.size() << " points)" << endl;
PrincipalCurvaturesEstimation<PointXYZRGB, Normal, PrincipalCurvatures> pcE;
pcE.setInputCloud(ref_out);
pcE.setInputNormals(n);
pcE.setSearchMethod(tree);
pcE.setRadiusSearch(pc_rf_);
pcE.compute(*pc);
cout << "PC: classification " << endl;
*pc_out = *ref_out;
int exp_rgb, pre_rgb;
float c_max,c_min;
cob_3d_mapping_common::LabelResults stats(fl2label(pc_rn_),fl2label(pc_rf_),pc_mls_enable_);
// apply rules to PC results
for (size_t idx = 0; idx < ref_out->points.size(); idx++)
{
exp_rgb = *reinterpret_cast<int*>(&ref_out->points[idx].rgb); // expected label
c_max = pc->points[idx].pc1;
c_min = pc->points[idx].pc2;
if ( c_max < c_low )
{
pre_rgb = LBL_PLANE;
if (exp_rgb != LBL_PLANE && exp_rgb != LBL_UNDEF) stats.fp[EVAL_PLANE]++;
}
else if (c_max > c_high)
{
if (c_max < c_r2 * c_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 (c_max < c_r1 * c_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;
}
pc_out->points[idx].rgb = *reinterpret_cast<float*>(&pre_rgb);
}
cout << "PC:\n" << stats << endl << endl;
}
