void planes_Segmentation(std::string fileName_Full, std::string fileOutput)
{
	source_pc.reset(new PointCloud<PointXYZ>);
	pcl::io::loadPCDFile(fileName_Full, *source_pc);
	if (reducePointCloud)
	{
		source_pc = util::resamplingPointcloud(source_pc, 2);
		minimumInliner = 100;
	}

	PointCloud<Normal>::Ptr source_n = proc::normalsEstimate(source_pc);
	util::displayPC(viewer, source_pc, 1, 1, 1);
	//viewer->addPointCloudNormals<PointXYZ, Normal>(source_pc, source_n, 64, 0.05, "NN");

	std::vector<PlanarRegion<PointXYZ>, Eigen::aligned_allocator<PlanarRegion<PointXYZ> > > regions;
	std::vector<ModelCoefficients> model_coefficients;
	std::vector<PointIndices> inlier_indices;
	PointCloud<Label>::Ptr labels(new PointCloud<Label>);
	std::vector<PointIndices> label_indices;
	std::vector<PointIndices> boundary_indices;

	// Segment Planes
	int64 mps_start = cv::getTickCount();

	OrganizedMultiPlaneSegmentation<PointXYZ, Normal, Label> mps;
	mps.setAngularThreshold(pcl::deg2rad(10.0));
	mps.setMinInliers(minimumInliner);
	mps.setInputNormals(source_n);
	mps.setInputCloud(source_pc);

	//mps.segment(model_coefficients,inlier_indices);
	mps.segmentAndRefine(regions, model_coefficients,
		inlier_indices, labels,
		label_indices, boundary_indices);

	double mps_end = cv::getTickCount();
	PCL_WARN("MPS + Refine took: %f sec \n=== \n", double(mps_end - mps_start) / cv::getTickFrequency());

	for (int i = 0; i < inlier_indices.size(); i++)
	{
		PointIndicesPtr idc(new PointIndices);
		idc->indices = inlier_indices[i].indices;

		PointCloud<PointXYZ>::Ptr planeCloud(new PointCloud<PointXYZ>);
		// Create the filtering object
		pcl::ExtractIndices<PointXYZ> extractPoints;
		extractPoints.setInputCloud(source_pc);
		extractPoints.setIndices(idc);
		extractPoints.setNegative(false);
		extractPoints.filter(*planeCloud);

		util::displayPC(viewer, planeCloud);
	}
	viewer->spinOnce();
	viewer->saveScreenshot(fileOutput);
	viewer->removeAllPointClouds();
}
Exemplo n.º 2
0
    void
    segment (const PointT &picked_point, 
             int picked_idx,
             PlanarRegion<PointT> &region,
             typename PointCloud<PointT>::Ptr &object)
    {
      object.reset ();

      // Segment out all planes
      vector<ModelCoefficients> model_coefficients;
      vector<PointIndices> inlier_indices, boundary_indices;
      vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;

      // Prefer a faster method if the cloud is organized, over RANSAC
      if (cloud_->isOrganized ())
      {
        print_highlight (stderr, "Estimating normals ");
        TicToc tt; tt.tic ();
        // Estimate normals
        PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
        estimateNormals (cloud_, *normal_cloud);
        print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");

        OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
        mps.setMinInliers (1000);
        mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
        mps.setDistanceThreshold (0.03); // 2 cm
        mps.setMaximumCurvature (0.001); // a small curvature
        mps.setProjectPoints (true);
        mps.setComparator (plane_comparator_);
        mps.setInputNormals (normal_cloud);
        mps.setInputCloud (cloud_);

        // Use one of the overloaded segmentAndRefine calls to get all the information that we want out
        PointCloud<Label>::Ptr labels (new PointCloud<Label>);
        vector<PointIndices> label_indices;
        mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
      }
      else
      {
        SACSegmentation<PointT> seg;
        seg.setOptimizeCoefficients (true);
        seg.setModelType (SACMODEL_PLANE);
        seg.setMethodType (SAC_RANSAC);
        seg.setMaxIterations (10000);
        seg.setDistanceThreshold (0.005);

        // Copy XYZ and Normals to a new cloud
        typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
        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);
      }
    }
int _tmain(int argc, _TCHAR* argv[])
{
	viewer = util::rgbVisualizer("RANSAC - Dev");
	//testMainMS();
	testMainWS();
	return 0;

	source_pc.reset(new PointCloud<PointXYZ>);
	PointCloud<Normal>::Ptr source_n(new PointCloud<Normal>);

	pcl::io::loadPCDFile(dPath + FILENAME, *source_pc);
	
	if (reducePointCloud)
	{
		source_pc = util::resamplingPointcloud(source_pc, 2);
		minimumInliner = 100;
	}

	source_n = proc::normalsEstimate(source_pc);

	util::displayPC(viewer, source_pc, 1, 1, 1);
	//viewer->addPointCloudNormals<PointXYZ, Normal>(source_pc, source_n, 64, 0.05, "NN");

	std::vector<PlanarRegion<PointXYZ>, Eigen::aligned_allocator<PlanarRegion<PointXYZ> > > regions;
	std::vector<ModelCoefficients> model_coefficients;
	std::vector<PointIndices> inlier_indices;
	PointCloud<Label>::Ptr labels(new PointCloud<Label>);
	std::vector<PointIndices> label_indices;
	std::vector<PointIndices> boundary_indices;

	// Segment Planes
	int64 mps_start = cv::getTickCount();
	OrganizedMultiPlaneSegmentation<PointXYZ, Normal, Label> mps;
	mps.setAngularThreshold(pcl::deg2rad(10.0));
	mps.setMinInliers(minimumInliner);
	mps.setInputNormals(source_n);
	mps.setInputCloud(source_pc);
	
	//mps.segment(model_coefficients,inlier_indices);
	mps.segmentAndRefine(regions, model_coefficients, 
		inlier_indices, labels, 
		label_indices, boundary_indices);

	double mps_end = cv::getTickCount();
	PCL_WARN("MPS + Refine took: %f sec \n=== \n", double(mps_end - mps_start)/cv::getTickFrequency());

	for (int i = 0; i < inlier_indices.size();i++)
	{
		PointIndicesPtr idc(new PointIndices);
		idc->indices = inlier_indices[i].indices;

		PointCloud<PointXYZ>::Ptr planeCloud(new PointCloud<PointXYZ>);

		/*Create the filtering object*/
		pcl::ExtractIndices<PointXYZ> extractPoints;
		extractPoints.setInputCloud(source_pc);
		extractPoints.setIndices(idc);
		extractPoints.setNegative(false);
		extractPoints.filter(*planeCloud);

		util::displayPC(viewer, planeCloud);
	}

	/*viewer->spinOnce(100);
	boost::this_thread::sleep(boost::posix_time::microseconds(100000));*/

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return 0;
}