Beispiel #1
0
int
main (int argc, char *argv[])
{
  std::string pcd_file, file_3dm;

  if (argc < 3)
  {
    printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd<PointXYZ>-in-file 3dm-out-file\n\n");
    exit (0);
  }
  pcd_file = argv[1];
  file_3dm = argv[2];

  pcl::visualization::PCLVisualizer viewer ("B-spline surface fitting");
  viewer.setSize (800, 600);

  // ############################################################################
  // load point cloud

  printf ("  loading %s\n", pcd_file.c_str ());
  pcl::PointCloud<Point>::Ptr cloud (new pcl::PointCloud<Point>);
  pcl::PCLPointCloud2 cloud2;
  pcl::on_nurbs::NurbsDataSurface data;

  if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
    throw std::runtime_error ("  PCD file not found.");

  fromPCLPointCloud2 (cloud2, *cloud);
  PointCloud2Vector3d (cloud, data.interior);
  pcl::visualization::PointCloudColorHandlerCustom<Point> handler (cloud, 0, 255, 0);
  viewer.addPointCloud<Point> (cloud, handler, "cloud_cylinder");
  printf ("  %lu points in data set\n", cloud->size ());

  // ############################################################################
  // fit B-spline surface

  // parameters
  unsigned order (3);
  unsigned refinement (5);
  unsigned iterations (10);
  unsigned mesh_resolution (256);

  pcl::on_nurbs::FittingSurface::Parameter params;
  params.interior_smoothness = 0.2;
  params.interior_weight = 1.0;
  params.boundary_smoothness = 0.2;
  params.boundary_weight = 0.0;

  // initialize
  printf ("  surface fitting ...\n");
  ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data);
  pcl::on_nurbs::FittingSurface fit (&data, nurbs);
  //  fit.setQuiet (false); // enable/disable debug output

  // mesh for visualization
  pcl::PolygonMesh mesh;
  pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::Vertices> mesh_vertices;
  std::string mesh_id = "mesh_nurbs";
  pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);

  // surface refinement
  for (unsigned i = 0; i < refinement; i++)
  {
    fit.refine (0);
    fit.refine (1);
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // surface fitting with final refinement level
  for (unsigned i = 0; i < iterations; i++)
  {
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // ############################################################################
  // fit B-spline curve

  // parameters
  pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
  curve_params.addCPsAccuracy = 5e-2;
  curve_params.addCPsIteration = 3;
  curve_params.maxCPs = 200;
  curve_params.accuracy = 1e-3;
  curve_params.iterations = 100;

  curve_params.param.closest_point_resolution = 0;
  curve_params.param.closest_point_weight = 1.0;
  curve_params.param.closest_point_sigma2 = 0.1;
  curve_params.param.interior_sigma2 = 0.00001;
  curve_params.param.smooth_concavity = 1.0;
  curve_params.param.smoothness = 1.0;

  // initialisation (circular)
  printf ("  curve fitting ...\n");
  pcl::on_nurbs::NurbsDataCurve2d curve_data;
  curve_data.interior = data.interior_param;
  curve_data.interior_weight_function.push_back (true);
  ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior);

  // curve fitting
  pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
  // curve_fit.setQuiet (false); // enable/disable debug output
  curve_fit.fitting (curve_params);
  visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer);

  // ############################################################################
  // triangulation of trimmed surface

  printf ("  triangulate trimmed surface ...\n");
  viewer.removePolygonMesh (mesh_id);
  pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh,
                                                                   mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);


  // save trimmed B-spline surface
  if ( fit.m_nurbs.IsValid() )
  {
    ONX_Model model;
    ONX_Model_Object& surf = model.m_object_table.AppendNew();
    surf.m_object = new ON_NurbsSurface(fit.m_nurbs);
    surf.m_bDeleteObject = true;
    surf.m_attributes.m_layer_index = 1;
    surf.m_attributes.m_name = "surface";

    ONX_Model_Object& curv = model.m_object_table.AppendNew();
    curv.m_object = new ON_NurbsCurve(curve_fit.m_nurbs);
    curv.m_bDeleteObject = true;
    curv.m_attributes.m_layer_index = 2;
    curv.m_attributes.m_name = "trimming curve";

    model.Write(file_3dm.c_str());
    printf("  model saved: %s\n", file_3dm.c_str());
  }

  printf ("  ... done.\n");

  viewer.spin ();
  return 0;
}
int
main (int argc, char *argv[])
{
  std::string pcd_file;

  if (argc > 1)
  {
    pcd_file = argv[1];
  }
  else
  {
    printf ("\nUsage: boundaryFittingPDM pcd-file \n\n");
    printf ("  pcd-file    point-cloud file containing the boundary points (xy)\n");
    exit (0);
  }

  // #################### LOAD FILE #########################
  printf ("  loading %s\n", pcd_file.c_str ());
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  sensor_msgs::PointCloud2 cloud2;

  if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
    throw std::runtime_error ("  PCD file not found.");

  fromROSMsg (cloud2, *cloud);

  viewer.setSize (800, 600);
  viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");

  pcl::on_nurbs::NurbsDataCurve2d data;
  PointCloud2Vector2d (cloud, data.interior);

  // #################### CURVE PARAMETERS #########################
  unsigned order (3);
  unsigned n_control_points (20);

  pcl::on_nurbs::FittingCurve2d::FitParameter curve_params;
  curve_params.addCPsAccuracy = 1000;   // no control points added
  curve_params.addCPsIteration = 1000;  // no control points added
  curve_params.maxCPs = 1000;
  curve_params.fitMaxError = 1e-6;
  curve_params.fitAvgError = 1e-8;
  curve_params.fitMaxSteps = 50;
  curve_params.refinement = 0;

  curve_params.param.closest_point_resolution = 0;
  curve_params.param.closest_point_weight = 0.0;
  curve_params.param.closest_point_sigma2 = 0.0;
  curve_params.param.interior_sigma2 = 0.0;
  curve_params.param.smooth_concavity = 0.0;
  curve_params.param.smoothness = 0.000001;

  data.interior_weight_function.push_back (false);

  // #################### CURVE FITTING #########################
  ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2d::initNurbsCurve2D (order, data.interior, n_control_points);
  pcl::on_nurbs::FittingCurve2d curve_fit (&data, curve);
  curve_fit.fitting (curve_params);

  VisualizeCurve (curve_fit.m_nurbs);

  viewer.spin ();
  return 0;
}