Ejemplo n.º 1
0
  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;
}