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);
    }
  }
Esempio n. 2
0
int main (int argc, char **argv)
{
   /* if (argc != 1)
    {
        PCL_ERROR ("Syntax: %s input.pcd output.ply\n", argv[0]);
         return -1;
    }*/

    PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
    io::loadPCDFile (argv[1], *cloud);
    MovingLeastSquares<PointXYZ, PointXYZ> mls;
    mls.setInputCloud (cloud);
    mls.setSearchRadius (4);
    mls.setPolynomialFit (true);
    mls.setPolynomialOrder (1);
    mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointXYZ>::SAMPLE_LOCAL_PLANE);
    mls.setUpsamplingRadius (1);
    mls.setUpsamplingStepSize (0.03);

    PointCloud<PointXYZ>::Ptr cloud_smoothed (new PointCloud<PointXYZ> ());
    mls.process (*cloud_smoothed);

    NormalEstimationOMP<PointXYZ, Normal> ne;
    ne.setNumberOfThreads (8);
    ne.setInputCloud (cloud_smoothed);
    ne.setRadiusSearch (0.8);
    Eigen::Vector4f centroid;
    compute3DCentroid (*cloud_smoothed, centroid);
    ne.setViewPoint (centroid[0], centroid[1], centroid[2]);

    PointCloud<Normal>::Ptr cloud_normals (new PointCloud<Normal> ());
    ne.compute (*cloud_normals);
    for (size_t i = 0; i < cloud_normals->size (); ++i)
        {
            cloud_normals->points[i].normal_x *= -1;
            cloud_normals->points[i].normal_y *= -1;
            cloud_normals->points[i].normal_z *= -1;
        }
    PointCloud<PointNormal>::Ptr cloud_smoothed_normals (new PointCloud<PointNormal> ());
    concatenateFields (*cloud_smoothed, *cloud_normals, *cloud_smoothed_normals);

    Poisson<pcl::PointNormal> poisson;
    poisson.setDepth (9);
    poisson.setInputCloud  (cloud_smoothed_normals); 
    PolygonMesh mesh_poisson;
    poisson.reconstruct (mesh_poisson);
    pcl::io::savePLYFile("mesh.ply", mesh_poisson);
    return 0;
}
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;
}