C++ PointCloudPtr类代码示例

TEST (GraphCommon, ComputeSignedCurvature)
{
  const size_t ROWS = 5;
  const size_t COLS = 12;
  const float STEP = 1.0;
  // Create a "staircase" cloud
  //
  //     top view            side view
  //
  //   * * * * * * * *           * * * * *
  //   * * * * * * * *           *
  //   * * * * * * * *           *
  //   * * * * * * * *           *
  //   * * * * * * * *     * * * *
  //
  PointCloudPtr cloud (new PointCloud (ROWS * COLS, 1));
  size_t idx = 0;
  for (size_t y = 0; y < ROWS; ++y)
  {
    for (size_t x = 0; x < COLS; ++x, ++idx)
    {
      cloud->at (idx).y = y * STEP;
      if (x < COLS / 3)
      {
        cloud->at (idx).x = x * STEP;
        cloud->at (idx).z = -5.0f;
      }
      else if (x < 2 * COLS / 3)
      {
        cloud->at (idx).x = (COLS / 3 - 1) * STEP;
        cloud->at (idx).z = (x - COLS / 3 + 1) * STEP - 5.0f;
      }
      else
      {
        cloud->at (idx).x = (x - COLS / 3) * STEP;
        cloud->at (idx).z = (COLS / 3) * STEP - 5.0f;
      }
    }
  }

  pcl::graph::NearestNeighborsGraphBuilder<PointT, Graph> gb (6);
  gb.setInputCloud (cloud);
  Graph graph;
  gb.compute (graph);
  pcl::graph::computeNormalsAndCurvatures (graph);

  // Expect that curvatures are all non-negative
  for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
    EXPECT_LE (0.0f, graph[i].curvature);

  pcl::graph::computeSignedCurvatures (graph);

  // Expect curvatures of the points on the concave corner to be less than zero,
  // and on the convex corner the other way round.
  for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
    if (i % COLS == 3)
      EXPECT_GT (0.0f, graph[i].curvature);
    else if (i % COLS == 7)
      EXPECT_LT (0.0f, graph[i].curvature);
}

//if a point is in a cloud
bool isPointInCloud(Point pt, PointCloudPtr cloud){
  for(int i=0;i<cloud->size();i++){
    if(pt.x==cloud->at(i).x&&pt.y==cloud->at(i).y&&pt.z==cloud->at(i).z){
      return true;
    }
  }

  return false;
}

void removeOutlier(
  PointCloudPtr cloud,
  PointCloudPtr cloud_output,
  int K,
  float distanceThreshold
)
{
  int size = cloud->width * cloud->height;

  pcl::KdTreeFLANN<PointT> kdtree;
  kdtree.setInputCloud(cloud);
  
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);
  std::vector<int> chosenIndices;

  for (int i=0; i<size; ++i)
  {
    PointT point = cloud->points[i];
    float avgKDis = averageKDistance(kdtree, pointIdxNKNSearch, pointNKNSquaredDistance, point, K);
    if (avgKDis<distanceThreshold) chosenIndices.push_back(i);    
  } 
  
  cloud_output->width = chosenIndices.size();
  cloud_output->height = 1;
  cloud_output->resize(chosenIndices.size());
  for (int i=0; i<chosenIndices.size(); ++i) cloud_output->points[i]=cloud->points[chosenIndices[i]];
 }

template<typename PointInT> void
pcl::nurbs::NurbsFitter<PointInT>::setInputCloud (PointCloudPtr &pcl_cloud)
{
  if (pcl_cloud.get () == 0 || pcl_cloud->points.size () == 0)
    throw std::runtime_error ("[NurbsFitter::setInputCloud] Error: Empty or invalid pcl-point-cloud.\n");

  cloud_ = pcl_cloud;
  have_cloud_ = true;
}

template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg)
{
  assert (cloud_arg==input_);

  cloud_arg->push_back (point_arg);

  this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
}

template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
                                                           IndicesPtr indices_arg)
{
  assert (cloud_arg==input_);
  assert (indices_arg==indices_);

  cloud_arg->push_back (point_arg);

  this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
}

