Esempio n. 1
0
void getCylClusters (boost::shared_ptr<PointCloud<T> > sceneCloud, vector<boost::shared_ptr<PointCloud<T> > > &outVector) {

  typedef typename pcl::search::KdTree<T>::Ptr my_KdTreePtr;
  typedef typename pcl::PointCloud<T>::Ptr my_PointCloudPtr;

  NormalEstimation<T, Normal> ne;
  SACSegmentationFromNormals<T, Normal> seg; 
  my_KdTreePtr tree (new pcl::search::KdTree<T> ());
  PointCloud<Normal>::Ptr cloud_normals (new PointCloud<pcl::Normal>);

  ModelCoefficients::Ptr coefficients_cylinder (new pcl::ModelCoefficients);
  PointIndices::Ptr  inliers_cylinder (new pcl::PointIndices);
  ExtractIndices<T> extract;

  // Estimate point normals
  ne.setSearchMethod (tree);
  ne.setInputCloud (sceneCloud);
  ne.setKSearch (50);
  ne.compute (*cloud_normals);

  seg.setOptimizeCoefficients (true);
  seg.setModelType (SACMODEL_CYLINDER);
  seg.setMethodType (SAC_RANSAC);
  seg.setNormalDistanceWeight (0.1);
  seg.setMaxIterations (10000);
  seg.setDistanceThreshold (0.05);
  seg.setRadiusLimits (0, 0.1);
  seg.setInputCloud (sceneCloud);
  seg.setInputNormals (cloud_normals);

  // Obtain the cylinder inliers and coefficients
  seg.segment (*inliers_cylinder, *coefficients_cylinder);
  cout << " Number of inliers vs total number of points " << inliers_cylinder->indices.size() << " vs " << sceneCloud->size() << endl; 
  // Write the cylinder inliers to disk
  extract.setInputCloud (sceneCloud);
  extract.setIndices (inliers_cylinder);
  extract.setNegative (false);
  my_PointCloudPtr cloud_cylinder (new PointCloud<T> ());
  extract.filter (*cloud_cylinder);
  
  outVector.push_back(cloud_cylinder);
}
Esempio n. 2
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;
}