C++ TicToc::tic方法代码示例

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, PolygonMesh &output,
         int hoppe_or_rbf, float iso_level, int grid_res, float extend_percentage, float off_surface_displacement)
{
  PointCloud<PointNormal>::Ptr xyz_cloud (new pcl::PointCloud<PointNormal> ());
  fromROSMsg (*input, *xyz_cloud);

  MarchingCubes<PointNormal> *mc;
  if (hoppe_or_rbf == 0)
    mc = new MarchingCubesHoppe<PointNormal> ();
  else
  {
    mc = new MarchingCubesRBF<PointNormal> ();
    ((MarchingCubesRBF<PointNormal>*) mc)->setOffSurfaceDisplacement (off_surface_displacement);
  }

  mc->setIsoLevel (iso_level);
  mc->setGridResolution (grid_res, grid_res, grid_res);
  mc->setPercentageExtendGrid (extend_percentage);
  mc->setInputCloud (xyz_cloud);

  TicToc tt;
  tt.tic ();

  print_highlight ("Computing ");
  mc->reconstruct (output);
  delete mc;

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}

void
compute (ConstCloudPtr &input, Cloud &output, int max_window_size, float slope, float max_distance, float initial_distance, float cell_size, float base, bool exponential)
{
  // Estimate
  TicToc tt;
  tt.tic ();

  print_highlight (stderr, "Computing ");

  std::vector<int> ground;

  ProgressiveMorphologicalFilter<PointType> pmf;
  pmf.setInputCloud (input);
  pmf.setMaxWindowSize (max_window_size);
  pmf.setSlope (slope);
  pmf.setMaxDistance (max_distance);
  pmf.setInitialDistance (initial_distance);
  pmf.setCellSize (cell_size);
  pmf.setBase (base);
  pmf.setExponential (exponential);
  pmf.extract (ground);

  PointIndicesPtr idx (new PointIndices);
  idx->indices = ground;

  ExtractIndices<PointType> extract;
  extract.setInputCloud (input);
  extract.setIndices (idx);
  extract.setNegative (false);
  extract.filter (output);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

void
compute (const CloudT::Ptr &input, std::vector<FeatureT::Ptr> &trained_features,
         CloudLT::Ptr &out,
         float normal_radius_search,
         float fpfh_radius_search,
         float feature_threshold)
{
  TicToc tt;
  tt.tic ();
  
  print_highlight ("Computing ");

  UnaryClassifier<PointT> classifier;
  classifier.setInputCloud (input);
  classifier.setTrainedFeatures (trained_features);
  classifier.setNormalRadiusSearch (normal_radius_search);
  classifier.setFPFHRadiusSearch (fpfh_radius_search);
  classifier.setFeatureThreshold (feature_threshold);

  classifier.segment (out);

  print_info ("[done, "); 
  print_value ("%g", tt.toc ()); 
  print_info (" ms : "); print_value ("%d", out->width * out->height); 
  print_info (" points]\n");

}

void
saveImage (const std::string &filename, const pcl::PCLImage& image)
{
  TicToc tt;
  tt.tic ();
  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  savePNGFile (filename, image);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", image.width * image.height); print_info (" points]\n");
}

int main(int argc, char *argv[])
{
	if (argc < 2)
	{
		print_help(argv);
		return -1;
	}

	// Parse the command line arguments for .pcd files
	vector<int> txt_file_indices;
	txt_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
	if (txt_file_indices.size () != 1)
	{
		print_error("Need one input PCD file to continue.\n");
		return -1;
	}

	string inputFileName = string(argv[txt_file_indices[0]]);
	size_t ext = inputFileName.find(".pcd");
	string outputFileName = inputFileName.substr(0, ext) + string(".vtk");

	PointCloud<PointNormal>::Ptr cloud_with_normals(new PointCloud<PointNormal>);
	loadPCDFile<PointNormal>(argv[1], *cloud_with_normals);

	search::KdTree<PointNormal>::Ptr tree2 (new search::KdTree<PointNormal>);
  	tree2->setInputCloud (cloud_with_normals);

	// Initialize objects
  	GreedyProjectionTriangulation<PointNormal> gp3;
  	PolygonMesh triangles;

  	// Set the maximum distance between connected points (maximum edge length)
  	gp3.setSearchRadius (0.025);

  	// Set typical values for the parameters
  	gp3.setMu (2.5);
  	gp3.setMaximumNearestNeighbors(500);
  	gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
  	gp3.setMinimumAngle(M_PI/18); // 10 degrees
  	gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
  	gp3.setNormalConsistency(false);

  	// Get result of triangulation
  	gp3.setInputCloud (cloud_with_normals);
  	gp3.setSearchMethod (tree2);

  	TicToc tt;
  	tt.tic();
  	print_highlight("Computing GreedyProjectionTriangulation ");
  	gp3.reconstruct (triangles);
  	print_info("[done, "); print_value("%g", tt.toc()); print_info(" ms]\n");
	
	saveVTKFile (outputFileName, triangles);
	print_highlight(string(string("Saving ") + outputFileName + string("\n")).c_str());

	return 0;
}

