// input: cloudInput
// input: pointCloudNormals
// output: cloudOutput
// output: pointCloudNormalsFiltered
void extractHandles(PointCloud::Ptr& cloudInput, PointCloud::Ptr& cloudOutput, PointCloudNormal::Ptr& pointCloudNormals, std::vector<int>& handles) {
// PCL objects
//pcl::PassThrough<Point> vgrid_; // Filtering + downsampling object
pcl::VoxelGrid<Point> vgrid_; // Filtering + downsampling object
pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_; // Planar segmentation object
pcl::SACSegmentation<Point> seg_line_; // Planar segmentation object
pcl::ProjectInliers<Point> proj_; // Inlier projection object
pcl::ExtractIndices<Point> extract_; // Extract (too) big tables
pcl::ConvexHull<Point> chull_;
pcl::ExtractPolygonalPrismData<Point> prism_;
pcl::PointCloud<Point> cloud_objects_;
pcl::EuclideanClusterExtraction<Point> cluster_, handle_cluster_;
pcl::PCDWriter writer;
double sac_distance_, normal_distance_weight_;
double eps_angle_, seg_prob_;
int max_iter_;
sac_distance_ = 0.05; //0.02
normal_distance_weight_ = 0.05;
max_iter_ = 500;
eps_angle_ = 30.0; //20.0
seg_prob_ = 0.99;
btVector3 axis(0.0, 0.0, 1.0);
seg_.setModelType(pcl::SACMODEL_NORMAL_PARALLEL_PLANE);
seg_.setMethodType(pcl::SAC_RANSAC);
seg_.setDistanceThreshold(sac_distance_);
seg_.setNormalDistanceWeight(normal_distance_weight_);
seg_.setOptimizeCoefficients(true);
seg_.setAxis(Eigen::Vector3f(fabs(axis.getX()), fabs(axis.getY()), fabs(axis.getZ())));
seg_.setEpsAngle(pcl::deg2rad(eps_angle_));
seg_.setMaxIterations(max_iter_);
seg_.setProbability(seg_prob_);
cluster_.setClusterTolerance(0.03);
cluster_.setMinClusterSize(200);
KdTreePtr clusters_tree_(new KdTree);
clusters_tree_->setEpsilon(1);
cluster_.setSearchMethod(clusters_tree_);
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setDistanceThreshold(0.05);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(max_iter_);
seg_line_.setProbability(seg_prob_);
//filter cloud
double leafSize = 0.001;
pcl::VoxelGrid<Point> sor;
sor.setInputCloud (cloudInput);
sor.setLeafSize (leafSize, leafSize, leafSize);
sor.filter (*cloudOutput);
//sor.setInputCloud (pointCloudNormals);
//sor.filter (*pointCloudNormalsFiltered);
//std::vector<int> tempIndices;
//pcl::removeNaNFromPointCloud(*cloudInput, *cloudOutput, tempIndices);
//pcl::removeNaNFromPointCloud(*pointCloudNormals, *pointCloudNormalsFiltered, tempIndices);
// Segment planes
pcl::OrganizedMultiPlaneSegmentation<Point, PointNormal, pcl::Label> mps;
ROS_INFO("Segmenting planes");
mps.setMinInliers (20000);
mps.setMaximumCurvature(0.02);
mps.setInputNormals (pointCloudNormals);
mps.setInputCloud (cloudInput);
std::vector<pcl::PlanarRegion<Point> > regions;
std::vector<pcl::PointIndices> regionPoints;
std::vector< pcl::ModelCoefficients > planes_coeff;
mps.segment(planes_coeff, regionPoints);
ROS_INFO_STREAM("Number of regions:" << regionPoints.size());
if ((int) regionPoints.size() < 1) {
ROS_ERROR("no planes found");
return;
}
std::stringstream filename;
for (size_t plane = 0; plane < regionPoints.size (); plane++)
{
filename.str("");
filename << "plane" << plane << ".pcd";
writer.write(filename.str(), *cloudInput, regionPoints[plane].indices, true);
ROS_INFO("Plane model: [%f, %f, %f, %f] with %d inliers.",
planes_coeff[plane].values[0], planes_coeff[plane].values[1],
planes_coeff[plane].values[2], planes_coeff[plane].values[3], (int)regionPoints[plane].indices.size ());
//Project Points into the Perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
pcl::PointIndicesPtr cloudPlaneIndicesPtr(new pcl::PointIndices(regionPoints[plane]));
pcl::ModelCoefficientsPtr coeff(new pcl::ModelCoefficients(planes_coeff[plane]));
proj_.setInputCloud(cloudInput);
proj_.setIndices(cloudPlaneIndicesPtr);
proj_.setModelCoefficients(coeff);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
proj_.filter(*cloud_projected);
PointCloud::Ptr cloud_hull(new PointCloud());
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
ROS_INFO("Convex hull has: %d data points.", (int)cloud_hull->points.size ());
if ((int) cloud_hull->points.size() == 0)
{
ROS_WARN("Convex hull has: %d data points. Returning.", (int)cloud_hull->points.size ());
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setHeightLimits(0.05, 0.1);
prism_.setInputCloud(cloudInput);
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
ROS_INFO("Number of handle candidates: %d.", (int)handlesIndicesPtr->indices.size ());
if((int)handlesIndicesPtr->indices.size () < 1100)
continue;
/*######### handle clustering code
handle_clusters.clear();
handle_cluster_.setClusterTolerance(0.03);
handle_cluster_.setMinClusterSize(200);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(handles);
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
filename.str("");
filename << "handle" << (int)i << "-" << (int)j << ".pcd";
writer.write(filename.str(), *handles, handle_clusters[j].indices, true);
}*/
pcl::StatisticalOutlierRemoval<Point> sor;
PointCloud::Ptr cloud_filtered (new pcl::PointCloud<Point>);
sor.setInputCloud (cloudInput);
sor.setIndices(handlesIndicesPtr);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
PointCloudNormal::Ptr cloud_filtered_hack (new PointCloudNormal);
pcl::copyPointCloud(*cloud_filtered, *cloud_filtered_hack);
// Our handle is in cloud_filtered. We want indices of a cloud (also filtered for NaNs)
pcl::KdTreeFLANN<PointNormal> kdtreeNN;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtreeNN.setInputCloud(pointCloudNormals);
for(size_t j = 0; j < cloud_filtered_hack->points.size(); j++)
{
kdtreeNN.nearestKSearch(cloud_filtered_hack->points[j], 1, pointIdxNKNSearch, pointNKNSquaredDistance);
handles.push_back(pointIdxNKNSearch[0]);
}
}
}
/**
* extract handles from a pointcloud
* @param[pointCloudIn] input point cloud
* @param[pointCloudNormals] normals for the input cloud
* @param[handle_indices] vector of indices, each item being a set of indices representing a handle
* @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle
*/
void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals,
std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients
)
{
// setup handle cluster object
pcl::EuclideanClusterExtraction<Point> handle_cluster_;
KdTreePtr clusters_tree_(new KdTree);
handle_cluster_.setClusterTolerance(handle_cluster_tolerance);
handle_cluster_.setMinClusterSize(min_handle_cluster_size);
handle_cluster_.setSearchMethod(clusters_tree_);
handle_cluster_.setInputCloud(cloudInput);
pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>());
pcl::VoxelGrid<Point> vg;
vg.setInputCloud(cloudInput);
vg.setLeafSize(0.01, 0.01, 0.01);
vg.filter(*cloud_filtered);
// setup line ransac object
pcl::SACSegmentation<Point> seg_line_;
seg_line_.setModelType(pcl::SACMODEL_LINE);
seg_line_.setMethodType(pcl::SAC_RANSAC);
seg_line_.setInputCloud(cloudInput);
seg_line_.setDistanceThreshold(line_ransac_distance);
seg_line_.setOptimizeCoefficients(true);
seg_line_.setMaxIterations(line_ransac_max_iter);
// setup Inlier projection object
pcl::ProjectInliers<Point> proj_;
proj_.setInputCloud(cloud_filtered);
proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);
// setup polygonal prism
pcl::ExtractPolygonalPrismData<Point> prism_;
prism_.setHeightLimits(min_handle_height, max_handle_height);
prism_.setInputCloud(cloudInput);
//setup Plane Segmentation
outInfo("Segmenting planes");
pcl::SACSegmentation<Point> seg_plane_;
seg_plane_.setOptimizeCoefficients(true);
seg_plane_.setModelType(pcl::SACMODEL_PLANE);
seg_plane_.setMethodType(pcl::SAC_RANSAC);
seg_plane_.setDistanceThreshold(0.03);
seg_plane_.setMaxIterations(500);
seg_plane_.setInputCloud(cloud_filtered);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices());
seg_plane_.segment(*plane_inliers, *plane_coefficients);
if(plane_inliers->indices.size() != 0)
{
outDebug("Plane model: "
<< plane_coefficients->values[0] << ","
<< plane_coefficients->values[1] << ","
<< plane_coefficients->values[2] << ","
<< plane_coefficients->values[3] << " with "
<< (int)plane_inliers->indices.size() << "inliers ");
//Project inliers of the planes into a perfect plane
PointCloud::Ptr cloud_projected(new PointCloud());
proj_.setIndices(plane_inliers);
proj_.setModelCoefficients(plane_coefficients);
proj_.filter(*cloud_projected);
// Create a Convex Hull representation using the projected inliers
PointCloud::Ptr cloud_hull(new PointCloud());
pcl::ConvexHull<Point> chull_;
chull_.setDimension(2);
chull_.setInputCloud(cloud_projected);
chull_.reconstruct(*cloud_hull);
outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points.");
if((int) cloud_hull->points.size() == 0)
{
outInfo("Convex hull has: no data points. Returning.");
return;
}
// Extract the handle clusters using a polygonal prism
pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
prism_.setInputPlanarHull(cloud_hull);
prism_.segment(*handlesIndicesPtr);
// cluster the points found in the prism
std::vector< pcl::PointIndices> handle_clusters;
handle_cluster_.setIndices(handlesIndicesPtr);
handle_cluster_.extract(handle_clusters);
for(size_t j = 0; j < handle_clusters.size(); j++)
{
// for each cluster in the prism, attempt to fit a line using ransac
pcl::PointIndices single_handle_indices;
pcl::ModelCoefficients handle_line_coefficients;
seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j]));
seg_line_.segment(single_handle_indices, handle_line_coefficients);
if(single_handle_indices.indices.size() > 0)
{
outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers.");
outDebug("Handle Line coefficients: " << handle_line_coefficients);
handle_indices.push_back(single_handle_indices);
handle_coefficients.push_back(handle_line_coefficients);
}
}
}
else
{
outInfo("no planes found");
return;
}
}
