예제 #1
0
  void EuclideanClustering::extract(
    const sensor_msgs::PointCloud2ConstPtr &input)
  {
    boost::mutex::scoped_lock lock(mutex_);
    vital_checker_->poke();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
      (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*input, *cloud);
    
    std::vector<pcl::PointIndices> cluster_indices;
    // list up indices which are not NaN to use
    // organized pointcloud
    pcl::PointIndices::Ptr nonnan_indices (new pcl::PointIndices);
    for (size_t i = 0; i < cloud->points.size(); i++) {
      pcl::PointXYZ p = cloud->points[i];
      if (!isnan(p.x) && !isnan(p.y) && !isnan(p.z)) {
        nonnan_indices->indices.push_back(i);
      }
    }

    if (nonnan_indices->indices.size() == 0) {
      // if input points is 0, publish empty data as result
      jsk_recognition_msgs::ClusterPointIndices result;
      result.header = input->header;
      result_pub_.publish(result);
      // do nothing and return it
      jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
      cluster_num_msg->header = input->header;
      cluster_num_msg->data = 0;
      cluster_num_pub_.publish(cluster_num_msg);
      return;
    }
    
    EuclideanClusterExtraction<pcl::PointXYZ> impl;
    {
      jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer();
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
      pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
      tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
      pcl::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ>);
      tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
#endif
      tree->setInputCloud (cloud);
      impl.setClusterTolerance (tolerance);
      impl.setMinClusterSize (minsize_);
      impl.setMaxClusterSize (maxsize_);
      impl.setSearchMethod (tree);
      impl.setIndices(nonnan_indices);
      impl.setInputCloud (cloud);
    }
    
    {
      jsk_topic_tools::ScopedTimer timer = segmentation_acc_.scopedTimer();
      impl.extract (cluster_indices);
    }
    
    // Publish result indices
    jsk_recognition_msgs::ClusterPointIndices result;
    result.cluster_indices.resize(cluster_indices.size());
    cluster_counter_.add(cluster_indices.size());
    result.header = input->header;
    if (cogs_.size() != 0 && cogs_.size() == cluster_indices.size()) {
      // tracking the labels
      //ROS_INFO("computing distance matrix");
      // compute distance matrix
      // D[i][j] --> distance between the i-th previous cluster
      //             and the current j-th cluster
      Vector4fVector new_cogs;
      computeCentroidsOfClusters(new_cogs, cloud, cluster_indices);
      double D[cogs_.size() * new_cogs.size()];
      computeDistanceMatrix(D, cogs_, new_cogs);
      std::vector<int> pivot_table = buildLabelTrackingPivotTable(D, cogs_, new_cogs, label_tracking_tolerance);
      if (pivot_table.size() != 0) {
        cluster_indices = pivotClusterIndices(pivot_table, cluster_indices);
      }
    }
    Vector4fVector tmp_cogs;
    computeCentroidsOfClusters(tmp_cogs, cloud, cluster_indices); // NB: not efficient
    cogs_ = tmp_cogs;
      
    for (size_t i = 0; i < cluster_indices.size(); i++) {
#if ROS_VERSION_MINIMUM(1, 10, 0)
      // hydro and later
      result.cluster_indices[i].header
        = pcl_conversions::fromPCL(cluster_indices[i].header);
#else
      // groovy
      result.cluster_indices[i].header = cluster_indices[i].header;
#endif
      result.cluster_indices[i].indices = cluster_indices[i].indices;
    }

    result_pub_.publish(result);
    
    jsk_recognition_msgs::Int32Stamped::Ptr cluster_num_msg (new jsk_recognition_msgs::Int32Stamped);
    cluster_num_msg->header = input->header;
    cluster_num_msg->data = cluster_indices.size();
    cluster_num_pub_.publish(cluster_num_msg);

    diagnostic_updater_->update();
  }