void
computeFeatureViaNormals (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
                          int argc, char** argv, bool set_search_flag = true)
{
    int n_k = default_n_k;
    int f_k = default_f_k;
    double n_radius = default_n_radius;
    double f_radius = default_f_radius;
    parse_argument (argc, argv, "-n_k", n_k);
    parse_argument (argc, argv, "-n_radius", n_radius);
    parse_argument (argc, argv, "-f_k", f_k);
    parse_argument (argc, argv, "-f_radius", f_radius);

    // Convert data to PointCloud<PointIn>
    typename PointCloud<PointIn>::Ptr xyz (new PointCloud<PointIn>);
    fromPCLPointCloud2 (*input, *xyz);

    // Estimate
    TicToc tt;
    tt.tic ();

    print_highlight (stderr, "Computing ");

    NormalEstimation<PointIn, NormalT> ne;
    ne.setInputCloud (xyz);
    ne.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>));
    ne.setKSearch (n_k);
    ne.setRadiusSearch (n_radius);

    typename PointCloud<NormalT>::Ptr normals = typename PointCloud<NormalT>::Ptr (new PointCloud<NormalT>);
    ne.compute (*normals);

    FeatureAlgorithm feature_est;
    feature_est.setInputCloud (xyz);
    feature_est.setInputNormals (normals);

    feature_est.setSearchMethod (typename pcl::search::KdTree<PointIn>::Ptr (new pcl::search::KdTree<PointIn>));

    PointCloud<PointOut> output_features;

    if (set_search_flag) {
        feature_est.setKSearch (f_k);
        feature_est.setRadiusSearch (f_radius);
    }

    feature_est.compute (output_features);

    print_info ("[done, ");
    print_value ("%g", tt.toc ());
    print_info (" ms : ");
    print_value ("%d", output.width * output.height);
    print_info (" points]\n");

    // Convert data back
    toPCLPointCloud2 (output_features, output);
}

template<typename PointInT> void
saveCloud(const std::string &filename, const PointCloud<PointInT> &cloud, bool format)
{
	TicToc tt;
	tt.tic();

	print_highlight("Saving "); print_value("%s ", filename.c_str());
	savePCDFile(filename, cloud, format);
	print_info("[done, "); print_value("%g", tt.toc()); print_info(" ms : "); print_value("%d", cloud.width * cloud.height); print_info(" points]\n");
}

void
compute (const pcl::PCLPointCloud2::ConstPtr &input,
         pcl::PCLPointCloud2 &output,
         double search_radius,
         bool sqr_gauss_param_set,
         double sqr_gauss_param,
         int polynomial_order)
{

  PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
      xyz_cloud (new pcl::PointCloud<PointXYZ> ());
  fromPCLPointCloud2 (*input, *xyz_cloud_pre);

  // Filter the NaNs from the cloud
  for (size_t i = 0; i < xyz_cloud_pre->size (); ++i)
    if (pcl_isfinite (xyz_cloud_pre->points[i].x))
      xyz_cloud->push_back (xyz_cloud_pre->points[i]);
  xyz_cloud->header = xyz_cloud_pre->header;
  xyz_cloud->height = 1;
  xyz_cloud->width = static_cast<uint32_t> (xyz_cloud->size ());
  xyz_cloud->is_dense = false;
  
  

  PointCloud<PointNormal>::Ptr xyz_cloud_smoothed (new PointCloud<PointNormal> ());

  MovingLeastSquares<PointXYZ, PointNormal> mls;
  mls.setInputCloud (xyz_cloud);
  mls.setSearchRadius (search_radius);
  if (sqr_gauss_param_set) mls.setSqrGaussParam (sqr_gauss_param);
  mls.setPolynomialOrder (polynomial_order);

//  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::SAMPLE_LOCAL_PLANE);
//  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::RANDOM_UNIFORM_DENSITY);
//  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
  mls.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::NONE);
  mls.setPointDensity (60000 * int (search_radius)); // 300 points in a 5 cm radius
  mls.setUpsamplingRadius (0.025);
  mls.setUpsamplingStepSize (0.015);
  mls.setDilationIterations (2);
  mls.setDilationVoxelSize (0.01f);

  search::KdTree<PointXYZ>::Ptr tree (new search::KdTree<PointXYZ> ());
  mls.setSearchMethod (tree);
  mls.setComputeNormals (true);

  PCL_INFO ("Computing smoothed surface and normals with search_radius %f , sqr_gaussian_param %f, polynomial order %d\n",
            mls.getSearchRadius(), mls.getSqrGaussParam(), mls.getPolynomialOrder());
  TicToc tt;
  tt.tic ();
  mls.process (*xyz_cloud_smoothed);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_smoothed->width * xyz_cloud_smoothed->height); print_info (" points]\n");

  toPCLPointCloud2 (*xyz_cloud_smoothed, output);
}

void
saveCloud (const std::string &filename, const PolygonMesh &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  saveVTKFile (filename, output);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  
  pcl::io::savePCDFile (filename, output, translation, orientation, false);
  
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  
  pcl::io::savePCDFile (filename, output, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true); // Save as binary
  
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &cloud)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  
  saveVTKFile (filename, cloud);
  
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
}

void
compute (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output,
         Eigen::Matrix4f &tform)
{
  TicToc tt;
  tt.tic ();
  
  print_highlight ("Transforming ");

  transformPointCloud2 (*input, output, tform);

  printElapsedTimeAndNumberOfPoints (tt.toc (), output.width, output.height);
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &cloud, bool binary, bool use_camera)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  
  pcl::PLYWriter writer;
  writer.write (filename, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), binary, use_camera);
  
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
}

bool
loadCloud (const std::string &filename, CloudT::Ptr &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, *cloud) < 0)
    return (false);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->width * cloud->height); print_info (" points]\n");

  return (true);
}