void Segmentation::createPlane(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters)
{
_planes.clear();
for (int v = 0 ; v < clusters.size(); v++)
{
Plane plane;
//pcl::transformPointCloud(*clusters[v], *clusters[v], transformXY);
plane.setPlaneCloud(clusters[v]);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
chull.setInputCloud (clusters[v]);
chull.setAlpha (0.1);
chull.reconstruct (*concaveHull);
//vectorHull.push_back(concaveHull);
plane.setHull(concaveHull);
_planes.push_back(plane);
}
//pcl::PointCloud<pcl::sPointXYZRGBA>::Ptr gr(new pcl::PointCloud<pcl::PointXYZRGBA>);
//gr = _ground.getPlaneCloud();
//pcl::transformPointCloud(*_mainCloud, *_mainCloud, transformXY);
//pcl::transformPointCloud(*gr, *gr, transformXY);
//_ground.setPlaneCloud(gr);
if(!first)
{
computeTransformation();
first = true;
}
computeHeight();
}
int main(int argc,char** argv)
{
// Object for storing the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inlierPoints(new pcl::PointCloud<pcl::PointXYZ>);
// Read a PCD file from disk
if(pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1],*cloud) != 0)
{
return -1;
}
// Object for storing the plane model coefficients
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> segmentation;
segmentation.setInputCloud(cloud);
// Configure the object to look for a cylinder
segmentation.setModelType(pcl::SACMODEL_PLANE);
// Use RANSAC method
segmentation.setMethodType(pcl::SAC_RANSAC);
// Set the maximum allowed dstance to the model
segmentation.setDistanceThreshold(0.01);
// Enable model coefficient refinement (optional)
segmentation.setOptimizeCoefficients(true);
pcl::PointIndices::Ptr inlierIndices(new pcl::PointIndices);
segmentation.segment(*inlierIndices,*coefficients);
if(inlierIndices->indices.size() == 0)
std::cout << "Could not find a plane in the scene." << std::endl;
else
{
// Copy the point of the plane to a new cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inlierIndices);
extract.filter(*plane);
// Object for retrieving the concave hull
pcl::ConcaveHull<pcl::PointXYZ> hull;
pcl::PointCloud<pcl::PointXYZ>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZ>);
hull.setInputCloud(plane);
// Set alpha, which is the maximum legth fromma vertex to the center of the voronoi cell
// (the smaller, the greater the resolution of the hull)
hull.setAlpha(0.1);
hull.reconstruct(*concaveHull);
// Visualize the hull
pcl::visualization::CloudViewer viewerPlane(" Concave hull");
viewerPlane.showCloud(concaveHull);
while(!viewerPlane.wasStopped())
{
// Do nothing
}
}
return 0;
}
/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
bool Segmentation::findGround(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, MainPlane &mp)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.030); //0.25 / 35
seg.setAxis(_axis); // Axis Y 0, -1, 0
seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (cloud);
seg.segment (*indices, *coeff);
mp.setCoefficients(coeff);
//mp.setIndices(indices);
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
return false;
}
else
{
extract.setInputCloud(cloud);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*ground);
mp.setPlaneCloud(ground);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Copy the points of the plane to a new cloud.
pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
extractHull.setInputCloud(cloud);
extractHull.setIndices(indices);
extractHull.filter(*plane);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
chull.setInputCloud (plane);
chull.setAlpha (0.1);
chull.reconstruct (*concaveHull);
mp.setHull(concaveHull);
//vectorCloudinliers.push_back(convexHull);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
return true;
}
}
/*
* Find the other planes in the environment.
* Params[in/out]: the cloud, the normal of the ground, the coeff of the planes, the planes, the hull
* return the number of inliers
*/
int Segmentation::findOtherPlanesRansac(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud,Eigen::Vector3f axis, std::vector <pcl::ModelCoefficients> &coeffPlanes, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorHull )
{
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
vectorCloudInliers.clear();
coeffPlanes.clear();
vectorHull.clear();
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
const int nb_points = cloud->points.size();
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setMaxIterations(5);
seg.setDistanceThreshold (0.020); //0.15
seg.setAxis(axis);
//std::cout<< "axis are a : " << axis[0] << " b : " << axis[1] << " c ; " << axis[2] << std::endl;
seg.setEpsAngle(20.0f * (M_PI/180.0f));
while (cloud->points.size() > _coeffRansac * nb_points)
{
// std::cout << "the number is " << cloud->points.size() << std::endl;
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
//PCL_ERROR ("Could not estimate a planar model for the given dataset.");
break;
}
else
{
coeffPlanes.push_back(*coefficients);
//vectorInliers.push_back(*inliers);
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_p);
//Eigen::Vector4f centroid;
//pcl::compute3DCentroid(*cloud_p,*inliers,centroid);
// passthroughFilter(centroid[0] - 2, centroid[0] + 2, centroid[1] - 2, centroid[1] + 2, centroid[2] - 2, centroid[2] + 2, cloud_p, cloud_p);
//statisticalRemovalOutliers(cloud_p);
//statisticalRemovalOutliers(cloud_p); -0.00485527 b : -0.895779 c ; -0.444474
//if((std::abs(coefficients->values[0]) < (0.1 )) && ( std::abs(coefficients->values[1]) > (0.89)) && (std::abs(coefficients->values[2]) > (0.40)))
//{
// std::cout<< "coeff are a : " << coefficients->values[0] << " b : " << coefficients->values[1] << " c ; " << coefficients->values[2] << std::endl;
vectorCloudInliers.push_back(cloud_p);
//}
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Copy the points of the plane to a new cloud.
pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
extractHull.setInputCloud(cloud);
extractHull.setIndices(inliers);
extractHull.filter(*plane);
// Object for retrieving the convex hull.
// pcl::ConvexHull<pcl::PointXYZRGBA> hull;
// hull.setInputCloud(plane);
// hull.reconstruct(*convexHull);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
// chull.setInputCloud (plane);
// chull.setAlpha (0.1);
// chull.reconstruct (*concaveHull);
// vectorHull.push_back(concaveHull);
//vectorCloudinliers.push_back(convexHull);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
// std::cout << "the number is " << cloud->points.size() << std::endl;
}
}
return vectorCloudInliers.size();
}
