Files
opencv-MIRROR/modules/geometry/src/ptcloud/sac_segmentation.cpp
Alexander Smorkalov fc3803c67b Merge pull request #29224 from asmorkalov:as/ptcloud2
Dedicated pointcloud module #29224

OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [ ] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake
2026-06-04 12:19:02 +03:00

273 lines
9.5 KiB
C++

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
//
// Copyright (C) 2021, Yechun Ruan <ruanyc@mail.sustech.edu.cn>
#include "../precomp.hpp"
#include "sac_segmentation.hpp"
#include "opencv2/geometry/segment.hpp"
#include "ptcloud_utils.hpp"
#include "../usac.hpp"
namespace cv {
// namespace _3d {
/**
*
* @param model_type 3D Model
* @return At least a few are needed to determine a model.
*/
inline int sacModelMinimumSampleSize(SacModelType model_type)
{
switch (model_type)
{
case SAC_MODEL_PLANE:
return 3;
case SAC_MODEL_SPHERE:
return 4;
default:
CV_Error(cv::Error::StsNotImplemented, "SacModel Minimum Sample Size not defined!");
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////// SACSegmentationImpl //////////////////////////////////////
//-------------------------- segmentSingle -----------------------
int
SACSegmentationImpl::segmentSingle(Mat &points, std::vector<bool> &label, Mat &model_coefficients)
{
CV_CheckDepthEQ(points.depth(), CV_32F, "Data with only depth CV_32F are supported");
CV_CheckChannelsEQ(points.channels(), 1, "Data with only one channel are supported");
CV_CheckEQ(points.rows, 3, "Data with only Mat with 3xN are supported");
// Since error function output squared error distance, so make
// threshold squared as well
double _threshold = threshold * threshold;
int state = (int) rng_state;
const int points_size = points.rows * points.cols / 3;
const double _radius_min = radius_min, _radius_max = radius_max;
const ModelConstraintFunction &_custom_constraint = custom_model_constraints;
ModelConstraintFunction &constraint_func = custom_model_constraints;
// RANSAC
using namespace usac;
SamplingMethod _sampling_method = SamplingMethod::SAMPLING_UNIFORM;
LocalOptimMethod _lo_method = LocalOptimMethod::LOCAL_OPTIM_INNER_LO;
ScoreMethod _score_method = ScoreMethod::SCORE_METHOD_RANSAC;
NeighborSearchMethod _neighbors_search_method = NeighborSearchMethod::NEIGH_GRID;
// Local optimization
int lo_sample_size = 16, lo_inner_iterations = 15, lo_iterative_iterations = 8,
lo_thr_multiplier = 15, lo_iter_sample_size = 30;
Ptr <Sampler> sampler;
Ptr <Quality> quality;
Ptr <ModelVerifier> verifier;
Ptr <LocalOptimization> lo;
Ptr <Degeneracy> degeneracy;
Ptr <Termination> termination;
Ptr <FinalModelPolisher> polisher;
Ptr <MinimalSolver> min_solver;
Ptr <NonMinimalSolver> non_min_solver;
Ptr <Estimator> estimator;
Ptr <usac::Error> error;
EstimationMethod est_method;
switch (sac_model_type)
{
case SAC_MODEL_PLANE:
est_method = EstimationMethod::PLANE;
min_solver = PlaneModelMinimalSolver::create(points);
non_min_solver = PlaneModelNonMinimalSolver::create(points);
error = PlaneModelError::create(points);
break;
// case SAC_MODEL_CYLINDER:
// min_solver = CylinderModelMinimalSolver::create(points);
// non_min_solver = CylinderModelNonMinimalSolver::create(points);
// error = CylinderModelError::create(points);
// break;
case SAC_MODEL_SPHERE:
est_method = EstimationMethod::SPHERE;
min_solver = SphereModelMinimalSolver::create(points);
non_min_solver = SphereModelNonMinimalSolver::create(points);
error = SphereModelError::create(points);
constraint_func = [_radius_min, _radius_max, _custom_constraint]
(const std::vector<double> &model_coeffs) {
double radius = model_coeffs[3];
return radius >= _radius_min && radius <= _radius_max &&
(!_custom_constraint || _custom_constraint(model_coeffs));
};
break;
default:
CV_Error(cv::Error::StsNotImplemented, "SAC_MODEL type is not implemented!");
}
const int min_sample_size = min_solver->getSampleSize();
if (points_size < min_sample_size)
{
return 0;
}
estimator = PointCloudModelEstimator::create(min_solver, non_min_solver, constraint_func);
sampler = UniformSampler::create(state++, min_sample_size, points_size);
quality = RansacQuality::create(points_size, _threshold, error);
verifier = ModelVerifier::create(quality);
Ptr <RandomGenerator> lo_sampler = UniformRandomGenerator::create(state++, points_size,
lo_sample_size);
lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler, points_size,
_threshold, false, lo_iter_sample_size, lo_inner_iterations,
lo_iterative_iterations, lo_thr_multiplier);
degeneracy = makePtr<Degeneracy>();
termination = StandardTerminationCriteria::create
(confidence, points_size, min_sample_size, max_iterations);
Ptr <SimpleUsacConfig> usacConfig = SimpleUsacConfig::create(est_method);
usacConfig->setMaxIterations(max_iterations);
usacConfig->setRandomGeneratorState(state);
usacConfig->setParallel(is_parallel);
usacConfig->setNeighborsSearchMethod(_neighbors_search_method);
usacConfig->setSamplingMethod(_sampling_method);
usacConfig->setScoreMethod(_score_method);
usacConfig->setLoMethod(_lo_method);
// The mask is needed to remove the points of the model that has been segmented
usacConfig->maskRequired(true);
UniversalRANSAC ransac(usacConfig, points_size, estimator, quality, sampler,
termination, verifier, degeneracy, lo, polisher);
Ptr <usac::RansacOutput> ransac_output;
if (!ransac.run(ransac_output))
{
return 0;
}
model_coefficients = ransac_output->getModel();
label = ransac_output->getInliersMask();
return ransac_output->getNumberOfInliers();
}
//-------------------------- segment -----------------------
int
SACSegmentationImpl::segment(InputArray input_pts, OutputArray labels,
OutputArray models_coefficients)
{
Mat points;
// Get Mat with 3xN CV_32F
getPointsMatFromInputArray(input_pts, points, 1);
int pts_size = points.rows * points.cols / 3;
std::vector<int> _labels(pts_size, 0);
std::vector<Mat> _models_coefficients;
// Keep the index array of the point corresponding to the original point
AutoBuffer<int> ori_pts_idx(pts_size);
int *pts_idx_ptr = ori_pts_idx.data();
for (int i = 0; i < pts_size; ++i)
pts_idx_ptr[i] = i;
int min_sample_size = sacModelMinimumSampleSize(sac_model_type);
for (int model_num = 1;
pts_size >= min_sample_size && model_num <= number_of_models_expected; ++model_num)
{
Mat model_coefficients;
std::vector<bool> label;
int best_inls = segmentSingle(points, label, model_coefficients);
if (best_inls < min_sample_size)
break;
_models_coefficients.emplace_back(model_coefficients);
// Move the outlier to the new point cloud and continue to segment the model
if (model_num != number_of_models_expected)
{
cv::Mat tmp_pts(points);
int next_pts_size = pts_size - best_inls;
points = cv::Mat(3, next_pts_size, CV_32F);
// Pointer (base address) of access point data x,y,z
float *const tmp_pts_ptr_x = (float *) tmp_pts.data;
float *const tmp_pts_ptr_y = tmp_pts_ptr_x + pts_size;
float *const tmp_pts_ptr_z = tmp_pts_ptr_y + pts_size;
float *const next_pts_ptr_x = (float *) points.data;
float *const next_pts_ptr_y = next_pts_ptr_x + next_pts_size;
float *const next_pts_ptr_z = next_pts_ptr_y + next_pts_size;
for (int j = 0, k = 0; k < pts_size; ++k)
{
if (label[k])
{
// mark a label on this point
_labels[pts_idx_ptr[k]] = model_num;
}
else
{
// If it is not inlier of the known plane,
// add the next iteration to find a new plane
pts_idx_ptr[j] = pts_idx_ptr[k];
next_pts_ptr_x[j] = tmp_pts_ptr_x[k];
next_pts_ptr_y[j] = tmp_pts_ptr_y[k];
next_pts_ptr_z[j] = tmp_pts_ptr_z[k];
++j;
}
}
pts_size = next_pts_size;
}
else
{
for (int k = 0; k < pts_size; ++k)
{
if (label[k])
_labels[pts_idx_ptr[k]] = model_num;
}
}
}
int number_of_models = (int) _models_coefficients.size();
if (labels.needed())
{
if (number_of_models != 0)
{
Mat(_labels).copyTo(labels);
}
else
{
labels.clear();
}
}
if (models_coefficients.needed())
{
if (number_of_models != 0)
{
Mat result;
for (int i = 0; i < number_of_models; i++)
{
result.push_back(_models_coefficients[i]);
}
result.copyTo(models_coefficients);
}
else
{
models_coefficients.clear();
}
}
return number_of_models;
}
// } // _3d::
} // cv::