example_nurbs_fitting_closed_curve3d.cpp
Go to the documentation of this file.
00001 #include <pcl/surface/on_nurbs/fitting_curve_pdm.h>
00002 #include <pcl/surface/on_nurbs/triangulation.h>
00003 
00004 #include <pcl/point_cloud.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/io/pcd_io.h>
00007 
00008 #include <pcl/visualization/pcl_visualizer.h>
00009 
00010 pcl::visualization::PCLVisualizer viewer ("Curve Fitting 3D");
00011 
00012 void
00013 PointCloud2Vector2d (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
00014 {
00015   for (unsigned i = 0; i < cloud->size (); i++)
00016   {
00017     pcl::PointXYZ &p = cloud->at (i);
00018     if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
00019       data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
00020   }
00021 }
00022 
00023 void
00024 VisualizeCurve (ON_NurbsCurve &curve, double r, double g, double b, bool show_cps)
00025 {
00026   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00027   pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, cloud, 8);
00028 
00029   for (std::size_t i = 0; i < cloud->size () - 1; i++)
00030   {
00031     pcl::PointXYZRGB &p1 = cloud->at (i);
00032     pcl::PointXYZRGB &p2 = cloud->at (i + 1);
00033     std::ostringstream os;
00034     os << "line_" << r << "_" << g << "_" << b << "_" << i;
00035     viewer.addLine<pcl::PointXYZRGB> (p1, p2, r, g, b, os.str ());
00036   }
00037 
00038   if (show_cps)
00039   {
00040     pcl::PointCloud<pcl::PointXYZ>::Ptr cps (new pcl::PointCloud<pcl::PointXYZ>);
00041     for (int i = 0; i < curve.CVCount (); i++)
00042     {
00043       ON_3dPoint cp;
00044       curve.GetCV (i, cp);
00045 
00046       pcl::PointXYZ p;
00047       p.x = float (cp.x);
00048       p.y = float (cp.y);
00049       p.z = float (cp.z);
00050       cps->push_back (p);
00051     }
00052     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler (cps, 255 * r, 255 * g, 255 * b);
00053     viewer.addPointCloud<pcl::PointXYZ> (cps, handler, "cloud_cps");
00054   }
00055 }
00056 
00057 int
00058 main (int argc, char *argv[])
00059 {
00060   std::string pcd_file;
00061 
00062   if (argc > 1)
00063   {
00064     pcd_file = argv[1];
00065   }
00066   else
00067   {
00068     printf ("\nUsage: pcl_example_nurbs_fitting_curve3d pcd-file \n\n");
00069     printf ("  pcd-file    point-cloud file\n");
00070     exit (0);
00071   }
00072 
00073   // #################### LOAD FILE #########################
00074   printf ("  loading %s\n", pcd_file.c_str ());
00075   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
00076   pcl::PCLPointCloud2 cloud2;
00077 
00078   if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
00079     throw std::runtime_error ("  PCD file not found.");
00080 
00081   fromPCLPointCloud2 (cloud2, *cloud);
00082 
00083   // convert to NURBS data structure
00084   pcl::on_nurbs::NurbsDataCurve data;
00085   PointCloud2Vector2d (cloud, data.interior);
00086 
00087   viewer.setSize (800, 600);
00088   viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");
00089 
00090   // #################### CURVE PARAMETERS #########################
00091   unsigned order (3);
00092   unsigned n_control_points (20);
00093 
00094   pcl::on_nurbs::FittingCurve::Parameter curve_params;
00095   curve_params.smoothness = 0.000001;
00096 
00097   // #################### CURVE FITTING #########################
00098   ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve::initNurbsCurvePCA (order, data.interior, n_control_points);
00099 
00100   pcl::on_nurbs::FittingCurve fit (&data, curve);
00101   fit.assemble (curve_params);
00102   fit.solve ();
00103 
00104   // visualize
00105   VisualizeCurve (fit.m_nurbs, 1.0, 0.0, 0.0, false);
00106   viewer.spin ();
00107 
00108   return 0;
00109 }
00110 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:36