Пример #1
0
    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_);
        }
      }
    }
void
determinePlaneNormal (PointCloud<PointXYZ>::Ptr& p, Eigen::Vector3f& normal)
{
  SACSegmentation<PointXYZ> seg;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (SACMODEL_PLANE);
  seg.setMethodType (SAC_RANSAC);
  seg.setMaxIterations (50);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (p);
  ModelCoefficients coefficients_plane;
  pcl::PointIndices inliers_plane;
  seg.segment (inliers_plane, coefficients_plane);
  normal (0) = coefficients_plane.values[0];
  normal (1) = coefficients_plane.values[1];
  normal (2) = coefficients_plane.values[2];
}
Пример #3
0
	PointCloud<PointXYZ>::Ptr PCLTools::segmentPlanar(PointCloud<PointXYZ>::Ptr cloud, bool negative) {

		// Create the segmentation object for the planar model and set all the parameters
		PointCloud<PointXYZ>::Ptr cloud_f(new PointCloud<PointXYZ>);
		SACSegmentation<PointXYZ> seg;
		PointIndices::Ptr inliers(new PointIndices);
		ModelCoefficients::Ptr coefficients(new ModelCoefficients);
		PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ> ());

		seg.setOptimizeCoefficients(true);
		seg.setModelType(pcl::SACMODEL_PLANE);
		seg.setMethodType(pcl::SAC_RANSAC);
		seg.setMaxIterations(100);
		seg.setDistanceThreshold(0.02);

		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size () == 0) {
			cerr << "Could not estimate a planar model for the given dataset." << endl;
			return cloud_f;
		}

		// Extract the planar inliers from the input cloud
		ExtractIndices<PointXYZ> extract;
		extract.setInputCloud(cloud);
		extract.setIndices(inliers);
		extract.setNegative(false);

		// Get the points associated with the planar surface
		extract.filter(*cloud_plane);

		// Remove the planar inliers, extract the rest
		extract.setNegative(negative);
		extract.filter(*cloud_f);

		return cloud_f;

	}
Пример #4
0
TEST (SACSegmentation, Segmentation)
{
  ModelCoefficients::Ptr sphere_coefficients (new ModelCoefficients);
  PointIndices::Ptr inliers (new PointIndices);

  SACSegmentation<PointXYZ> seg;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (SACMODEL_SPHERE);
  seg.setMethodType (SAC_RANSAC);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.01);
  seg.setRadiusLimits (0.03, 0.07); // true radius is 0.05
  seg.setInputCloud (cloud_);
  seg.segment (*inliers, *sphere_coefficients);

  EXPECT_NEAR (sphere_coefficients->values[0], 0.998776,  1e-2);
  EXPECT_NEAR (sphere_coefficients->values[1], 0.752023,  1e-2);
  EXPECT_NEAR (sphere_coefficients->values[2], 1.24558,   1e-2);
  EXPECT_NEAR (sphere_coefficients->values[3], 0.0536238, 1e-2);

  EXPECT_NEAR (static_cast<int> (inliers->indices.size ()), 3516, 10);
}
Пример #5
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);
      }
    }
