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