int main (int argc, char *argv[]) { std::string pcd_file; if (argc > 1) { pcd_file = argv[1]; } else { printf ("\nUsage: pcl_example_nurbs_fitting_curve pcd-file \n\n"); printf (" pcd-file point-cloud file\n"); exit (0); } // #################### LOAD FILE ######################### printf (" loading %s\n", pcd_file.c_str ()); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PCLPointCloud2 cloud2; if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1) throw std::runtime_error (" PCD file not found."); fromPCLPointCloud2 (cloud2, *cloud); // convert to NURBS data structure pcl::on_nurbs::NurbsDataCurve2d data; PointCloud2Vector2d (cloud, data.interior); viewer.setSize (800, 600); viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud"); // #################### CURVE PARAMETERS ######################### unsigned order (3); unsigned n_control_points (10); pcl::on_nurbs::FittingCurve2d::Parameter curve_params; curve_params.smoothness = 0.000001; curve_params.rScale = 1.0; // #################### CURVE FITTING ######################### ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2d::initNurbsPCA (order, &data, n_control_points); pcl::on_nurbs::FittingCurve2d fit (&data, curve); fit.assemble (curve_params); Eigen::Vector2d fix1 (0.1, 0.1); Eigen::Vector2d fix2 (1.0, 0.0); // fit.addControlPointConstraint (0, fix1, 100.0); // fit.addControlPointConstraint (curve.CVCount () - 1, fix2, 100.0); fit.solve (); // visualize VisualizeCurve (fit.m_nurbs, 1.0, 0.0, 0.0, true); viewer.spin (); return 0; }
void compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float th_dd, int max_search) { CloudPtr cloud (new Cloud); fromROSMsg (*input, *cloud); pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>); pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne; ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); ne.setNormalSmoothingSize (10.0f); ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR); ne.setInputCloud (cloud); ne.compute (*normal); TicToc tt; tt.tic (); //OrganizedEdgeBase<PointXYZRGBA, Label> oed; //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed; //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed; OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed; oed.setInputNormals (normal); oed.setInputCloud (cloud); oed.setDepthDisconThreshold (th_dd); oed.setMaxSearchNeighbors (max_search); oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY); PointCloud<Label> labels; std::vector<PointIndices> label_indices; oed.compute (labels, label_indices); print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n"); // Make gray point clouds for (int idx = 0; idx < (int)cloud->points.size (); idx++) { uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3; cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray; } // Display edges in PCLVisualizer viewer.setSize (640, 480); viewer.addCoordinateSystem (0.2f); viewer.addPointCloud (cloud, "original point cloud"); viewer.registerKeyboardCallback(&keyboard_callback); pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>); pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges); pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges); pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges); pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges); pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges); const int point_size = 2; viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges"); viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges"); viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges"); while (!viewer.wasStopped ()) { viewer.spinOnce (); pcl_sleep(0.1); } // Combine point clouds and edge labels sensor_msgs::PointCloud2 output_edges; toROSMsg (labels, output_edges); concatenateFields (*input, output_edges, output); }
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; }