机器人 Grasp Pose Detection (GPD) 移除平面

flyfish

使用了点云分割RanSaC 算法 随机采样一致（Random sample consensus）
KD树

std::vector<cv::Mat> Learning::createImages(const CloudCamera& cloud_cam, const std::vector<GraspSet>& hand_set_list)
  const
{
  double t0_total = omp_get_wtime();

  PointCloudRGBA::Ptr cloud(new PointCloudRGBA);
  *cloud = *cloud_cam.getCloudProcessed();
  Eigen::Matrix3Xd points = cloud->getMatrixXfMap().cast<double>();
  PointList point_list(points, cloud_cam.getNormals(), cloud_cam.getCameraSource(), cloud_cam.getViewPoints());

  if (remove_plane_)
  {
	  // Segment the plane to speed up shadow computation.
    pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    seg.setInputCloud(cloud_cam.getCloudProcessed());
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
//    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.01);
    seg.segment(*inliers, *coefficients);
    if (inliers->indices.size () > 0)
    {
      pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
      extract.setInputCloud(cloud_cam.getCloudProcessed());
      extract.setIndices(inliers);
      extract.setNegative(true);
      std::vector<int> indices;
      extract.filter(indices);
//      pcl::visualization::CloudViewer viewer("Filtered Cloud");
//      viewer.showCloud(cloud);
//      while (!viewer.wasStopped()) { }
      if (indices.size() > 0)
      {
        extract.filter(*cloud);
        point_list = point_list.slice(indices);
        printf("Removed plane from point cloud. Remaining points in cloud: %d.\n", (int) cloud->size());
        std::cout << point_list.getPoints().col(0).transpose() << "\n" << cloud->at(0).getVector3fMap().transpose() << "\n";
      }
      else
      {
        printf("No points remaining after plane is removed! Using entire point cloud ...\n");
      }
    }
  }

  // Prepare kd-tree for neighborhood searches in the point cloud.
  pcl::KdTreeFLANN<pcl::PointXYZRGBA> kdtree;
  kdtree.setInputCloud(cloud);
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;

  // Set the radius for the neighborhood search to the largest image dimension.
  Eigen::Vector3d image_dims;
  image_dims << image_params_.depth_, image_params_.height_ / 2.0, image_params_.outer_diameter_;
  double radius = image_dims.maxCoeff();

  // 1. Find points within image dimensions.
  bool is_valid[hand_set_list.size()];
  std::vector<PointList> nn_points_list;
  nn_points_list.resize(hand_set_list.size());

#ifdef _OPENMP // parallelization using OpenMP
#pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_)
#endif
  for (int i = 0; i < hand_set_list.size(); i++)
  {
    pcl::PointXYZRGBA sample_pcl;
    sample_pcl.getVector3fMap() = hand_set_list[i].getSample().cast<float>();

    if (kdtree.radiusSearch(sample_pcl, radius, nn_indices, nn_dists) > 0)
    {
      nn_points_list[i] = point_list.slice(nn_indices);
      is_valid[i] = true;
    }
    else
    {
      is_valid[i] = false;
    }
  }

  std::cout << "time for computing " << nn_points_list.size() << " point neighborhoods with " << num_threads_ << " threads: " << omp_get_wtime() - t0_total << "s\n";

  if (image_params_.num_channels_ == 1 || image_params_.num_channels_ == 3) // 3 channels image (only surface normals)
  {
    return createImages1or3Channels(hand_set_list, nn_points_list, is_valid, image_dims);
  }
  else if (image_params_.num_channels_ == 15) // 15 channels image
  {
    return createImages15Channels(hand_set_list, nn_points_list, is_valid, image_dims);
  }

  std::vector<cv::Mat> empty;
  empty.resize(0);
  return empty;
}

remove_plane_ 由remove_plane_before_image_calculation控制
Learning在构造的时候将remove_plane_赋值

GraspDetector::GraspDetector(ros::NodeHandle& node)
{

  bool remove_plane;
  node.param("remove_plane_before_image_calculation", remove_plane, false);

  // Create object to create grasp images from grasp candidates (used for classification)
  learning_ = new Learning(image_params_, hand_search_params.num_threads_, hand_search_params.num_orientations_, false,
                           remove_plane);
}
 Learning(const ImageParameters& params, int num_threads, int num_orientations, bool is_plotting, bool remove_plane)
      : image_params_(params), num_threads_(num_threads), num_orientations_(num_orientations),
        is_plotting_(is_plotting), remove_plane_(remove_plane) { }