void removeModel(pcl::PointCloud<PointT>::Ptr cloud,
pcl::PointCloud<pcl::Normal>::Ptr normals,
pcl::SACSegmentationFromNormals<PointT, pcl::Normal> seg,
bool negative)
{
// Objects needed
pcl::ExtractIndices<PointT> extract;
pcl::ExtractIndices<pcl::Normal> extract_normals;
//Data
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object for the planar model and set all the parameters
seg.setInputCloud (cloud);
seg.setInputNormals (normals);
// Obtain the plane inliers and coefficients
seg.segment (*inliers, *coefficients);
// Extract the planar inliers from the input cloud
extract.setInputCloud (cloud);
extract.setIndices (inliers);
// Remove the planar inliers, extract the rest
extract.setNegative (negative);
extract.filter (*cloud);
extract_normals.setNegative (negative);
extract_normals.setInputCloud (normals);
extract_normals.setIndices (inliers);
extract_normals.filter (*normals);
}
float compute_cylinder() {
seg_cylinder.setInputCloud(cloud_filtered);
seg_cylinder.setInputNormals(cloud_normals);
// Obtain the cylinder inliers and coefficients
seg_cylinder.segment(*inliers_cylinder_, *coefficients_cylinder_);
std::cerr << "Cylinder coefficients: " << *coefficients_cylinder_ << std::endl;
float cost = (float) inliers_cylinder_->indices.size();
return cost;
}
float compute_plane() {
seg_plane.setInputCloud(cloud_filtered);
seg_plane.setInputNormals(cloud_normals);
seg_plane.segment(*inliers_plane_, *coefficients_plane_);
std::cerr << "Plane coefficients: " << *coefficients_plane_ << std::endl;
//seg.getDistancesToModel(*plane_coefficients_, distances);
// compute score
// Ideally this should be the distances and angular distance
// At here I will just use the sie of the pc.
float cost = (float) inliers_plane_->indices.size();
return cost;
}