//find nearest neighbor
bool findNearestNeighbor(PointCloudPtr cloud, PointCloudPtr except_cloud, Point search_pt, Point& finded_pt){
  int min=10;
  bool flag=false;

  for(int i=0;i<cloud->size();i++){
    int dis=sqrt(pow(search_pt.x-cloud->at(i).x, 2)+pow(search_pt.y-cloud->at(i).y, 2)+pow(search_pt.z-cloud->at(i).z, 2));
    if(dis<min){
      min=dis;
      finded_pt.x=cloud->at(i).x;
      finded_pt.y=cloud->at(i).y;
      finded_pt.z=cloud->at(i).z;

      if(!isPointInCloud(finded_pt, except_cloud)){
        flag=true;
      }
    }
  }

  return flag;
}

void ppfmap::CudaPPFMatch<PointT, NormalT>::detect(
    const PointCloudPtr cloud, 
    const NormalsPtr normals, 
    std::vector<Pose>& poses) {

    float affine_s[12];
    std::vector<Pose> pose_vector;
    const float radius = map->getCloudDiameter() * neighborhood_percentage;

    std::vector<int> indices;
    std::vector<float> distances;

    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(cloud);

    if (!use_indices) {
        ref_point_indices->resize(cloud->size());
        for (int i = 0; i < cloud->size(); i++) {
            (*ref_point_indices)[i] = i;
        }
    }

    poses.clear();
    for (const auto index : *ref_point_indices) {
        const auto& point = cloud->at(index);
        const auto& normal = normals->at(index);

        if (!pcl::isFinite(point)) continue;

        getAlignmentToX(point, normal, &affine_s);
        kdtree.radiusSearch(point, radius, indices, distances);

        poses.push_back(getPose(index, indices, cloud, normals, affine_s));
    }

    sort(poses.begin(), poses.end(), 
         [](const Pose& a, const Pose& b) -> bool { 
             return a.votes > b.votes; 
         });
}

template<typename PointInT> std::size_t
pcl::nurbs::NurbsFitter<PointInT>::PCL2Eigen (PointCloudPtr &pcl_cloud, const std::vector<int> &indices, vector_vec3 &on_cloud)
{
  std::size_t numPoints (0);

  for (unsigned i = 0; i < indices.size (); i++)
  {
    const PointInT &pt = pcl_cloud->at (indices[i]);

    if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
    {
      on_cloud.push_back (vec3 (pt.x, pt.y, pt.z));
      ++numPoints;
    }
  }

  return (numPoints);
}

