I have this function for fitting a plane to a pointcloud using PCL's sac model fitting. I want the best result I can get, so I want to run with seg.setOptimizeCoefficients(true).
The problem is that a lot of the time, the pointcloud passed in, will not have enough points to optimise coefficients, so I get a continuous stream of:
[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (0)! Returning the same coefficients.
I would like to have coefficient optimisation to run when it can, and when it can't to just carry on without polluting the CLI output with many red warning messages.
according to this issue this message just means that there are fewer than 3 inlier points for the SAC model fitting. I do extract the inlier points, so I could manually check if there are 3 or more. But I can't see how to do this first, and THEN find the optimized model coefficients. Is there a way?
inline void fit_plane_to_points(
const pcl::PointCloud<pcl::PointXYZI>::ConstPtr& det_points,
const pcl::ModelCoefficients::Ptr& coefficients,
const Eigen::Vector3f& vec,
const pcl::PointCloud<pcl::PointXYZI>::Ptr& inlier_pts) {
// if no det points to work with, don't try and segment
if (det_points->size() < 3) {
return;
}
// fit surface point samples to a plane
pcl::PointIndices::Ptr inlier_indices(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZI> seg;
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
// max allowed difference between the plane normal and the given axis
seg.setEpsAngle(sac_angle_threshold_);
seg.setAxis(vec);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(sac_distance_threshold_);
seg.setMaxIterations(1000);
seg.setInputCloud(det_points);
seg.setOptimizeCoefficients(sac_optimise_coefficients_);
seg.segment(*inlier_indices, *coefficients);
if (inlier_indices->indices.empty()) {
// if no inlier points don't try and extract
return;
}
// extract the planar points
pcl::ExtractIndices<pcl::PointXYZI> extract;
extract.setInputCloud(det_points);
extract.setIndices(inlier_indices);
extract.setNegative(false);
extract.filter(*inlier_pts);
return;
}
I would say the best way to do this is to disable setOptimizeCoefficients and then do that manually after seg.segment. You basically have to recreate these lines: https://github.com/PointCloudLibrary/pcl/blob/10235c9c1ad47989bdcfebe47f4a369871357e2a/segmentation/include/pcl/segmentation/impl/sac_segmentation.hpp#L115-L123 .
You can access the model via getModel() (https://pointclouds.org/documentation/classpcl_1_1_s_a_c_segmentation.html#ac7b9564ceba35754837b4848cf448d78).