void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
print_highlight (stderr, "Computing ");
tt.tic ();
// Refine the plane indices
typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
RandomSampleConsensus<PointXYZ> sac (model, threshold);
sac.setMaxIterations (max_iterations);
bool res = sac.computeModel ();
vector<int> inliers;
sac.getInliers (inliers);
Eigen::VectorXf coefficients;
sac.getModelCoefficients (coefficients);
if (!res || inliers.empty ())
{
PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
return;
}
sac.refineModel (2, 50);
sac.getInliers (inliers);
sac.getModelCoefficients (coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");
print_info ("Model coefficients: [");
print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
// Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
if (negative)
{
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
std::vector<int> indices_fullset (xyz->size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
indices_fullset[p_it] = p_it;
std::sort (inliers.begin (), inliers.end ());
set_difference (indices_fullset.begin (), indices_fullset.end (),
inliers.begin (), inliers.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract largest cluster minus the plane
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setInputCloud (xyz);
ec.setIndices (everything_but_the_plane);
ec.extract (cluster_indices);
// Convert data back
copyPointCloud (*input, cluster_indices[0].indices, output);
}
else
{
// Convert data back
PointCloud<PointXYZ> output_inliers;
copyPointCloud (*input, inliers, output);
}
}