int 
main (int argc, char ** argv)
{
  if (argc < 2) 
  {
    pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
    pcl::console::print_info ("  where options are:\n");
    pcl::console::print_info ("    -n radius  ...................................... Estimate surface normals\n");
    pcl::console::print_info ("    -k min_scale,nr_octaves,nr_scales,min_contrast... Detect keypoints\n");
    pcl::console::print_info ("    -l radius ....................................... Compute local descriptors\n");
    pcl::console::print_info ("    -s output_name (without .pcd extension).......... Save outputs\n");
    pcl::console::print_info ("Note: \n");
    pcl::console::print_info ("  Each of the first four options depends on the options above it.\n");
    pcl::console::print_info ("  Saving (-s) will output individual files for each option used (-n,-k,-f,-g).\n");
    return (1);
  }
  

  // Load the input file
  PointCloudPtr cloud (new PointCloud);
  pcl::io::loadPCDFile (argv[1], *cloud);
  pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ());
  
  // Estimate surface normals
  SurfaceNormalsPtr normals;
  double surface_radius;
  bool estimate_surface_normals = pcl::console::parse_argument (argc, argv, "-n", surface_radius) > 0;
  
  if (estimate_surface_normals)
  {  
    normals = estimateSurfaceNormals (cloud, surface_radius);
    pcl::console::print_info ("Estimated surface normals\n");
  }
  
  // Detect keypoints
  PointCloudPtr keypoints;
  std::string params_string;
  bool detect_keypoints = pcl::console::parse_argument (argc, argv, "-k", params_string) > 0;
  if (detect_keypoints)
  {
    assert (normals);
    std::vector<std::string> tokens;
    boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
    assert (tokens.size () == 4);    
    float min_scale = atof(tokens[0].c_str ());
    int nr_octaves = atoi(tokens[1].c_str ());
    int nr_scales = atoi(tokens[2].c_str ());
    float min_contrast = atof(tokens[3].c_str ());
    keypoints = detectKeypoints (cloud, normals, min_scale, nr_octaves, nr_scales, min_contrast);
    pcl::console::print_info ("Detected %lu keypoints\n", keypoints->size ());
  }
  
  // Compute local descriptors
  LocalDescriptorsPtr local_descriptors;
  double feature_radius;
  bool compute_local_descriptors = pcl::console::parse_argument (argc, argv, "-l", feature_radius) > 0;
  if (compute_local_descriptors)
  {
    assert (normals && keypoints);
    local_descriptors = computeLocalDescriptors (cloud, normals, keypoints, feature_radius);
    pcl::console::print_info ("Computed local descriptors\n");
  }

  // Compute global descriptor
  GlobalDescriptorsPtr global_descriptor;
  if (normals)
  {
    global_descriptor = computeGlobalDescriptor (cloud, normals);
    pcl::console::print_info ("Computed global descriptor\n");
  }

  // Save output
  std::string base_filename, output_filename;
  bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", base_filename) > 0;
  if (save_cloud)
  {
    if (normals)
    {
      output_filename = base_filename;
      output_filename.append ("_normals.pcd");
      pcl::io::savePCDFile (output_filename, *normals);
      pcl::console::print_info ("Saved surface normals as %s\n", output_filename.c_str ());
    }
    if (keypoints)
    {
      output_filename = base_filename;
      output_filename.append ("_keypoints.pcd");
      pcl::io::savePCDFile (output_filename, *keypoints);
      pcl::console::print_info ("Saved keypoints as %s\n", output_filename.c_str ());
    }
    if (local_descriptors)
    {
      output_filename = base_filename;
      output_filename.append ("_localdesc.pcd");
      pcl::io::savePCDFile (output_filename, *local_descriptors);
      pcl::console::print_info ("Saved local descriptors as %s\n", output_filename.c_str ());
    }
    if (global_descriptor)
    {
      output_filename = base_filename;
//.........这里部分代码省略.........

int 
main (int argc, char ** argv)
{
  if (argc < 2) 
  {
    pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
    pcl::console::print_info ("  where options are:\n");
    pcl::console::print_info ("    -p dist_threshold,max_iters  ..... Subtract the dominant plane\n");
    pcl::console::print_info ("    -c tolerance,min_size,max_size ... Cluster points\n");
    pcl::console::print_info ("    -s output.pcd .................... Save the largest cluster\n");
    return (1);
  }

  // Load the input file
  PointCloudPtr cloud (new PointCloud);
  pcl::io::loadPCDFile (argv[1], *cloud);
  pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());

  // Subtract the dominant plane
  double dist_threshold, max_iters;
  bool subtract_plane = pcl::console::parse_2x_arguments (argc, argv, "-p", dist_threshold, max_iters) > 0;
  if (subtract_plane)
  {
    size_t n = cloud->size ();
    cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters);
    pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ());
  }

  // Cluster points
  double tolerance, min_size, max_size;
  std::vector<pcl::PointIndices> cluster_indices;
  bool cluster_points = pcl::console::parse_3x_arguments (argc, argv, "-c", tolerance, min_size, max_size) > 0;
  if (cluster_points)
  {
    clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices);
    pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ());
  }

  // Save output
  std::string output_filename;
  bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
  if (save_cloud)
  {
    // If clustering was performed, save only the first (i.e., largest) cluster
    if (cluster_points)
    {
      PointCloudPtr temp_cloud (new PointCloud);
      pcl::copyPointCloud (*cloud, cluster_indices[0], *temp_cloud);
      cloud = temp_cloud;
    }
    pcl::console::print_info ("Saving result as %s...\n", output_filename.c_str ());
    pcl::io::savePCDFile (output_filename, *cloud);
  }
  // Or visualize the result
  else
  {
    pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
    pcl::visualization::PCLVisualizer vis;

    // If clustering was performed, display each cluster with a random color
    if (cluster_points)
    {
      for (size_t i = 0; i < cluster_indices.size (); ++i)
      {
        // Extract the i_th cluster into a new cloud
        pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_i (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud (*cloud, cluster_indices[i], *cluster_i);

        // Create a random color
        pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_color (cluster_i);

        // Create a unique identifier
        std::stringstream cluster_id ("cluster");
        cluster_id << i;

        // Add the i_th cluster to the visualizer with a random color and a unique identifier
        vis.addPointCloud<pcl::PointXYZ> (cluster_i, random_color, cluster_id.str ());
      }
    }
    else
    {
      // If clustering wasn't performed, just display the cloud
      vis.addPointCloud (cloud);
    }
    vis.resetCamera ();
    vis.spin ();
  }

  return (0);
}