예제 #2
0
    /** \brief Given a plane, and the set of inlier indices representing it,
      * segment out the object of intererest supported by it. 
      *
      * \param[in] picked_idx the index of a point on the object
      * \param[in] cloud the full point cloud dataset
      * \param[in] plane_indices a set of indices representing the plane supporting the object of interest
      * \param[out] object the segmented resultant object 
      */
    void
    segmentObject (int picked_idx, 
                   const typename PointCloud<PointT>::ConstPtr &cloud, 
                   const PointIndices::Ptr &plane_indices, 
                   PointCloud<PointT> &object)
    {
      typename PointCloud<PointT>::Ptr plane_hull (new PointCloud<PointT>);

      // Compute the convex hull of the plane
      ConvexHull<PointT> chull;
      chull.setDimension (2);
      chull.setInputCloud (cloud);
      chull.setIndices (plane_indices);
      chull.reconstruct (*plane_hull);

      // Remove the plane indices from the data
      typename PointCloud<PointT>::Ptr plane (new PointCloud<PointT>);
      ExtractIndices<PointT> extract (true);
      extract.setInputCloud (cloud);
      extract.setIndices (plane_indices);
      extract.setNegative (false);
      extract.filter (*plane);
      PointIndices::Ptr indices_but_the_plane (new PointIndices);
      extract.getRemovedIndices (*indices_but_the_plane);

      // Extract all clusters above the hull
      PointIndices::Ptr points_above_plane (new PointIndices);
      ExtractPolygonalPrismData<PointT> exppd;
      exppd.setInputCloud (cloud);
      exppd.setIndices (indices_but_the_plane);
      exppd.setInputPlanarHull (plane_hull);
      exppd.setViewPoint (cloud->points[picked_idx].x, cloud->points[picked_idx].y, cloud->points[picked_idx].z);
      exppd.setHeightLimits (0.001, 0.5);           // up to half a meter
      exppd.segment (*points_above_plane);

      vector<PointIndices> euclidean_label_indices;
      // Prefer a faster method if the cloud is organized, over EuclidanClusterExtraction
      if (cloud_->isOrganized ())
      {
        // Use an organized clustering segmentation to extract the individual clusters
        typename EuclideanClusterComparator<PointT, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Label>);
        euclidean_cluster_comparator->setInputCloud (cloud);
        euclidean_cluster_comparator->setDistanceThreshold (0.03f, false);
        // Set the entire scene to false, and the inliers of the objects located on top of the plane to true
        Label l; l.label = 0;
        PointCloud<Label>::Ptr scene (new PointCloud<Label> (cloud->width, cloud->height, l));
        // Mask the objects that we want to split into clusters
        for (const int &index : points_above_plane->indices)
          scene->points[index].label = 1;
        euclidean_cluster_comparator->setLabels (scene);

        boost::shared_ptr<std::set<uint32_t> > exclude_labels = boost::make_shared<std::set<uint32_t> > ();
        exclude_labels->insert (0);
        euclidean_cluster_comparator->setExcludeLabels (exclude_labels);

        OrganizedConnectedComponentSegmentation<PointT, Label> euclidean_segmentation (euclidean_cluster_comparator);
        euclidean_segmentation.setInputCloud (cloud);

        PointCloud<Label> euclidean_labels;
        euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);
      }
      else
      {
        print_highlight (stderr, "Extracting individual clusters from the points above the reference plane ");
        TicToc tt; tt.tic ();

        EuclideanClusterExtraction<PointT> ec;
        ec.setClusterTolerance (0.02); // 2cm
        ec.setMinClusterSize (100);
        ec.setSearchMethod (search_);
        ec.setInputCloud (cloud);
        ec.setIndices (points_above_plane);
        ec.extract (euclidean_label_indices);
        
        print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", euclidean_label_indices.size ()); print_info (" clusters]\n");
      }

      // For each cluster found
      bool cluster_found = false;
      for (const auto &euclidean_label_index : euclidean_label_indices)
      {
        if (cluster_found)
          break;
        // Check if the point that we picked belongs to it
        for (size_t j = 0; j < euclidean_label_index.indices.size (); ++j)
        {
          if (picked_idx != euclidean_label_index.indices[j])
            continue;
          copyPointCloud (*cloud, euclidean_label_index.indices, object);
          cluster_found = true;
          break;
        }
      }
    }
예제 #3
0
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
  // Convert data to PointCloud<T>
  PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
  fromROSMsg (*input, *xyz);

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

  // Refine the plane indices
  typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
  SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
  RandomSampleConsensus<PointXYZ> sac (model, threshold);
  sac.setMaxIterations (max_iterations);
  bool res = sac.computeModel ();
  
  vector<int> inliers;
  sac.getInliers (inliers);
  Eigen::VectorXf coefficients;
  sac.getModelCoefficients (coefficients);

  if (!res || inliers.empty ())
  {
    PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
    return;
  }
  sac.refineModel (2, 50);
  sac.getInliers (inliers);
  sac.getModelCoefficients (coefficients);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");

  print_info ("Model coefficients: [");
  print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");

  // Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
  if (negative)
  {
    // Remove the plane indices from the data
    PointIndices::Ptr everything_but_the_plane (new PointIndices);
    std::vector<int> indices_fullset (xyz->size ());
    for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
      indices_fullset[p_it] = p_it;
    
    std::sort (inliers.begin (), inliers.end ());
    set_difference (indices_fullset.begin (), indices_fullset.end (),
                    inliers.begin (), inliers.end (),
                    inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));

    // Extract largest cluster minus the plane
    vector<PointIndices> cluster_indices;
    EuclideanClusterExtraction<PointXYZ> ec;
    ec.setClusterTolerance (0.02); // 2cm
    ec.setMinClusterSize (100);
    ec.setInputCloud (xyz);
    ec.setIndices (everything_but_the_plane);
    ec.extract (cluster_indices);

    // Convert data back
    copyPointCloud (*input, cluster_indices[0].indices, output);
  }
  else
  {
    // Convert data back
    PointCloud<PointXYZ> output_inliers;
    copyPointCloud (*input, inliers, output);
  }
}