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);
}
/* 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;
}