C++ PCLVisualizer::setSize方法代码示例

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;
}