C++ TicToc类代码示例

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

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);

  printElapsedTimeAndNumberOfPoints (tt.toc (), cloud.width, cloud.height);

  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         std::string method,
         int min_pts, double radius,
         int mean_k, double std_dev_mul, bool negative)
{

  PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
      xyz_cloud (new pcl::PointCloud<PointXYZ> ());
  fromROSMsg (*input, *xyz_cloud_pre);
  
  std::vector<int> index_vector;
  removeNaNFromPointCloud<PointXYZ> (*xyz_cloud_pre, *xyz_cloud, index_vector);

      
  TicToc tt;
  tt.tic ();
  PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
  if (method == "statistical")
  {
    StatisticalOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setMeanK (mean_k);
    filter.setStddevMulThresh (std_dev_mul);
    filter.setNegative (negative);
    PCL_INFO ("Computing filtered cloud with mean_k %d, std_dev_mul %f, inliers %d\n", filter.getMeanK (), filter.getStddevMulThresh (), filter.getNegative ());
    filter.filter (*xyz_cloud_filtered);
  }
  else if (method == "radius")
  {
    RadiusOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setRadiusSearch (radius);
    filter.setMinNeighborsInRadius (min_pts);
    PCL_INFO ("Computing filtered cloud with radius %f, min_pts %d\n", radius, min_pts);
    filter.filter (*xyz_cloud_filtered);
  }
  else
  {
    PCL_ERROR ("%s is not a valid filter name! Quitting!\n", method.c_str ());
    return;
  }
    
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_filtered->width * xyz_cloud_filtered->height); print_info (" points]\n");

  toROSMsg (*xyz_cloud_filtered, output);
}

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

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

    pcl::io::savePCDFile (filename, output);

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

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

  tt.tic ();
  if (pcl::io::load (filename, cloud)) {
    print_error ("Cannot found input file name (%s).\n", filename.c_str ());
    return (false);
  }
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : ");
  print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
  print_info ("Available dimensions: "); print_value ("%s\n", getFieldsList (cloud).c_str ());

  return (true);
}