Пример #6
0
/**
* This function tries to find all planes in a point cloud by applying
* a model-based RANSAC algorithm.
*
* Heavily inspired by
* http://www.pointclouds.org/documentation/tutorials/extract_indices.php
*/
void filterPlanes(
	TheiaCloudPtr inCloud,
	TheiaCloudPtr outPlanes,
	TheiaCloudPtr outObjects
){
	TheiaCloudPtr workingCloudPtr(new TheiaCloud(*inCloud));
	size_t origCloudSize = workingCloudPtr->points.size();
	size_t minPlanePoints = config.minPercentage * origCloudSize;

	if(origCloudSize < 3){
		ROS_INFO("Input cloud for findPlanes is too small");
		ROS_INFO("Check cropping and rescaling parameters");
		return;
	}


	TheiaCloudPtr allPlanesCloudPtr(new TheiaCloud());
	PointIndices::Ptr inliers(new PointIndices());
	ModelCoefficients::Ptr coefficients(new ModelCoefficients());

	// prepare plane segmentation
	SACSegmentation<TheiaPoint> seg;
	seg.setModelType(SACMODEL_PLANE);
	seg.setMethodType(SAC_RANSAC);
	seg.setMaxIterations(config.numbIterations);
	seg.setDistanceThreshold(config.planeDistThresh);
	seg.setOptimizeCoefficients(config.planeOptimize);

	// prepare extractor
	ExtractIndices<TheiaPoint> extract;

	while(workingCloudPtr->points.size() >= 4){
		seg.setInputCloud(workingCloudPtr);
		seg.segment(*inliers, *coefficients);

		/**
		* Stop the search for planes when the found plane
		* is too small (in terms of points).
		*/
		size_t numbInliers = inliers->indices.size();
		if(!numbInliers) break;
		if(numbInliers < minPlanePoints) break;
		
		// extract plane points
		TheiaCloudPtr planeCloudPtr(new TheiaCloud());
		extract.setInputCloud(workingCloudPtr);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*planeCloudPtr);

		*allPlanesCloudPtr += *planeCloudPtr;

		/**
		* Remove plane from input cloud
		*/
		TheiaCloudPtr remainingCloudPtr(new TheiaCloud());
		extract.setNegative(true);
		extract.filter(*remainingCloudPtr);

		/**
		* Continue search 
		* But only use remaining point cloud
		*/
		workingCloudPtr->swap(*remainingCloudPtr);
	}

	*outPlanes = TheiaCloud(*allPlanesCloudPtr);
	*outObjects = TheiaCloud(*workingCloudPtr);
}
Пример #7
0
int main(int argc, char** argv)
{
	if (argc < 2)
	{
		cout << "Input a PCD file name...\n";
		return 0;
	}

	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>), cloud_f(new PointCloud<PointXYZ>);
	PCDReader reader;
	reader.read(argv[1], *cloud);
	cout << "PointCloud before filtering has: " << cloud->points.size() << " data points.\n";

	PointCloud<PointXYZ>::Ptr cloud_filtered(new PointCloud<PointXYZ>);
	VoxelGrid<PointXYZ> vg;
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points.\n";

	SACSegmentation<PointXYZ> seg;
	PointIndices::Ptr inliers(new PointIndices);
	PointCloud<PointXYZ>::Ptr cloud_plane(new PointCloud<PointXYZ>);

	ModelCoefficients::Ptr coefficients(new ModelCoefficients);
	seg.setOptimizeCoefficients(true);
	seg.setModelType(SACMODEL_PLANE);
	seg.setMethodType(SAC_RANSAC);
	seg.setMaxIterations(100);
	seg.setDistanceThreshold(0.02);

	int i=0, nr_points = (int)cloud_filtered->points.size();
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			cout << "Coud not estimate a planar model for the given dataset.\n";
			break;
		}

		ExtractIndices<PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_plane);
		cout << "PointCloud representing the planar component has: " << cloud_filtered->points.size() << " data points.\n";

		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered->swap(*cloud_f);
	}

	search::KdTree<PointXYZ>::Ptr kdtree(new search::KdTree<PointXYZ>);
	kdtree->setInputCloud(cloud_filtered);

	vector<PointIndices> cluster_indices;
	EuclideanClusterExtraction<PointXYZ> ece;
	ece.setClusterTolerance(0.02);
	ece.setMinClusterSize(100);
	ece.setMaxClusterSize(25000);
	ece.setSearchMethod(kdtree);
	ece.setInputCloud(cloud_filtered);
	ece.extract(cluster_indices);

	PCDWriter writer;
	int j = 0;
	for (vector<PointIndices>::const_iterator it=cluster_indices.begin(); it != cluster_indices.end(); ++it)
	{
		PointCloud<PointXYZ>::Ptr cluster_cloud(new PointCloud<PointXYZ>);
		for (vector<int>::const_iterator pit=it->indices.begin(); pit != it->indices.end(); ++pit)
			cluster_cloud->points.push_back(cloud_filtered->points[*pit]);
		cluster_cloud->width = cluster_cloud->points.size();
		cluster_cloud->height = 1;
		cluster_cloud->is_dense = true;

		cout << "PointCloud representing a cluster has: " << cluster_cloud->points.size() << " data points.\n";

		stringstream ss;
		ss << "cloud_cluster_" << j << ".pcd";
		writer.write<PointXYZ>(ss.str(), *cluster_cloud, false);
		j++;
	}

	return 0;
}
Пример #8
0
/* Compute the corner points of the biggest planar in the area (based on depth image)
* Input 	: Depth image (CV_16U), and rgb image, which is used for display
* Output	: vector of planes, where each plane is represented by 4 Corner points on the planar surface
*/
vector<vector<PointXYZI> > PlanarExtractor::getPlanar(Mat &depth, Mat rgb) {
	vector<vector<PointXYZI> > allCorners;
	PointCloud<PointXYZI>::Ptr cloud_inliers(new PointCloud<PointXYZI>);
	PointCloud<PointXYZI>::Ptr cloud_projection(new PointCloud<PointXYZI>);
	PointIndices::Ptr inliers(new PointIndices);
	ModelCoefficients::Ptr coefficients(new ModelCoefficients);
	vector<Point> positions;

	// *Extract Point cloud from depth image*
	PointCloud<PointXYZI>::Ptr cloud = getPointCloud(depth, positions);

	for (int plannr = 1;; plannr++) {
		// *Compute the planar*
#if USE_NORMAL_PLANE
		PointCloud<Normal>::Ptr normals_out(new PointCloud<Normal>);
		NormalEstimationOMP<PointXYZI, Normal> norm_est;
		norm_est.setSearchMethod(search::KdTree<PointXYZI>::Ptr(new search::KdTree<PointXYZI>));
		norm_est.setRadiusSearch(radius);
		norm_est.setInputCloud(cloud);
		norm_est.compute(*normals_out);

		SACSegmentationFromNormals<PointXYZI, Normal> segNormal;
		segNormal.setOptimizeCoefficients(true);
		segNormal.setModelType(SACMODEL_NORMAL_PLANE);
		segNormal.setNormalDistanceWeight(distanceWeight);
		segNormal.setMethodType(SAC_RANSAC);
		segNormal.setMaxIterations(maxIterations);
		segNormal.setDistanceThreshold(distanceThreshold);
		segNormal.setInputCloud(cloud);
		segNormal.setInputNormals(normals_out);
		segNormal.segment(*inliers, *coefficients);
#else
		SACSegmentation<PointXYZI> seg;
		seg.setOptimizeCoefficients(true);
		seg.setModelType(SACMODEL_PLANE);
		seg.setMethodType(SAC_RANSAC);
		seg.setDistanceThreshold(params.distanceThreshold); 							// WARNING : non normalized kinect input data, possible max: 4096 (needs verfification), how does this threshold compare to other devices???
		seg.setInputCloud(cloud);
		seg.segment(*inliers, *coefficients);
#endif

		ExtractIndices<PointXYZI> extract_inliers;
		extract_inliers.setInputCloud(cloud);
		extract_inliers.setIndices(inliers);
		extract_inliers.setNegative(false);
		extract_inliers.filter(*cloud_inliers);
		const size_t numInliers = inliers->indices.size();

		if (numInliers < MIN_INLIERS) break;

		// *Place inlier overlay over rgb image*
		const int channel = plannr % 3;
		for (int i = 0, iend = numInliers; i < iend; i++) {
			Point &p = positions[inliers->indices[i]];
			for (int x = p.x, xend = min(int(p.x + stride), rgb.cols - 1); x < xend; x++) {
				for (int y = p.y, yend = min(int(p.y + stride), rgb.rows - 1); y < yend; y++) {
					Vec3b &col = rgb.at<Vec3b>(y, x);
					col[channel] = (col[channel] + 255) / 2;
				}
			}
		}

		float a = coefficients->values[0];
		float b = coefficients->values[1];
		float c = coefficients->values[2];

		Eigen::Vector3f x_axis(b / sqrt(a * a + b * b), -a / sqrt(a * a + b * b), 0);
		Eigen::Vector3f y_direction(a, b, c);
		Eigen::Affine3f rotation = getTransFromUnitVectorsXY(x_axis, y_direction);

		for (int x = 0; x < 3; x++) // The rotation actually flattens the XZ axis, instead of XY, since using XY variables is much more convienient, swap the rotation of the Y and Z axis around
			swap(rotation(1, x), rotation(2, x));

		PointCloud<PointXYZI>::Ptr rotated(new PointCloud<PointXYZI>);
		transformPointCloud(*cloud_inliers, *rotated, rotation);

#if 0
		// viewing purposes:: view rotated inlier plane
		for (int i = 0; i < 10; i++) {
			cout << rotated->points[i] << endl;
		}
		visualization::CloudViewer viewer("Simple Cloud Viewer");
		viewer.showCloud(rotated);
		while (!viewer.wasStopped()) sleep(500);
#endif

		vector<PointXYZI> rect1(4);					// Corner points in projection-space
		vector<PointXYZI> rect2(4);					// Corner points in projection-space, but rotated 45 degrees
		for (int i = 0; i < 4; i++) {				// Initialize corner points to the first point
			rect1[i] = rotated->points[0];
			rect2[i] = rotate45(rotated->points[0]);
		}

		// *Compute corner points of planar*
		// Do estimation of rectangle, in order to really compute the corners compute for each point the point that has the LARGEST distance (farthest neighbor?). Keep track how many times each point has been listed as farthest, and create histogram.
		// For estimation, make 2 rectangles, first extract the 4 extrema (top/right/bottom/left), do the same but then rotate 45 degrees. Take the rectangle with the largest surface area.
		for (size_t i = 1; i < numInliers; i++) { 		// start from 1, since 0 is already set
			PointXYZI p = rotated->points[i];
			PointXYZI p45 = rotate45(p);

			if (p.y > rect1[0].y) rect1[0] = p; 	// TOP
			if (p.x > rect1[1].x) rect1[1] = p; 	// RIGHT
			if (p.y < rect1[2].y) rect1[2] = p; 	// BOTTOM
			if (p.x < rect1[3].x) rect1[3] = p; 	// LEFT

			if (p45.y > rect2[0].y) rect2[0] = p45; // TOP-LEFT
			if (p45.x > rect2[1].x) rect2[1] = p45; // TOP-RIGHT
			if (p45.y < rect2[2].y) rect2[2] = p45; // BOTTOM-RIGHT
			if (p45.x < rect2[3].x) rect2[3] = p45; // BOTTOM-LEFT
		}

		// Measure rectangle surface area for both suggestions, and use the largest one
		double area1 = getArea(rect1);
		double area2 = getArea(rect2);
		vector<PointXYZI> corners = fetchFromIndex<PointXYZI>(area1 > area2 ? rect1 : rect2, cloud->points);
		vector<Point> cvCorners = fetchFromIndex<Point>(corners, positions);
		allCorners.push_back(corners);

		// compute rectangle
		Point prev = cvCorners.back();
		for (int i = 0; i < 4; i++) {
			Point &p = cvCorners[i];
			circle(rgb, p, 5, cvScalar(0, 0, 255), -1);
			line(rgb, prev, p, cvScalar(0, 0, 255), 1);
			prev = p;
		}

		removeIndexes(cloud->points, inliers->indices);
		cloud->width = cloud->points.size();

		cout << "Corners: " << corners[0] << " " << corners[1] << " " << corners[2] << " " << corners[3] << std::endl;
		imshow("planar", rgb);
		if (waitKey() == 27) break;
	}

	return allCorners;
}