C++ ConvexHull::setDimension方法代码示例

    void
    cloud_callback (const CloudConstPtr& cloud)
    {
      FPS_CALC ("cloud callback");
      boost::mutex::scoped_lock lock (cloud_mutex_);
      cloud_ = cloud;
      search_.setInputCloud (cloud);

      // Subsequent frames are segmented by "tracking" the parameters of the previous frame
      // We do this by using the estimated inliers from previous frames in the current frame, 
      // and refining the coefficients

      if (!first_frame_)
      {
        if (!plane_indices_ || plane_indices_->indices.empty () || !search_.getInputCloud ())
        {
          PCL_ERROR ("Lost tracking. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }
        SACSegmentation<PointT> seg;
        seg.setOptimizeCoefficients (true);
        seg.setModelType (SACMODEL_PLANE);
        seg.setMethodType (SAC_RANSAC);
        seg.setMaxIterations (1000);
        seg.setDistanceThreshold (0.01);
        seg.setInputCloud (search_.getInputCloud ());
        seg.setIndices (plane_indices_);
        ModelCoefficients coefficients;
        PointIndices inliers;
        seg.segment (inliers, coefficients);

        if (inliers.indices.empty ())
        {
          PCL_ERROR ("No planar model found. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }

        // Visualize the object in 3D...
        CloudPtr plane_inliers (new Cloud);
        pcl::copyPointCloud (*search_.getInputCloud (), inliers.indices, *plane_inliers);
        if (plane_inliers->points.empty ())
        {
          PCL_ERROR ("No planar model found. Select the object again to continue.\n");
          first_frame_ = true;
          return;
        }
        else
        {
          plane_.reset (new Cloud);

          // Compute the convex hull of the plane
          ConvexHull<PointT> chull;
          chull.setDimension (2);
          chull.setInputCloud (plane_inliers);
          chull.reconstruct (*plane_);
        }
      }
    }

//.........这里部分代码省略.........
        typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);

        ModelCoefficients coefficients;
        ExtractIndices<PointT> extract;
        PointIndices::Ptr inliers (new PointIndices ());

        // Up until 30% of the original cloud is left
        int i = 1;
        while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
        {
          seg.setInputCloud (cloud_segmented);

          print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
          TicToc tt; tt.tic ();
          seg.segment (*inliers, coefficients);
          print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
 
          // No datasets could be found anymore
          if (inliers->indices.empty ())
            break;

          // Save this plane
          PlanarRegion<PointT> region;
          region.setCoefficients (coefficients);
          regions.push_back (region);

          inlier_indices.push_back (*inliers);
          model_coefficients.push_back (coefficients);

          // Extract the outliers
          extract.setInputCloud (cloud_segmented);
          extract.setIndices (inliers);
          extract.setNegative (true);
          extract.filter (*cloud_remaining);
          cloud_segmented.swap (cloud_remaining);
        }
      }
      print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());

      double max_dist = numeric_limits<double>::max ();
      // Compute the distances from all the planar regions to the picked point, and select the closest region
      int idx = -1;
      for (size_t i = 0; i < regions.size (); ++i)
      {
        double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ()); 
        if (dist < max_dist)
        {
          max_dist = dist;
          idx = static_cast<int> (i);
        }
      }

      // Get the plane that holds the object of interest
      if (idx != -1)
      {
        plane_indices_.reset (new PointIndices (inlier_indices[idx]));

        if (cloud_->isOrganized ())
        {
          approximatePolygon (regions[idx], region, 0.01f, false, true);
          print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
        }
        else
        {
          // Save the current region
          region = regions[idx]; 

          print_highlight (stderr, "Obtaining the boundary points for the region ");
          TicToc tt; tt.tic ();
          // Project the inliers to obtain a better hull
          typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
          ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
          ProjectInliers<PointT> proj;
          proj.setModelType (SACMODEL_PLANE);
          proj.setInputCloud (cloud_);
          proj.setIndices (plane_indices_);
          proj.setModelCoefficients (coefficients);
          proj.filter (*cloud_projected);

          // Compute the boundary points as a ConvexHull
          ConvexHull<PointT> chull;
          chull.setDimension (2);
          chull.setInputCloud (cloud_projected);
          PointCloud<PointT> plane_hull;
          chull.reconstruct (plane_hull);
          region.setContour (plane_hull);
          print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
        }

      }

      // Segment the object of interest
      if (plane_indices_ && !plane_indices_->indices.empty ())
      {
        plane_.reset (new PointCloud<PointT>);
        copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
        object.reset (new PointCloud<PointT>);
        segmentObject (picked_idx, cloud_, plane_indices_, *object);
      }
    }

    /** \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;
        }
//.........这里部分代码省略.........

    /** \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[in] plane_boundary_indices a set of indices representing the boundary of the plane
      * \param[out] object the segmented resultant object 
      */
    void
    segmentObject (int picked_idx, 
                   const CloudConstPtr &cloud, 
                   const PointIndices::Ptr &plane_indices, 
                   const PointIndices::Ptr &plane_boundary_indices, 
                   Cloud &object)
    {
      CloudPtr plane_hull (new Cloud);

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

      // Remove the plane indices from the data
      PointIndices::Ptr everything_but_the_plane (new PointIndices);
      if (indices_fullset_.size () != cloud->points.size ())
      {
        indices_fullset_.resize (cloud->points.size ());
        for (int p_it = 0; p_it < static_cast<int> (indices_fullset_.size ()); ++p_it)
          indices_fullset_[p_it] = p_it;
      }
      std::vector<int> indices_subset = plane_indices->indices;
      std::sort (indices_subset.begin (), indices_subset.end ());
      set_difference (indices_fullset_.begin (), indices_fullset_.end (), 
                      indices_subset.begin (), indices_subset.end (), 
                      inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));

      // Extract all clusters above the hull
      PointIndices::Ptr points_above_plane (new PointIndices);
      ExtractPolygonalPrismData<PointT> exppd;
      exppd.setInputCloud (cloud);
      exppd.setInputPlanarHull (plane_hull);
      exppd.setIndices (everything_but_the_plane);
      exppd.setHeightLimits (0.0, 0.5);           // up to half a meter
      exppd.segment (*points_above_plane);

      // Use an organized clustering segmentation to extract the individual clusters
      EuclideanClusterComparator<PointT, Normal, Label>::Ptr euclidean_cluster_comparator (new EuclideanClusterComparator<PointT, Normal, 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 (int i = 0; i < static_cast<int> (points_above_plane->indices.size ()); ++i)
        scene->points[points_above_plane->indices[i]].label = 1;
      euclidean_cluster_comparator->setLabels (scene);

      vector<bool> exclude_labels (2);  exclude_labels[0] = true; exclude_labels[1] = false;
      euclidean_cluster_comparator->setExcludeLabels (exclude_labels);

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

      PointCloud<Label> euclidean_labels;
      vector<PointIndices> euclidean_label_indices;
      euclidean_segmentation.segment (euclidean_labels, euclidean_label_indices);

      // For each cluster found
      bool cluster_found = false;
      for (size_t i = 0; i < euclidean_label_indices.size (); i++)
      {
        if (cluster_found)
          break;
        // Check if the point that we picked belongs to it
        for (size_t j = 0; j < euclidean_label_indices[i].indices.size (); ++j)
        {
          if (picked_idx != euclidean_label_indices[i].indices[j])
            continue;
          //pcl::PointCloud<PointT> cluster;
          pcl::copyPointCloud (*cloud, euclidean_label_indices[i].indices, object);
          cluster_found = true;
          break;
          //object_indices = euclidean_label_indices[i].indices;
          //clusters.push_back (cluster);
        }
      }
    }