bool
loadPCLZF (const std::string &filename_depth,
           const std::string &filename_params,
           pcl::PointCloud<pcl::PointXYZ> &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename_depth.c_str ());
  tt.tic ();

  pcl::io::LZFDepth16ImageReader depth;
  depth.readParameters (filename_params);
  depth.read (filename_depth, cloud);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         float leaf_x, float leaf_y, float leaf_z, const std::string &field, double fmin, double fmax)
{
  TicToc tt;
  tt.tic ();
  
  print_highlight ("Computing ");

  VoxelGrid<sensor_msgs::PointCloud2> grid;
  grid.setInputCloud (input);
  grid.setFilterFieldName (field);
  grid.setFilterLimits (fmin, fmax);
  grid.setLeafSize (leaf_x, leaf_y, leaf_z);
  grid.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
project (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output, float a, float b, float c, float d)
{
  Eigen::Vector4f coeffs;
  coeffs << a, b, c, d;

  // Convert data to PointCloud<T>
  PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
  fromROSMsg (*input, *xyz);

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

  //First, we'll find a point on the plane
  print_highlight (stderr, "Projecting ");

  PointCloud<PointXYZ>::Ptr projected_cloud_pcl (new PointCloud<PointXYZ>);
  projected_cloud_pcl->width = xyz->width;
  projected_cloud_pcl->height = xyz->height;
  projected_cloud_pcl->is_dense = xyz->is_dense;
  projected_cloud_pcl->sensor_origin_ = xyz->sensor_origin_;
  projected_cloud_pcl->sensor_orientation_ = xyz->sensor_orientation_;

  for(size_t i = 0; i < xyz->points.size(); ++i)
  {
    pcl::PointXYZ projection;
    pcl::projectPoint<PointXYZ> (xyz->points[i], coeffs, projection);
    projected_cloud_pcl->points.push_back(projection);
  }


  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : ");
  pcl::io::savePCDFile ("foo.pcd", *projected_cloud_pcl);

  // Convert data back
  sensor_msgs::PointCloud2 projected_cloud;
  toROSMsg (*projected_cloud_pcl, projected_cloud);

  //we can actually use concatenate fields to inject our projection into the
  //output, the second argument overwrites the first's fields for those that
  //are shared
  concatenateFields (*input, projected_cloud, output);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         std::string field_name, float min, float max, bool inside)
{
  // Estimate
  TicToc tt;
  tt.tic ();

  print_highlight (stderr, "Computing ");

  PassThrough<sensor_msgs::PointCloud2> passthrough_filter;
  passthrough_filter.setInputCloud (input);
  passthrough_filter.setFilterFieldName (field_name);
  passthrough_filter.setFilterLimits (min, max);
  passthrough_filter.setFilterLimitsNegative (!inside);
  passthrough_filter.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
saveCloud (const std::string &filename, const std::vector<sensor_msgs::PointCloud2::Ptr> &output)
{
  TicToc tt;
  tt.tic ();

  std::string basename = filename.substr (0, filename.length () - 4);

  for (size_t i = 0; i < output.size (); i++)
  {
    std::string clustername = basename + boost::lexical_cast<std::string>(i) + ".pcd";
    print_highlight ("Saving "); print_value ("%s ", clustername.c_str ());

    pcl::io::savePCDFile (clustername, *(output[i]), translation, orientation, false);

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

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

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

    PCDWriter w;
    //w.writeBinaryCompressed (filename, output, translation, orientation);
    w.writeASCII (filename, output, translation, orientation);

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

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

  tt.tic ();
  if (loadPCDFile (filename, cloud, translation, orientation) < 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");
  print_info ("Available dimensions: "); print_value ("%s\n", getFieldsList (cloud).c_str ());

  // Check if the dataset has normals
  if (getFieldIndex (cloud, "normal_x") == -1)
  {
    print_error ("The input dataset does not contain normal information!\n");
    return (false);
  }
  return (true);
}

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

    tt.tic ();
    if (loadPCDFile (filename, cloud, translation, orientation) < 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");
    print_info ("Available dimensions: ");
    print_value ("%s\n", getFieldsList (cloud).c_str ());

    return (true);
}

void
transform (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output)
{
  // Check for 'normals'
  bool has_normals = false;
  for (size_t i = 0; i < input->fields.size (); ++i)
    if (input->fields[i].name == "normals")
      has_normals = true;

  // Estimate
  TicToc tt;
  tt.tic ();
  print_highlight (stderr, "Transforming ");

  // Convert data to PointCloud<T>
  if (has_normals)
  {
    PointCloud<PointNormal> xyznormals;
    fromROSMsg (*input, xyznormals);
    pcl::transformPointCloud<PointNormal> (xyznormals, xyznormals, translation.head<3> (), orientation);
    // Copy back the xyz and normals
    sensor_msgs::PointCloud2 output_xyznormals;
    toROSMsg (xyznormals, output_xyznormals);
    concatenateFields (*input, output_xyznormals, output);
  }
  else
  {
    PointCloud<PointXYZ> xyz;
    fromROSMsg (*input, xyz);
    pcl::transformPointCloud<PointXYZ> (xyz, xyz, translation.head<3> (), orientation);
    // Copy back the xyz and normals
    sensor_msgs::PointCloud2 output_xyz;
    toROSMsg (xyz, output_xyz);
    concatenateFields (*input, output_xyz, output);
  }

  translation = Eigen::Vector4f::Zero ();
  orientation = Eigen::Quaternionf::Identity ();

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

int main(int argc, char * argv[])
{
  double portion = 0.9;
  uint64 memory_size = getMemorySize() * portion;
  uint64 page_size = sysconf(_SC_PAGESIZE);
  uint64 page_nums = memory_size/page_size;
  // Need to Configure Parse
  bool debug = true;

  if(debug)
  {
    printf("*--------------------------*\nMemory Information... \n*--------------------------*\n");
    printf("Memory Size (bytes): %lu | ",memory_size);
    printf("Page Size (bytes): %lu | ",page_size);
    printf("Number of Pages: %lu \n",page_nums);
    printf("Note: We allocte %f of the memory \n",portion*100);
  }

  // Allocate memory
  Byte * memory = (Byte * ) malloc(memory_size);

  double iterations = 10;
  double total_mem_time = 0;
  double total_alligned_page = 0;
  double total_unalligned_page =0;

  for(double j =0 ; j < iterations; j++)
  {
      TicToc timer;
      timer.tic();
      memset(memory,0,memory_size);
      total_mem_time+= (timer.toc()/1000000);

      timer.tic();
      for(uint64 i =0 ; i < page_nums; i++ )
      {
         memset(memory + i* page_size,i,page_size);
      }
      total_alligned_page+= (timer.toc()/page_size);

      timer.tic();
      for(uint64 i =0 ; i < page_nums - 1 ; i++ )
      {
         memset(memory + i * page_size + 2,i,page_size);
      }
      total_unalligned_page+=(timer.toc()/(page_size - 1));

  }
  printf("*--------------------------*\nBenchmarking Starting... \n*--------------------------*\n");
  printf("Memory Access Time: %f (secs)\n",total_mem_time/iterations);
  printf("Alligned Page Access Time: %f (usec)\n",total_alligned_page/iterations);
  printf("Unalligned Page Access Time: %f (usec)\n",total_unalligned_page/iterations);

  // Free allocated memory
  free(memory);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, std::vector<sensor_msgs::PointCloud2::Ptr> &output,
         int min, int max, double tolerance)
{
  // Convert data to PointCloud<T>
  PointCloud<pcl::PointXYZ>::Ptr xyz (new PointCloud<pcl::PointXYZ>);
  fromROSMsg (*input, *xyz);

  // Estimate
  TicToc tt;
  tt.tic ();
  
  print_highlight (stderr, "Computing ");

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (xyz);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (tolerance);
  ec.setMinClusterSize (min);
  ec.setMaxClusterSize (max);
  ec.setSearchMethod (tree);
  ec.setInputCloud (xyz);
  ec.extract (cluster_indices);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cluster_indices.size ()); print_info (" clusters]\n");

  output.reserve (cluster_indices.size ());
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    pcl::ExtractIndices<sensor_msgs::PointCloud2> extract;
    extract.setInputCloud (input);
    extract.setIndices (boost::make_shared<const pcl::PointIndices> (*it));
    sensor_msgs::PointCloud2::Ptr out (new sensor_msgs::PointCloud2);
    extract.filter (*out);
    output.push_back (out);
  }
}