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;
}
Extractor() {
tree.reset(new pcl::KdTreeFLANN<Point > ());
cloud.reset(new pcl::PointCloud<Point>);
cloud_filtered.reset(new pcl::PointCloud<Point>);
cloud_normals.reset(new pcl::PointCloud<pcl::Normal>);
coefficients_plane_.reset(new pcl::ModelCoefficients);
coefficients_cylinder_.reset(new pcl::ModelCoefficients);
inliers_plane_.reset(new pcl::PointIndices);
inliers_cylinder_.reset(new pcl::PointIndices);
// Filter Pass
pass.setFilterFieldName("z");
pass.setFilterLimits(-100, 100);
// VoxelGrid for Downsampling
LEAFSIZE = 0.01f;
vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE);
// Any object < CUT_THRESHOLD will be abandoned.
//CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700
CUT_THRESHOLD = 700; //for nonfiltering
// Clustering
cluster_.setClusterTolerance(0.06 * UNIT);
cluster_.setMinClusterSize(50.0);
cluster_.setSearchMethod(clusters_tree_);
// Normals
ne.setSearchMethod(tree);
ne.setKSearch(50); // 50 by default
// plane SAC
seg_plane.setOptimizeCoefficients(true);
seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE);
seg_plane.setNormalDistanceWeight(0.1); // 0.1
seg_plane.setMethodType(pcl::SAC_RANSAC);
seg_plane.setMaxIterations(1000); // 10000
seg_plane.setDistanceThreshold(0.05); // 0.03
// cylinder SAC
seg_cylinder.setOptimizeCoefficients(true);
seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER);
seg_cylinder.setMethodType(pcl::SAC_RANSAC);
seg_cylinder.setNormalDistanceWeight(0.1);
seg_cylinder.setMaxIterations(10000);
seg_cylinder.setDistanceThreshold(0.02); // 0.05
seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1]
}
void mpcl_sacnormal_set_axis_PointXYZRGB(pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> &sac,
double ax, double ay, double az)
{
Eigen::Vector3f vect(ax,ay,az);
sac.setAxis(vect);
}