int 
main (int argc, char ** argv)
{
  if (argc < 3) 
  {
    pcl::console::print_info ("Syntax is: %s query.pcd <options>\n", argv[0]);
    pcl::console::print_info ("  where options are:\n");
    pcl::console::print_info ("    --objects_root_path root_path ......... Root path to append to files in object_files.txt \n");
    pcl::console::print_info ("    --objects object_files.txt ......... A list of the files to use as exemplars\n");
    pcl::console::print_info ("    --filter parameters.txt ............ Threshold, downsample, and denoise\n");
    pcl::console::print_info ("    --segment parameters.txt  .......... Remove dominant plane and cluster\n");
    pcl::console::print_info ("    --feature parameters.txt ........... Compute normals, keypoints, and descriptors\n");
    pcl::console::print_info ("    --registration parameters.txt ...... Align best fitting model to query\n");
    pcl::console::print_info ("  and where the parameters files must contain the following values (one per line):\n");
    pcl::console::print_info ("    filter parameters:\n");
    pcl::console::print_info ("      * min_depth\n");
    pcl::console::print_info ("      * max_depth\n");
    pcl::console::print_info ("      * downsample_leaf_size\n");
    pcl::console::print_info ("      * outlier_rejection_radius\n");
    pcl::console::print_info ("      * outlier_rejection_min_neighbors\n");
    pcl::console::print_info ("    segmentation parameters:\n");
    pcl::console::print_info ("      * plane_inlier_distance_threshold\n");
    pcl::console::print_info ("      * max_ransac_iterations\n");
    pcl::console::print_info ("      * cluster_tolerance\n");
    pcl::console::print_info ("      * min_cluster_size\n");
    pcl::console::print_info ("      * max_cluster_size\n");
    pcl::console::print_info ("    feature estimation parameters:\n");
    pcl::console::print_info ("      * surface_normal_radius\n");
    pcl::console::print_info ("      * keypoints_min_scale\n");
    pcl::console::print_info ("      * keypoints_nr_octaves\n");
    pcl::console::print_info ("      * keypoints_nr_scales_per_octave\n");
    pcl::console::print_info ("      * keypoints_min_contrast\n");
    pcl::console::print_info ("      * local_descriptor_radius\n");
    pcl::console::print_info ("    registration parameters:\n");
    pcl::console::print_info ("      * initial_alignment_min_sample_distance\n");
    pcl::console::print_info ("      * initial_alignment_max_correspondence_distance\n");
    pcl::console::print_info ("      * initial_alignment_nr_iterations\n");
    pcl::console::print_info ("      * icp_max_correspondence_distance\n");
    pcl::console::print_info ("      * icp_rejection_threshold\n");
    pcl::console::print_info ("      * icp_transformation_epsilon\n");
    pcl::console::print_info ("      * icp_max_iterations\n");

    return (1);
  }

  // Load input file
  PointCloudPtr query (new PointCloud);
  pcl::io::loadPCDFile (argv[1], *query);
  pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], query->size ());    

  ifstream input_stream;
  ObjectRecognitionParameters params;

  // Parse the exemplar files
  std::string objects_root_path;
  pcl::console::parse_argument (argc, argv, "--objects_root_path", objects_root_path);

  std::string objects_file;
  pcl::console::parse_argument (argc, argv, "--objects", objects_file);
  std::vector<std::string> exemplar_filenames (0);
  input_stream.open (objects_file.c_str ());
  if (input_stream.is_open ())
  {
    while (input_stream.good ())
    {
      std::string filename;
      input_stream >> filename;
      if (filename.size () > 0)
      {
        exemplar_filenames.push_back (objects_root_path + "/" + filename);
      }
    }
    input_stream.close ();
  }
  
  //Parse filter parameters
  std::string filter_parameters_file;
  pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;    
  input_stream.open (filter_parameters_file.c_str ());
  if (input_stream.is_open())
  {
    input_stream >> params.min_depth;
    input_stream >> params.max_depth;
    input_stream >> params.downsample_leaf_size;
    input_stream >> params.outlier_rejection_radius;
    input_stream >> params.outlier_rejection_min_neighbors;
    input_stream.close ();
  }

