TEST (PCL, NormalEstimationOpenMPEigen)
{
NormalEstimationOMP<PointXYZ, Eigen::MatrixXf> n (4); // instantiate 4 threads
// Object
PointCloud<Eigen::MatrixXf>::Ptr normals (new PointCloud<Eigen::MatrixXf>);
// set parameters
PointCloud<PointXYZ>::Ptr cloudptr = cloud.makeShared ();
n.setInputCloud (cloudptr);
EXPECT_EQ (n.getInputCloud (), cloudptr);
boost::shared_ptr<vector<int> > indicesptr (new vector<int> (indices));
n.setIndices (indicesptr);
EXPECT_EQ (n.getIndices (), indicesptr);
n.setSearchMethod (tree);
EXPECT_EQ (n.getSearchMethod (), tree);
n.setKSearch (static_cast<int> (indices.size ()));
// estimate
n.computeEigen (*normals);
EXPECT_EQ (normals->points.rows (), indices.size ());
for (int i = 0; i < normals->points.rows (); ++i)
{
EXPECT_NEAR (normals->points.row (i)[0], -0.035592, 1e-4);
EXPECT_NEAR (normals->points.row (i)[1], -0.369596, 1e-4);
EXPECT_NEAR (normals->points.row (i)[2], -0.928511, 1e-4);
EXPECT_NEAR (normals->points.row (i)[3], 0.0693136, 1e-4);
}
}
int
main (int argc, char** argv)
{
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
if(io::loadPLYFile<PointXYZ> (argv[1], *cloud) == -1){
cout << "ERROR: couldn't find file" << endl;
return (1);
} else {
cout << "loaded" << endl;
NormalEstimationOMP<PointXYZ, Normal> ne;
search::KdTree<PointXYZ>::Ptr tree1 (new search::KdTree<PointXYZ>);
tree1->setInputCloud (cloud);
ne.setInputCloud (cloud);
ne.setSearchMethod (tree1);
ne.setKSearch (20);
PointCloud<Normal>::Ptr normals (new PointCloud<Normal>);
ne.compute (*normals);
// Concatenate the XYZ and normal fields*
PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
concatenateFields(*cloud, *normals, *cloud_with_normals);
// Create search tree*
search::KdTree<PointNormal>::Ptr tree (new search::KdTree<PointNormal>);
tree->setInputCloud (cloud_with_normals);
cout << "begin marching cubes reconstruction" << endl;
MarchingCubesRBF<PointNormal> mc;
PolygonMesh::Ptr triangles(new PolygonMesh);
mc.setInputCloud (cloud_with_normals);
mc.setSearchMethod (tree);
mc.reconstruct (*triangles);
cout << triangles->polygons.size() << " triangles created" << endl;
}
return (0);
}
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;
}