int 
main (int argc, char ** argv)
{
  if (argc < 2) 
  {
    pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
    pcl::console::print_info ("  where options are:\n");
    pcl::console::print_info ("    -t min_depth,max_depth  ............... Threshold depth\n");
    pcl::console::print_info ("    -d leaf_size  ......................... Downsample\n");
    pcl::console::print_info ("    -r radius,min_neighbors ............... Radius outlier removal\n");
    pcl::console::print_info ("    -s output.pcd ......................... Save output\n");
    return (1);
  }

  // Load the input file
  PointCloudPtr cloud (new PointCloud);
  pcl::io::loadPCDFile (argv[1], *cloud);
  pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());

  // Threshold depth
  double min_depth, max_depth;
  bool threshold_depth = pcl::console::parse_2x_arguments (argc, argv, "-t", min_depth, max_depth) > 0;
  if (threshold_depth)
  {
    size_t n = cloud->size ();
    cloud = thresholdDepth (cloud, min_depth, max_depth);
    pcl::console::print_info ("Eliminated %zu points outside depth limits\n", n - cloud->size ());
  }

  // Downsample and threshold depth
  double leaf_size;
  bool downsample_cloud = pcl::console::parse_argument (argc, argv, "-d", leaf_size) > 0;
  if (downsample_cloud)
  {
    size_t n = cloud->size ();
    cloud = downsample (cloud, leaf_size);
    pcl::console::print_info ("Downsampled from %zu to %zu points\n", n, cloud->size ());
  }

  // Remove outliers
  double radius, min_neighbors;
  bool remove_outliers = pcl::console::parse_2x_arguments (argc, argv, "-r", radius, min_neighbors) > 0;
  if (remove_outliers)
  {
    size_t n = cloud->size ();
    cloud = removeOutliers (cloud, radius, (int)min_neighbors);
    pcl::console::print_info ("Removed %zu outliers\n", n - cloud->size ());
  }

  // Save output
  std::string output_filename;
  bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
  if (save_cloud)
  {
    pcl::io::savePCDFile (output_filename, *cloud);
    pcl::console::print_info ("Saved result as %s\n", output_filename.c_str ());
  }
  // Or visualize the result
  else
  {
    pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
    pcl::visualization::PCLVisualizer vis;
    vis.addPointCloud (cloud);
    vis.resetCamera ();
    vis.spin ();
  }

  return (0);
}

ppfmap::Pose ppfmap::CudaPPFMatch<PointT, NormalT>::getPose(
    const int reference_index,
    const std::vector<int>& indices,
    const PointCloudPtr cloud,
    const NormalsPtr normals,
    const float affine_s[12]) {

    Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tsg_map(affine_s);

    float affine_m[12];
    const std::size_t n = indices.size();

    const auto& ref_point = cloud->at(reference_index);
    const auto& ref_normal = normals->at(reference_index);

    thrust::host_vector<uint32_t> hash_list(n);
    thrust::host_vector<float> alpha_s_list(n);

    // Compute the PPF feature for all the pairs in the neighborhood
    for (int i = 0; i < n; i++) {
        const int index = indices[i];
        const auto& point = cloud->at(index);
        const auto& normal = normals->at(index);

        // Compute the PPF between reference_point and the i-th neighbor
        hash_list[i] = computePPFFeatureHash(ref_point, ref_normal,
                                             point, normal,
                                             distance_step,
                                             angle_step);

        // Compute the alpha_s angle
        const Eigen::Vector3f transformed(Tsg_map * point.getVector4fMap());
        alpha_s_list[i] = atan2f(-transformed(2), transformed(1));

    }

    int index;
    float alpha;
    int votes;

    map->searchBestMatch(hash_list, alpha_s_list, index, alpha, votes);

    const auto& model_point = model_->at(index);
    const auto& model_normal = normals_->at(index);

    getAlignmentToX(model_point, model_normal, &affine_m);

    Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tmg_map(affine_m);

    Eigen::Affine3f Tsg, Tmg;
    Tsg.matrix().block<3, 4>(0, 0) = Tsg_map.matrix();
    Tmg.matrix().block<3, 4>(0, 0) = Tmg_map.matrix();

    // Set final pose
    Pose final_pose;
    final_pose.c = pcl::Correspondence(reference_index, index, 0.0f);
    final_pose.t = Tsg.inverse() * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX()) * Tmg;
    final_pose.votes = votes;

    return final_pose;
}

void Skeletonization3D::translate_points_to_inside(PointCloudPtr& cloud,
		int cam_num) const {
	for (unsigned int i = 0; i < cloud->size(); i++) {
		cloud->set_z(i, cloud->get_z(i) + move_distance);
	}
}