mirror of
https://github.com/opencv/opencv.git
synced 2024-12-14 08:59:11 +08:00
0661aff4a5
Fixing typos in usac #23900 Just read and correct some typos in `usac` ### 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 - [x] 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
1471 lines
81 KiB
C++
1471 lines
81 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.
|
|
|
|
#include "../precomp.hpp"
|
|
#include "../usac.hpp"
|
|
#include <atomic>
|
|
|
|
namespace cv {
|
|
UsacParams::UsacParams() {
|
|
confidence=0.99;
|
|
isParallel=false;
|
|
loIterations=5;
|
|
loMethod=LOCAL_OPTIM_INNER_LO;
|
|
loSampleSize=14;
|
|
maxIterations=5000;
|
|
neighborsSearch=NEIGH_GRID;
|
|
randomGeneratorState=0;
|
|
sampler=SAMPLING_UNIFORM;
|
|
score=SCORE_METHOD_MSAC;
|
|
threshold=1.5;
|
|
final_polisher=COV_POLISHER;
|
|
final_polisher_iterations=3;
|
|
}
|
|
|
|
namespace usac {
|
|
int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp);
|
|
void setParameters (int flag, Ptr<Model> ¶ms, EstimationMethod estimator, double thr,
|
|
int max_iters, double conf, bool mask_needed);
|
|
|
|
class RansacOutputImpl : public RansacOutput {
|
|
private:
|
|
std::vector<int> inliers;
|
|
cv::Mat model, K1, K2;
|
|
// vector of number_inliers size
|
|
// vector of points size, true if inlier, false - outlier
|
|
std::vector<bool> inliers_mask;
|
|
// vector of points size, value of i-th index corresponds to error of i-th point if i is inlier.
|
|
std::vector<float> residuals;
|
|
int number_inliers, number_iterations;
|
|
ModelConfidence conf;
|
|
public:
|
|
RansacOutputImpl (const cv::Mat &model_, const std::vector<bool> &inliers_mask_, int number_inliers_,
|
|
int number_iterations_, ModelConfidence conf_, const std::vector<float> &errors_) {
|
|
model_.copyTo(model);
|
|
inliers_mask = inliers_mask_;
|
|
number_inliers = number_inliers_;
|
|
number_iterations = number_iterations_;
|
|
residuals = errors_;
|
|
conf = conf_;
|
|
}
|
|
|
|
// Return inliers' indices of size = number of inliers
|
|
const std::vector<int> &getInliers() override {
|
|
if (inliers.empty()) {
|
|
inliers.reserve(number_inliers);
|
|
int pt_cnt = 0;
|
|
for (bool is_inlier : inliers_mask) {
|
|
if (is_inlier)
|
|
inliers.emplace_back(pt_cnt);
|
|
pt_cnt++;
|
|
}
|
|
}
|
|
return inliers;
|
|
}
|
|
const std::vector<bool> &getInliersMask() const override {
|
|
return inliers_mask;
|
|
}
|
|
int getNumberOfInliers() const override {
|
|
return number_inliers;
|
|
}
|
|
const Mat &getModel() const override {
|
|
return model;
|
|
}
|
|
int getNumberOfIters() const override {
|
|
return number_iterations;
|
|
}
|
|
ModelConfidence getConfidence() const override {
|
|
return conf;
|
|
}
|
|
const std::vector<float> &getResiduals() const override {
|
|
return residuals;
|
|
}
|
|
};
|
|
|
|
Ptr<RansacOutput> RansacOutput::create(const cv::Mat &model_, const std::vector<bool> &inliers_mask_, int number_inliers_,
|
|
int number_iterations_, ModelConfidence conf, const std::vector<float> &errors_) {
|
|
return makePtr<RansacOutputImpl>(model_, inliers_mask_, number_inliers_,
|
|
number_iterations_, conf, errors_);
|
|
}
|
|
|
|
double getLambda (std::vector<int> &supports, double cdf_thr, int points_size,
|
|
int sample_size, bool is_independent, int &min_non_random_inliers) {
|
|
std::sort(supports.begin(), supports.end());
|
|
double lambda = supports.size() % 2 ? (supports[supports.size()/2] + supports[supports.size()/2+1])*0.5 : supports[supports.size()/2];
|
|
const double cdf = lambda + cdf_thr*sqrt(lambda * (1 - lambda / (is_independent ? points_size - sample_size : points_size)));
|
|
int lower_than_cdf = 0; lambda = 0;
|
|
for (const auto &inl : supports)
|
|
if (inl < cdf) {
|
|
lambda += inl; lower_than_cdf++;
|
|
} else break; // list is sorted
|
|
lambda /= lower_than_cdf;
|
|
if (lambda < 1 || lower_than_cdf == 0) lambda = 1;
|
|
// use 0.9999 quantile https://keisan.casio.com/exec/system/14060745333941
|
|
if (! is_independent) // do not calculate it for all inliers
|
|
min_non_random_inliers = (int)(lambda + 2.32*sqrt(lambda * (1 - lambda / points_size))) + 1;
|
|
return lambda;
|
|
}
|
|
|
|
class Ransac {
|
|
public:
|
|
const Ptr<const Model> params;
|
|
Ptr<Estimator> _estimator;
|
|
Ptr<Error> _error;
|
|
Ptr<Quality> _quality;
|
|
Ptr<Sampler> _sampler;
|
|
Ptr<Termination> _termination;
|
|
Ptr<ModelVerifier> _model_verifier;
|
|
Ptr<Degeneracy> _degeneracy;
|
|
Ptr<LocalOptimization> _local_optimization;
|
|
Ptr<FinalModelPolisher> polisher;
|
|
Ptr<GammaValues> _gamma_generator;
|
|
Ptr<MinimalSolver> _min_solver;
|
|
Ptr<NonMinimalSolver> _lo_solver, _fo_solver;
|
|
Ptr<RandomGenerator> _lo_sampler;
|
|
Ptr<WeightFunction> _weight_fnc;
|
|
|
|
int points_size, _state, filtered_points_size;
|
|
double threshold, max_thr;
|
|
bool parallel;
|
|
|
|
Matx33d T1, T2;
|
|
Mat points, K1, K2, calib_points, image_points, norm_points, filtered_points;
|
|
Ptr<NeighborhoodGraph> graph;
|
|
std::vector<Ptr<NeighborhoodGraph>> layers;
|
|
|
|
Ransac (const Ptr<const Model> ¶ms_, cv::InputArray points1, cv::InputArray points2,
|
|
cv::InputArray K1_, cv::InputArray K2_, cv::InputArray dist_coeff1, cv::InputArray dist_coeff2) : params(params_) {
|
|
_state = params->getRandomGeneratorState();
|
|
threshold = params->getThreshold();
|
|
max_thr = std::max(threshold, params->getMaximumThreshold());
|
|
parallel = params->isParallel();
|
|
Mat undist_points1, undist_points2;
|
|
if (params->isPnP()) {
|
|
if (! K1_.empty()) {
|
|
K1 = K1_.getMat().clone(); K1.convertTo(K1, CV_64F);
|
|
if (! dist_coeff1.empty()) {
|
|
// undistortPoints also calibrate points using K
|
|
undistortPoints(points1.isContinuous() ? points1 : points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
|
|
points_size = mergePoints(undist_points1, points2, points, true);
|
|
Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points);
|
|
} else {
|
|
points_size = mergePoints(points1, points2, points, true);
|
|
Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points);
|
|
}
|
|
} else points_size = mergePoints(points1, points2, points, true);
|
|
} else {
|
|
if (params->isEssential()) {
|
|
CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
|
|
K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
|
|
K2 = K2_.getMat(); K2.convertTo(K2, CV_64F);
|
|
if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
|
|
// undistortPoints also calibrate points using K
|
|
if (! dist_coeff1.empty()) undistortPoints(points1.isContinuous() ? points1 : points1.getMat().clone(), undist_points1, K1_, dist_coeff1);
|
|
else undist_points1 = points1.getMat();
|
|
if (! dist_coeff2.empty()) undistortPoints(points2.isContinuous() ? points2 : points2.getMat().clone(), undist_points2, K2_, dist_coeff2);
|
|
else undist_points2 = points2.getMat();
|
|
points_size = mergePoints(undist_points1, undist_points2, calib_points, false);
|
|
} else {
|
|
points_size = mergePoints(points1, points2, points, false);
|
|
Utils::calibratePoints(K1, K2, points, calib_points);
|
|
}
|
|
threshold = Utils::getCalibratedThreshold(threshold, K1, K2);
|
|
max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2);
|
|
} else {
|
|
points_size = mergePoints(points1, points2, points, false);
|
|
if (params->isFundamental() && ! K1_.empty() && ! K2_.empty()) {
|
|
K1 = K1_.getMat(); K1.convertTo(K1, CV_64F);
|
|
K2 = K2_.getMat(); K2.convertTo(K2, CV_64F);
|
|
Utils::calibratePoints(K1, K2, points, calib_points);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) {
|
|
if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) {
|
|
graph = GridNeighborhoodGraph::create(points, points_size,
|
|
params->getCellSize(), params->getCellSize(), params->getCellSize(), params->getCellSize(), 10);
|
|
} else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) {
|
|
graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1);
|
|
} else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) {
|
|
graph = RadiusSearchNeighborhoodGraph::create(points, points_size, params->getGraphRadius(), 5, 1);
|
|
} else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!");
|
|
}
|
|
|
|
if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
|
|
CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!");
|
|
const auto &cell_number_per_layer = params->getGridCellNumber();
|
|
layers.reserve(cell_number_per_layer.size());
|
|
const auto * const pts = (float *) points.data;
|
|
float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0;
|
|
for (int i = 0; i < 4 * points_size; i += 4) {
|
|
if (pts[i ] > img1_width ) img1_width = pts[i ];
|
|
if (pts[i + 1] > img1_height) img1_height = pts[i + 1];
|
|
if (pts[i + 2] > img2_width ) img2_width = pts[i + 2];
|
|
if (pts[i + 3] > img2_height) img2_height = pts[i + 3];
|
|
}
|
|
// Create grid graphs (overlapping layes of given cell numbers)
|
|
for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) {
|
|
const int cell_number = cell_number_per_layer[layer_idx];
|
|
if (layer_idx > 0)
|
|
if (cell_number_per_layer[layer_idx-1] <= cell_number)
|
|
CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: "
|
|
"Cell number in layers must be in decreasing order!");
|
|
layers.emplace_back(GridNeighborhoodGraph::create(points, points_size,
|
|
(int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number),
|
|
(int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10));
|
|
}
|
|
}
|
|
|
|
// update points by calibrated for Essential matrix after graph is calculated
|
|
if (params->isEssential()) {
|
|
image_points = points;
|
|
points = calib_points;
|
|
// if maximum calibrated threshold significanlty differs threshold then set upper bound
|
|
if (max_thr > 10*threshold)
|
|
max_thr = 10*threshold;
|
|
}
|
|
|
|
// Since error function output squared error distance, so make
|
|
// threshold squared as well
|
|
threshold *= threshold;
|
|
|
|
if ((params->isHomography() || (params->isFundamental() && (K1.empty() || K2.empty() || !params->isLarssonOptimization())) ||
|
|
params->getEstimator() == EstimationMethod::AFFINE) && (params->getLO() != LOCAL_OPTIM_NULL || params->getFinalPolisher() == COV_POLISHER)) {
|
|
const auto normTr = NormTransform::create(points);
|
|
std::vector<int> sample (points_size);
|
|
for (int i = 0; i < points_size; i++) sample[i] = i;
|
|
normTr->getNormTransformation(norm_points, sample, points_size, T1, T2);
|
|
}
|
|
|
|
if (params->getScore() == SCORE_METHOD_MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA || params->getFinalPolisher() == MAGSAC)
|
|
_gamma_generator = GammaValues::create(params->getDegreesOfFreedom()); // is thread safe
|
|
initialize (_state, _min_solver, _lo_solver, _error, _estimator, _degeneracy, _quality,
|
|
_model_verifier, _local_optimization, _termination, _sampler, _lo_sampler, _weight_fnc, false/*parallel*/);
|
|
if (params->getFinalPolisher() != NONE_POLISHER) {
|
|
polisher = NonMinimalPolisher::create(_quality, _fo_solver,
|
|
params->getFinalPolisher() == MAGSAC ? _weight_fnc : nullptr, params->getFinalLSQIterations(), 0.99);
|
|
}
|
|
}
|
|
|
|
void initialize (int state, Ptr<MinimalSolver> &min_solver, Ptr<NonMinimalSolver> &non_min_solver,
|
|
Ptr<Error> &error, Ptr<Estimator> &estimator, Ptr<Degeneracy> °eneracy, Ptr<Quality> &quality,
|
|
Ptr<ModelVerifier> &verifier, Ptr<LocalOptimization> &lo, Ptr<Termination> &termination,
|
|
Ptr<Sampler> &sampler, Ptr<RandomGenerator> &lo_sampler, Ptr<WeightFunction> &weight_fnc, bool parallel_call) {
|
|
|
|
const int min_sample_size = params->getSampleSize(), prosac_termination_length = std::min((int)(.5*points_size), 100);
|
|
// inner inlier threshold will be used in LO to obtain inliers
|
|
// additionally in DEGENSAC for F
|
|
double inner_inlier_thr_sqr = threshold;
|
|
if (params->isHomography() && inner_inlier_thr_sqr < 5.25) inner_inlier_thr_sqr = 5.25; // at least 2.5 px
|
|
else if (params->isFundamental() && inner_inlier_thr_sqr < 4) inner_inlier_thr_sqr = 4; // at least 2 px
|
|
|
|
if (params->getFinalPolisher() == MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA)
|
|
weight_fnc = MagsacWeightFunction::create(_gamma_generator, params->getDegreesOfFreedom(), params->getUpperIncompleteOfSigmaQuantile(), params->getC(), params->getMaximumThreshold());
|
|
else weight_fnc = nullptr;
|
|
|
|
switch (params->getError()) {
|
|
case ErrorMetric::SYMM_REPR_ERR:
|
|
error = ReprojectionErrorSymmetric::create(points); break;
|
|
case ErrorMetric::FORW_REPR_ERR:
|
|
if (params->getEstimator() == EstimationMethod::AFFINE)
|
|
error = ReprojectionErrorAffine::create(points);
|
|
else error = ReprojectionErrorForward::create(points);
|
|
break;
|
|
case ErrorMetric::SAMPSON_ERR:
|
|
error = SampsonError::create(points); break;
|
|
case ErrorMetric::SGD_ERR:
|
|
error = SymmetricGeometricDistance::create(points); break;
|
|
case ErrorMetric::RERPOJ:
|
|
error = ReprojectionErrorPmatrix::create(points); break;
|
|
default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!");
|
|
}
|
|
|
|
const double k_mlesac = params->getKmlesac ();
|
|
switch (params->getScore()) {
|
|
case ScoreMethod::SCORE_METHOD_RANSAC :
|
|
quality = RansacQuality::create(points_size, threshold, error); break;
|
|
case ScoreMethod::SCORE_METHOD_MSAC :
|
|
quality = MsacQuality::create(points_size, threshold, error, k_mlesac); break;
|
|
case ScoreMethod::SCORE_METHOD_MAGSAC :
|
|
quality = MagsacQuality::create(max_thr, points_size, error, _gamma_generator,
|
|
threshold, params->getDegreesOfFreedom(), params->getSigmaQuantile(),
|
|
params->getUpperIncompleteOfSigmaQuantile()); break;
|
|
case ScoreMethod::SCORE_METHOD_LMEDS :
|
|
quality = LMedsQuality::create(points_size, threshold, error); break;
|
|
default: CV_Error(cv::Error::StsNotImplemented, "Score is not implemented!");
|
|
}
|
|
|
|
const auto is_ge_solver = params->getRansacSolver() == GEM_SOLVER;
|
|
if (params->isHomography()) {
|
|
degeneracy = HomographyDegeneracy::create(points);
|
|
min_solver = HomographyMinimalSolver4pts::create(points, is_ge_solver);
|
|
non_min_solver = HomographyNonMinimalSolver::create(norm_points, T1, T2, true);
|
|
estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy);
|
|
if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
|
|
if (params->getFinalPolisher() == COV_POLISHER)
|
|
_fo_solver = CovarianceHomographySolver::create(norm_points, T1, T2);
|
|
else _fo_solver = HomographyNonMinimalSolver::create(points);
|
|
}
|
|
} else if (params->isFundamental()) {
|
|
if (K1.empty() || K2.empty()) {
|
|
degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size,
|
|
params->getPlaneAndParallaxIters(), std::max(threshold, 8.) /*sqr homogr thr*/, inner_inlier_thr_sqr, K1, K2);
|
|
} else degeneracy = FundamentalDegeneracyViaE::create(quality, points, calib_points, K1, K2, true/*is F*/);
|
|
if (min_sample_size == 7) {
|
|
min_solver = FundamentalMinimalSolver7pts::create(points, is_ge_solver);
|
|
} else min_solver = FundamentalMinimalSolver8pts::create(points);
|
|
if (params->isLarssonOptimization() && !K1.empty() && !K2.empty()) {
|
|
non_min_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqItersLO(), true/*F*/);
|
|
} else {
|
|
if (weight_fnc)
|
|
non_min_solver = EpipolarNonMinimalSolver::create(points, true);
|
|
else
|
|
non_min_solver = EpipolarNonMinimalSolver::create(norm_points, T1, T2, true);
|
|
}
|
|
estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy);
|
|
if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
|
|
if (params->isLarssonOptimization() && !K1.empty() && !K2.empty())
|
|
_fo_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqIters(), true/*F*/);
|
|
else if (params->getFinalPolisher() == COV_POLISHER)
|
|
_fo_solver = CovarianceEpipolarSolver::create(norm_points, T1, T2);
|
|
else _fo_solver = EpipolarNonMinimalSolver::create(points, true);
|
|
}
|
|
} else if (params->isEssential()) {
|
|
if (params->getEstimator() == EstimationMethod::ESSENTIAL) {
|
|
min_solver = EssentialMinimalSolver5pts::create(points, !is_ge_solver, true/*Nister*/);
|
|
degeneracy = EssentialDegeneracy::create(points, min_sample_size);
|
|
}
|
|
non_min_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqItersLO(), false/*E*/);
|
|
estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy);
|
|
if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER)
|
|
_fo_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqIters(), false/*E*/);
|
|
} else if (params->isPnP()) {
|
|
degeneracy = makePtr<Degeneracy>();
|
|
if (min_sample_size == 3) {
|
|
min_solver = P3PSolver::create(points, calib_points, K1);
|
|
non_min_solver = DLSPnP::create(points, calib_points, K1);
|
|
} else {
|
|
if (is_ge_solver)
|
|
min_solver = PnPMinimalSolver6Pts::create(points);
|
|
else min_solver = PnPSVDSolver::create(points);
|
|
non_min_solver = PnPNonMinimalSolver::create(points);
|
|
}
|
|
estimator = PnPEstimator::create(min_solver, non_min_solver);
|
|
if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) _fo_solver = non_min_solver;
|
|
} else if (params->getEstimator() == EstimationMethod::AFFINE) {
|
|
degeneracy = makePtr<Degeneracy>();
|
|
min_solver = AffineMinimalSolver::create(points);
|
|
non_min_solver = AffineNonMinimalSolver::create(points, cv::noArray(), cv::noArray());
|
|
estimator = AffineEstimator::create(min_solver, non_min_solver);
|
|
if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
|
|
if (params->getFinalPolisher() == COV_POLISHER)
|
|
_fo_solver = CovarianceAffineSolver::create(points);
|
|
else _fo_solver = non_min_solver;
|
|
}
|
|
} else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!");
|
|
|
|
switch (params->getSampler()) {
|
|
case SamplingMethod::SAMPLING_UNIFORM:
|
|
sampler = UniformSampler::create(state++, min_sample_size, points_size);
|
|
break;
|
|
case SamplingMethod::SAMPLING_PROSAC:
|
|
if (!parallel_call) // for parallel only one PROSAC sampler
|
|
sampler = ProsacSampler::create(state++, points_size, min_sample_size, params->getProsacMaxSamples());
|
|
break;
|
|
case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC:
|
|
sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break;
|
|
case SamplingMethod::SAMPLING_NAPSAC:
|
|
sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break;
|
|
default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!");
|
|
}
|
|
|
|
const bool is_sprt = params->getVerifier() == VerificationMethod::SPRT_VERIFIER || params->getVerifier() == VerificationMethod::ASPRT;
|
|
if (is_sprt)
|
|
verifier = AdaptiveSPRT::create(state++, quality, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold,
|
|
params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(),
|
|
params->getSPRTavgNumModels(), params->getScore(), k_mlesac, params->getVerifier() == VerificationMethod::ASPRT);
|
|
else if (params->getVerifier() == VerificationMethod::NULL_VERIFIER)
|
|
verifier = ModelVerifier::create(quality);
|
|
else CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!");
|
|
|
|
if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) {
|
|
if (parallel_call) {
|
|
termination = ProsacTerminationCriteria::create(nullptr, error,
|
|
points_size, min_sample_size, params->getConfidence(), params->getMaxIters(), prosac_termination_length, 0.05, 0.05, threshold,
|
|
_termination.dynamicCast<ProsacTerminationCriteria>()->getNonRandomInliers());
|
|
} else {
|
|
termination = ProsacTerminationCriteria::create(sampler.dynamicCast<ProsacSampler>(), error,
|
|
points_size, min_sample_size, params->getConfidence(), params->getMaxIters(), prosac_termination_length, 0.05, 0.05, threshold,
|
|
std::vector<int>());
|
|
}
|
|
} else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
|
|
if (is_sprt)
|
|
termination = SPRTPNapsacTermination::create(verifier.dynamicCast<AdaptiveSPRT>(),
|
|
params->getConfidence(), points_size, min_sample_size,
|
|
params->getMaxIters(), params->getRelaxCoef());
|
|
else termination = StandardTerminationCriteria::create (params->getConfidence(),
|
|
points_size, min_sample_size, params->getMaxIters());
|
|
} else if (is_sprt && params->getLO() == LocalOptimMethod::LOCAL_OPTIM_NULL) {
|
|
termination = SPRTTermination::create(verifier.dynamicCast<AdaptiveSPRT>(),
|
|
params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
|
|
} else {
|
|
termination = StandardTerminationCriteria::create
|
|
(params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
|
|
}
|
|
|
|
// if normal ransac or parallel call, avoid redundant init
|
|
if ((! params->isParallel() || parallel_call) && params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) {
|
|
lo_sampler = UniformRandomGenerator::create(state, points_size, params->getLOSampleSize());
|
|
const auto lo_termination = StandardTerminationCriteria::create(params->getConfidence(), points_size, min_sample_size, params->getMaxIters());
|
|
switch (params->getLO()) {
|
|
case LocalOptimMethod::LOCAL_OPTIM_INNER_LO: case LocalOptimMethod::LOCAL_OPTIM_SIGMA:
|
|
lo = SimpleLocalOptimization::create(quality, non_min_solver, lo_termination, lo_sampler,
|
|
weight_fnc, params->getLOInnerMaxIters(), inner_inlier_thr_sqr, true); break;
|
|
case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO:
|
|
lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler,
|
|
points_size, threshold, true, params->getLOIterativeSampleSize(),
|
|
params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(),
|
|
params->getLOThresholdMultiplier()); break;
|
|
case LocalOptimMethod::LOCAL_OPTIM_GC:
|
|
lo = GraphCut::create(estimator, quality, graph, lo_sampler, threshold,
|
|
params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters(), lo_termination); break;
|
|
default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!");
|
|
}
|
|
}
|
|
}
|
|
|
|
int getIndependentInliers (const Mat &model_, const std::vector<int> &sample,
|
|
std::vector<int> &inliers, const int num_inliers_) {
|
|
bool is_F = params->isFundamental();
|
|
Mat model = model_;
|
|
int sample_size = 0;
|
|
if (is_F) sample_size = 7;
|
|
else if (params->isHomography()) sample_size = 4;
|
|
else if (params->isEssential()) {
|
|
is_F = true;
|
|
// convert E to F
|
|
model = Mat(Matx33d(K2).inv().t() * Matx33d(model) * Matx33d(K1).inv());
|
|
sample_size = 5;
|
|
} else if (params->isPnP() || params->getEstimator() == EstimationMethod::AFFINE) sample_size = 3;
|
|
else
|
|
CV_Error(cv::Error::StsNotImplemented, "Method for independent inliers is not implemented for this problem");
|
|
if (num_inliers_ <= sample_size) return 0; // minimal sample size generates model
|
|
model.convertTo(model, CV_32F);
|
|
int num_inliers = num_inliers_, num_pts_near_ep = 0,
|
|
num_pts_validatin_or_constr = 0, pt1 = 0;
|
|
const auto * const pts = params->isEssential() ? (float *) image_points.data : (float *) points.data;
|
|
// scale for thresholds should be used
|
|
const float ep_thr_sqr = 0.000001f, line_thr = 0.01f, neigh_thr = 4.0f;
|
|
float sign1=0,a1=0, b1=0, c1=0, a2=0, b2=0, c2=0, ep1_x, ep1_y, ep2_x, ep2_y;
|
|
const auto * const m = (float *) model.data;
|
|
Vec3f ep1;
|
|
bool do_or_test = false, ep1_inf = false, ep2_inf = false;
|
|
if (is_F) { // compute epipole and sign of the first point for orientation test
|
|
model *= (1/norm(model));
|
|
ep1 = Utils::getRightEpipole(model);
|
|
const Vec3f ep2 = Utils::getLeftEpipole(model);
|
|
if (fabsf(ep1[2]) < DBL_EPSILON) {
|
|
ep1_inf = true;
|
|
} else {
|
|
ep1_x = ep1[0] / ep1[2];
|
|
ep1_y = ep1[1] / ep1[2];
|
|
}
|
|
if (fabsf(ep2[2]) < DBL_EPSILON) {
|
|
ep2_inf = true;
|
|
} else {
|
|
ep2_x = ep2[0] / ep2[2];
|
|
ep2_y = ep2[1] / ep2[2];
|
|
}
|
|
}
|
|
const auto * const e1 = ep1.val; // of size 3x1
|
|
|
|
// we move sample points to the end, so every inlier will be checked by sample point
|
|
int num_sample_in_inliers = 0;
|
|
if (!sample.empty()) {
|
|
num_sample_in_inliers = 0;
|
|
int temp_idx = num_inliers;
|
|
for (int i = 0; i < temp_idx; i++) {
|
|
const int inl = inliers[i];
|
|
for (int s : sample) {
|
|
if (inl == s) {
|
|
std::swap(inliers[i], inliers[--temp_idx]);
|
|
i--; // we need to check inlier that we just swapped
|
|
num_sample_in_inliers++;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (is_F) {
|
|
int MIN_TEST = std::min(15, num_inliers);
|
|
for (int i = 0; i < MIN_TEST; i++) {
|
|
pt1 = 4*inliers[i];
|
|
sign1 = (m[0]*pts[pt1+2]+m[3]*pts[pt1+3]+m[6])*(e1[1]-e1[2]*pts[pt1+1]);
|
|
int validate = 0;
|
|
for (int j = 0; j < MIN_TEST; j++) {
|
|
if (i == j) continue;
|
|
const int inl_idx = 4*inliers[j];
|
|
if (sign1*(m[0]*pts[inl_idx+2]+m[3]*pts[inl_idx+3]+m[6])*(e1[1]-e1[2]*pts[inl_idx+1])<0)
|
|
validate++;
|
|
}
|
|
if (validate < MIN_TEST/2) {
|
|
do_or_test = true; break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// verification does not include sample points as they are surely random
|
|
const int max_verify = num_inliers - num_sample_in_inliers;
|
|
if (max_verify <= 0)
|
|
return 0;
|
|
int num_non_random_inliers = num_inliers - sample_size;
|
|
auto removeDependentPoints = [&] (bool do_orient_test, bool check_epipoles) {
|
|
for (int i = 0; i < max_verify; i++) {
|
|
// checks over inliers if they are dependent to other inliers
|
|
const int inl_idx = 4*inliers[i];
|
|
const auto x1 = pts[inl_idx], y1 = pts[inl_idx+1], x2 = pts[inl_idx+2], y2 = pts[inl_idx+3];
|
|
if (is_F) {
|
|
// epipolar line on image 2 = l2
|
|
a2 = m[0] * x1 + m[1] * y1 + m[2];
|
|
b2 = m[3] * x1 + m[4] * y1 + m[5];
|
|
c2 = m[6] * x1 + m[7] * y1 + m[8];
|
|
// epipolar line on image 1 = l1
|
|
a1 = m[0] * x2 + m[3] * y2 + m[6];
|
|
b1 = m[1] * x2 + m[4] * y2 + m[7];
|
|
c1 = m[2] * x2 + m[5] * y2 + m[8];
|
|
if ((!ep1_inf && fabsf(x1-ep1_x)+fabsf(y1-ep1_y) < neigh_thr) ||
|
|
(!ep2_inf && fabsf(x2-ep2_x)+fabsf(y2-ep2_y) < neigh_thr)) {
|
|
num_non_random_inliers--;
|
|
num_pts_near_ep++;
|
|
continue; // is dependent, continue to the next point
|
|
} else if (check_epipoles) {
|
|
if (a2 * a2 + b2 * b2 + c2 * c2 < ep_thr_sqr ||
|
|
a1 * a1 + b1 * b1 + c1 * c1 < ep_thr_sqr) {
|
|
num_non_random_inliers--;
|
|
num_pts_near_ep++;
|
|
continue; // is dependent, continue to the next point
|
|
}
|
|
}
|
|
else if (do_orient_test && pt1 != inl_idx && sign1*(m[0]*x2+m[3]*y2+m[6])*(e1[1]-e1[2]*y1)<0) {
|
|
num_non_random_inliers--;
|
|
num_pts_validatin_or_constr++;
|
|
continue;
|
|
}
|
|
const auto mag2 = 1 / sqrt(a2 * a2 + b2 * b2), mag1 = 1/sqrt(a1 * a1 + b1 * b1);
|
|
a2 *= mag2; b2 *= mag2; c2 *= mag2;
|
|
a1 *= mag1; b1 *= mag1; c1 *= mag1;
|
|
}
|
|
|
|
for (int j = i+1; j < num_inliers; j++) {// verify through all including sample points
|
|
const int inl_idx_j = 4*inliers[j];
|
|
const auto X1 = pts[inl_idx_j], Y1 = pts[inl_idx_j+1], X2 = pts[inl_idx_j+2], Y2 = pts[inl_idx_j+3];
|
|
// use L1 norm instead of L2 for faster evaluation
|
|
if (fabsf(X1-x1) + fabsf(Y1 - y1) < neigh_thr || fabsf(X2-x2) + fabsf(Y2 - y2) < neigh_thr) {
|
|
num_non_random_inliers--;
|
|
// num_pts_bad_conditioning++;
|
|
break; // is dependent stop verification
|
|
} else if (is_F) {
|
|
if (fabsf(a2 * X2 + b2 * Y2 + c2) < line_thr && //|| // xj'^T F xi
|
|
fabsf(a1 * X1 + b1 * Y1 + c1) < line_thr) { // xj^T F^T xi'
|
|
num_non_random_inliers--;
|
|
break; // is dependent stop verification
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
if (params->isPnP()) {
|
|
for (int i = 0; i < max_verify; i++) {
|
|
const int inl_idx = 5*inliers[i];
|
|
const auto x = pts[inl_idx], y = pts[inl_idx+1], X = pts[inl_idx+2], Y = pts[inl_idx+3], Z = pts[inl_idx+4];
|
|
for (int j = i+1; j < num_inliers; j++) {
|
|
const int inl_idx_j = 5*inliers[j];
|
|
if (fabsf(x-pts[inl_idx_j ]) + fabsf(y-pts[inl_idx_j+1]) < neigh_thr ||
|
|
fabsf(X-pts[inl_idx_j+2]) + fabsf(Y-pts[inl_idx_j+3]) + fabsf(Z-pts[inl_idx_j+4]) < neigh_thr) {
|
|
num_non_random_inliers--;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
removeDependentPoints(do_or_test, !ep1_inf && !ep2_inf);
|
|
if (is_F) {
|
|
const bool is_pts_vald_constr_normal = (double)num_pts_validatin_or_constr / num_inliers < 0.6;
|
|
const bool is_pts_near_ep_normal = (double)num_pts_near_ep / num_inliers < 0.6;
|
|
if (!is_pts_near_ep_normal || !is_pts_vald_constr_normal) {
|
|
num_non_random_inliers = num_inliers-sample_size;
|
|
num_pts_near_ep = 0; num_pts_validatin_or_constr = 0;
|
|
removeDependentPoints(is_pts_vald_constr_normal, is_pts_near_ep_normal);
|
|
}
|
|
}
|
|
}
|
|
return num_non_random_inliers;
|
|
}
|
|
|
|
bool run(Ptr<RansacOutput> &ransac_output) {
|
|
if (points_size < params->getSampleSize())
|
|
return false;
|
|
const bool LO = params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL,
|
|
IS_FUNDAMENTAL = params->isFundamental(), IS_NON_RAND_TEST = params->isNonRandomnessTest();
|
|
const int MAX_MODELS_ADAPT = 21, MAX_ITERS_ADAPT = MAX_MODELS_ADAPT/*assume at least 1 model from 1 sample*/,
|
|
sample_size = params->getSampleSize();
|
|
const double IOU_SIMILARITY_THR = 0.80;
|
|
std::vector<int> non_degen_sample, best_sample;
|
|
|
|
double lambda_non_random_all_inliers = -1;
|
|
int final_iters, num_total_tested_models = 0;
|
|
|
|
// non-random
|
|
const int MAX_TEST_MODELS_NONRAND = IS_NON_RAND_TEST ? MAX_MODELS_ADAPT : 0;
|
|
std::vector<Mat> models_for_random_test; models_for_random_test.reserve(MAX_TEST_MODELS_NONRAND);
|
|
std::vector<std::vector<int>> samples_for_random_test; samples_for_random_test.reserve(MAX_TEST_MODELS_NONRAND);
|
|
|
|
bool last_model_from_LO = false;
|
|
Mat best_model, best_model_not_from_LO, K1_approx, K2_approx;
|
|
Score best_score, best_score_model_not_from_LO;
|
|
std::vector<bool> best_inliers_mask(points_size);
|
|
if (! parallel) {
|
|
// adaptive sprt test
|
|
double IoU = 0, mean_num_est_models = 0;
|
|
bool adapt = IS_NON_RAND_TEST || params->getVerifier() == VerificationMethod ::ASPRT, was_LO_run = false;
|
|
int min_non_random_inliers = 30, iters = 0, num_estimations = 0, max_iters = params->getMaxIters();
|
|
Mat non_degenerate_model, lo_model;
|
|
Score current_score, non_degenerate_model_score, best_score_sample;
|
|
std::vector<bool> model_inliers_mask (points_size);
|
|
std::vector<Mat> models(_estimator->getMaxNumSolutions());
|
|
std::vector<int> sample(_estimator->getMinimalSampleSize()), supports;
|
|
supports.reserve(3*MAX_MODELS_ADAPT); // store model supports during adaption
|
|
auto update_best = [&] (const Mat &new_model, const Score &new_score, bool from_lo=false) {
|
|
_quality->getInliers(new_model, model_inliers_mask);
|
|
IoU = Utils::intersectionOverUnion(best_inliers_mask, model_inliers_mask);
|
|
best_inliers_mask = model_inliers_mask;
|
|
|
|
if (!best_model.empty() && (int)models_for_random_test.size() < MAX_TEST_MODELS_NONRAND && IoU < IOU_SIMILARITY_THR && !from_lo) { // use IoU to not save similar models
|
|
// save old best model for non-randomness test if necessary
|
|
models_for_random_test.emplace_back(best_model.clone());
|
|
samples_for_random_test.emplace_back(best_sample);
|
|
}
|
|
|
|
// update score, model, inliers and max iterations
|
|
best_score = new_score;
|
|
new_model.copyTo(best_model);
|
|
|
|
if (!from_lo) {
|
|
best_sample = sample;
|
|
if (IS_FUNDAMENTAL) { // avoid degeneracy after LO run
|
|
// save last model not from LO
|
|
best_model.copyTo(best_model_not_from_LO);
|
|
best_score_model_not_from_LO = best_score;
|
|
}
|
|
}
|
|
|
|
_model_verifier->update(best_score, iters);
|
|
max_iters = _termination->update(best_model, best_score.inlier_number);
|
|
// max_iters = std::max(max_iters, std::min(10, params->getMaxIters()));
|
|
if (!adapt) // update quality and verifier to save evaluation time of a model
|
|
_quality->setBestScore(best_score.score);
|
|
last_model_from_LO = from_lo;
|
|
};
|
|
|
|
auto run_lo = [&] (const Mat &_model, const Score &_score, bool force_lo) {
|
|
was_LO_run = true;
|
|
_local_optimization->setCurrentRANSACiter(force_lo ? iters : -1);
|
|
Score lo_score;
|
|
if (_local_optimization->refineModel(_model, _score, lo_model, lo_score) && lo_score.isBetter(best_score))
|
|
update_best(lo_model, lo_score, true);
|
|
};
|
|
|
|
for (; iters < max_iters; iters++) {
|
|
_sampler->generateSample(sample);
|
|
int number_of_models;
|
|
if (adapt) {
|
|
number_of_models = _estimator->estimateModels(sample, models);
|
|
mean_num_est_models += number_of_models;
|
|
num_estimations++;
|
|
} else {
|
|
number_of_models = _estimator->estimateModels(sample, models);
|
|
}
|
|
for (int i = 0; i < number_of_models; i++) {
|
|
num_total_tested_models++;
|
|
if (adapt) {
|
|
current_score = _quality->getScore(models[i]);
|
|
supports.emplace_back(current_score.inlier_number);
|
|
if (IS_NON_RAND_TEST && best_score_sample.isBetter(current_score)) {
|
|
models_for_random_test.emplace_back(models[i].clone());
|
|
samples_for_random_test.emplace_back(sample);
|
|
}
|
|
} else {
|
|
if (! _model_verifier->isModelGood(models[i], current_score))
|
|
continue;
|
|
}
|
|
if (current_score.isBetter(best_score_sample)) {
|
|
if (_degeneracy->recoverIfDegenerate(sample, models[i], current_score,
|
|
non_degenerate_model, non_degenerate_model_score)) {
|
|
// check if best non degenerate model is better than so far the best model
|
|
if (non_degenerate_model_score.isBetter(best_score)) {
|
|
update_best(non_degenerate_model, non_degenerate_model_score);
|
|
best_score_sample = current_score.isBetter(best_score) ? best_score : current_score;
|
|
} else continue;
|
|
} else {
|
|
best_score_sample = current_score;
|
|
update_best(models[i], current_score);
|
|
}
|
|
|
|
if (LO && ((iters < max_iters && best_score.inlier_number > min_non_random_inliers && IoU < IOU_SIMILARITY_THR)))
|
|
run_lo(best_model, best_score, false);
|
|
|
|
} // end of if so far the best score
|
|
} // end loop of number of models
|
|
if (adapt && iters >= MAX_ITERS_ADAPT && num_total_tested_models >= MAX_MODELS_ADAPT) {
|
|
adapt = false;
|
|
lambda_non_random_all_inliers = getLambda(supports, 2.32, points_size, sample_size, false, min_non_random_inliers);
|
|
_model_verifier->updateSPRT(params->getTimeForModelEstimation(), 1.0, mean_num_est_models/num_estimations, lambda_non_random_all_inliers/points_size,(double)std::max(min_non_random_inliers, best_score.inlier_number)/points_size, best_score);
|
|
}
|
|
} // end main while loop
|
|
final_iters = iters;
|
|
if (! was_LO_run && !best_model.empty() && LO)
|
|
run_lo(best_model, best_score, true);
|
|
} else { // parallel VSAC
|
|
const int MAX_THREADS = getNumThreads(), growth_max_samples = params->getProsacMaxSamples();
|
|
const bool is_prosac = params->getSampler() == SamplingMethod::SAMPLING_PROSAC;
|
|
std::atomic_bool success(false);
|
|
std::atomic_int num_hypothesis_tested(0), thread_cnt(0), max_number_inliers(0), subset_size, termination_length;
|
|
std::atomic<double> best_score_all(std::numeric_limits<double>::max());
|
|
std::vector<Score> best_scores(MAX_THREADS), best_scores_not_LO;
|
|
std::vector<Mat> best_models(MAX_THREADS), best_models_not_LO, K1_apx, K2_apx;
|
|
std::vector<int> num_tested_models_threads(MAX_THREADS), growth_function, non_random_inliers;
|
|
std::vector<std::vector<Mat>> tested_models_threads(MAX_THREADS);
|
|
std::vector<std::vector<std::vector<int>>> tested_samples_threads(MAX_THREADS);
|
|
std::vector<std::vector<int>> best_samples_threads(MAX_THREADS);
|
|
std::vector<bool> last_model_from_LO_vec;
|
|
std::vector<double> lambda_non_random_all_inliers_vec(MAX_THREADS);
|
|
if (IS_FUNDAMENTAL) {
|
|
last_model_from_LO_vec = std::vector<bool>(MAX_THREADS);
|
|
best_models_not_LO = std::vector<Mat>(MAX_THREADS);
|
|
best_scores_not_LO = std::vector<Score>(MAX_THREADS);
|
|
K1_apx = std::vector<Mat>(MAX_THREADS);
|
|
K2_apx = std::vector<Mat>(MAX_THREADS);
|
|
}
|
|
if (is_prosac) {
|
|
growth_function = _sampler.dynamicCast<ProsacSampler>()->getGrowthFunction();
|
|
subset_size = 2*sample_size; // n, size of the current sampling pool
|
|
termination_length = points_size;
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
parallel_for_(Range(0, MAX_THREADS), [&](const Range & /*range*/) {
|
|
if (!success) { // cover all if not success to avoid thread creating new variables
|
|
const int thread_rng_id = thread_cnt++;
|
|
bool adapt = params->getVerifier() == VerificationMethod ::ASPRT || IS_NON_RAND_TEST;
|
|
int thread_state = _state + thread_rng_id, min_non_random_inliers = 0, num_tested_models = 0,
|
|
num_estimations = 0, mean_num_est_models = 0, iters, max_iters = params->getMaxIters();
|
|
double IoU = 0, lambda_non_random_all_inliers_thread = -1;
|
|
std::vector<Mat> tested_models_thread; tested_models_thread.reserve(MAX_TEST_MODELS_NONRAND);
|
|
std::vector<std::vector<int>> tested_samples_thread; tested_samples_thread.reserve(MAX_TEST_MODELS_NONRAND);
|
|
Ptr<UniformRandomGenerator> random_gen;
|
|
if (is_prosac) random_gen = UniformRandomGenerator::create(thread_state);
|
|
Ptr<Error> error;
|
|
Ptr<Estimator> estimator;
|
|
Ptr<Degeneracy> degeneracy;
|
|
Ptr<Quality> quality;
|
|
Ptr<ModelVerifier> model_verifier;
|
|
Ptr<Sampler> sampler;
|
|
Ptr<RandomGenerator> lo_sampler;
|
|
Ptr<Termination> termination;
|
|
Ptr<LocalOptimization> local_optimization;
|
|
Ptr<MinimalSolver> min_solver;
|
|
Ptr<NonMinimalSolver> non_min_solver;
|
|
Ptr<WeightFunction> weight_fnc;
|
|
initialize (thread_state, min_solver, non_min_solver, error, estimator, degeneracy, quality,
|
|
model_verifier, local_optimization, termination, sampler, lo_sampler, weight_fnc, true);
|
|
bool is_last_from_LO_thread = false;
|
|
Mat best_model_thread, non_degenerate_model, lo_model, best_not_LO_thread;
|
|
Score best_score_thread, current_score, non_denegenerate_model_score, lo_score,best_score_all_threads, best_not_LO_score_thread;
|
|
std::vector<int> sample(estimator->getMinimalSampleSize()), best_sample_thread, supports;
|
|
supports.reserve(3*MAX_MODELS_ADAPT); // store model supports
|
|
std::vector<bool> best_inliers_mask_local(points_size, false), model_inliers_mask(points_size, false);
|
|
std::vector<Mat> models(estimator->getMaxNumSolutions());
|
|
auto update_best = [&] (const Score &new_score, const Mat &new_model, bool from_LO=false) {
|
|
// update best score of all threads
|
|
if (max_number_inliers < new_score.inlier_number) max_number_inliers = new_score.inlier_number;
|
|
if (best_score_all > new_score.score) best_score_all = new_score.score;
|
|
best_score_all_threads = Score(max_number_inliers, best_score_all);
|
|
//
|
|
quality->getInliers(new_model, model_inliers_mask);
|
|
IoU = Utils::intersectionOverUnion(best_inliers_mask_local, model_inliers_mask);
|
|
if (!best_model_thread.empty() && (int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND && IoU < IOU_SIMILARITY_THR) {
|
|
tested_models_thread.emplace_back(best_model_thread.clone());
|
|
tested_samples_thread.emplace_back(best_sample_thread);
|
|
}
|
|
if (!adapt) { // update quality and verifier
|
|
quality->setBestScore(best_score_all);
|
|
model_verifier->update(best_score_all_threads, iters);
|
|
}
|
|
// copy new score to best score
|
|
best_score_thread = new_score;
|
|
best_sample_thread = sample;
|
|
best_inliers_mask_local = model_inliers_mask;
|
|
// remember best model
|
|
new_model.copyTo(best_model_thread);
|
|
|
|
// update upper bound of iterations
|
|
if (is_prosac) {
|
|
int new_termination_length;
|
|
max_iters = termination.dynamicCast<ProsacTerminationCriteria>()->
|
|
updateTerminationLength(best_model_thread, best_score_thread.inlier_number, new_termination_length);
|
|
// update termination length
|
|
if (new_termination_length < termination_length)
|
|
termination_length = new_termination_length;
|
|
} else max_iters = termination->update(best_model_thread, max_number_inliers);
|
|
if (IS_FUNDAMENTAL) {
|
|
is_last_from_LO_thread = from_LO;
|
|
if (!from_LO) {
|
|
best_model_thread.copyTo(best_not_LO_thread);
|
|
best_not_LO_score_thread = best_score_thread;
|
|
}
|
|
}
|
|
};
|
|
bool was_LO_run = false;
|
|
auto runLO = [&] (int current_ransac_iters) {
|
|
was_LO_run = true;
|
|
local_optimization->setCurrentRANSACiter(current_ransac_iters);
|
|
if (local_optimization->refineModel(best_model_thread, best_score_thread, lo_model,
|
|
lo_score) && lo_score.isBetter(best_score_thread))
|
|
update_best(lo_score, lo_model, true);
|
|
};
|
|
for (iters = 0; iters < max_iters && !success; iters++) {
|
|
success = num_hypothesis_tested++ > max_iters;
|
|
if (iters % 10 && !adapt) {
|
|
// Synchronize threads. just to speed verification of model.
|
|
quality->setBestScore(std::min(best_score_thread.score, (double)best_score_all));
|
|
model_verifier->update(best_score_thread.inlier_number > max_number_inliers ? best_score_thread : best_score_all_threads, iters);
|
|
}
|
|
|
|
if (is_prosac) {
|
|
if (num_hypothesis_tested > growth_max_samples) {
|
|
// if PROSAC has not converged to solution then do uniform sampling.
|
|
random_gen->generateUniqueRandomSet(sample, sample_size, points_size);
|
|
} else {
|
|
if (num_hypothesis_tested >= growth_function[subset_size-1] && subset_size < termination_length-MAX_THREADS) {
|
|
subset_size++;
|
|
if (subset_size >= points_size) subset_size = points_size-1;
|
|
}
|
|
if (growth_function[subset_size-1] < num_hypothesis_tested) {
|
|
// The sample contains m-1 points selected from U_(n-1) at random and u_n
|
|
random_gen->generateUniqueRandomSet(sample, sample_size-1, subset_size-1);
|
|
sample[sample_size-1] = subset_size-1;
|
|
} else
|
|
// Select m points from U_n at random.
|
|
random_gen->generateUniqueRandomSet(sample, sample_size, subset_size);
|
|
}
|
|
} else sampler->generateSample(sample); // use local sampler
|
|
|
|
const int number_of_models = estimator->estimateModels(sample, models);
|
|
if (adapt) {
|
|
num_estimations++; mean_num_est_models += number_of_models;
|
|
}
|
|
for (int i = 0; i < number_of_models; i++) {
|
|
num_tested_models++;
|
|
if (adapt) {
|
|
current_score = quality->getScore(models[i]);
|
|
supports.emplace_back(current_score.inlier_number);
|
|
} else if (! model_verifier->isModelGood(models[i], current_score))
|
|
continue;
|
|
|
|
if (current_score.isBetter(best_score_all_threads)) {
|
|
if (degeneracy->recoverIfDegenerate(sample, models[i], current_score,
|
|
non_degenerate_model, non_denegenerate_model_score)) {
|
|
// check if best non degenerate model is better than so far the best model
|
|
if (non_denegenerate_model_score.isBetter(best_score_thread))
|
|
update_best(non_denegenerate_model_score, non_degenerate_model);
|
|
else continue;
|
|
} else update_best(current_score, models[i]);
|
|
if (LO && num_hypothesis_tested < max_iters && IoU < IOU_SIMILARITY_THR &&
|
|
best_score_thread.inlier_number > min_non_random_inliers)
|
|
runLO(iters);
|
|
} // end of if so far the best score
|
|
else if ((int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND) {
|
|
tested_models_thread.emplace_back(models[i].clone());
|
|
tested_samples_thread.emplace_back(sample);
|
|
}
|
|
if (num_hypothesis_tested > max_iters) {
|
|
success = true; break;
|
|
}
|
|
} // end loop of number of models
|
|
if (adapt && iters >= MAX_ITERS_ADAPT && num_tested_models >= MAX_MODELS_ADAPT) {
|
|
adapt = false;
|
|
lambda_non_random_all_inliers_thread = getLambda(supports, 2.32, points_size, sample_size, false, min_non_random_inliers);
|
|
model_verifier->updateSPRT(params->getTimeForModelEstimation(), 1, (double)mean_num_est_models/num_estimations, lambda_non_random_all_inliers_thread/points_size,
|
|
(double)std::max(min_non_random_inliers, best_score.inlier_number)/points_size, best_score_all_threads);
|
|
}
|
|
if (!adapt && LO && num_hypothesis_tested < max_iters && !was_LO_run && !best_model_thread.empty() &&
|
|
best_score_thread.inlier_number > min_non_random_inliers)
|
|
runLO(iters);
|
|
} // end of loop over iters
|
|
if (! was_LO_run && !best_model_thread.empty() && LO)
|
|
runLO(-1 /*use full iterations of LO*/);
|
|
best_model_thread.copyTo(best_models[thread_rng_id]);
|
|
best_scores[thread_rng_id] = best_score_thread;
|
|
num_tested_models_threads[thread_rng_id] = num_tested_models;
|
|
tested_models_threads[thread_rng_id] = tested_models_thread;
|
|
tested_samples_threads[thread_rng_id] = tested_samples_thread;
|
|
best_samples_threads[thread_rng_id] = best_sample_thread;
|
|
if (IS_FUNDAMENTAL) {
|
|
best_scores_not_LO[thread_rng_id] = best_not_LO_score_thread;
|
|
best_not_LO_thread.copyTo(best_models_not_LO[thread_rng_id]);
|
|
last_model_from_LO_vec[thread_rng_id] = is_last_from_LO_thread;
|
|
}
|
|
lambda_non_random_all_inliers_vec[thread_rng_id] = lambda_non_random_all_inliers_thread;
|
|
}}); // end parallel
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// find best model from all threads' models
|
|
best_score = best_scores[0];
|
|
int best_thread_idx = 0;
|
|
for (int i = 1; i < MAX_THREADS; i++)
|
|
if (best_scores[i].isBetter(best_score)) {
|
|
best_score = best_scores[i];
|
|
best_thread_idx = i;
|
|
}
|
|
best_model = best_models[best_thread_idx];
|
|
if (IS_FUNDAMENTAL) {
|
|
last_model_from_LO = last_model_from_LO_vec[best_thread_idx];
|
|
K1_approx = K1_apx[best_thread_idx];
|
|
K2_approx = K2_apx[best_thread_idx];
|
|
}
|
|
final_iters = num_hypothesis_tested;
|
|
best_sample = best_samples_threads[best_thread_idx];
|
|
int num_lambdas = 0;
|
|
double avg_lambda = 0;
|
|
for (int i = 0; i < MAX_THREADS; i++) {
|
|
if (IS_FUNDAMENTAL && best_scores_not_LO[i].isBetter(best_score_model_not_from_LO)) {
|
|
best_score_model_not_from_LO = best_scores_not_LO[i];
|
|
best_models_not_LO[i].copyTo(best_model_not_from_LO);
|
|
}
|
|
if (IS_NON_RAND_TEST && lambda_non_random_all_inliers_vec[i] > 0) {
|
|
num_lambdas ++;
|
|
avg_lambda += lambda_non_random_all_inliers_vec[i];
|
|
}
|
|
num_total_tested_models += num_tested_models_threads[i];
|
|
if ((int)models_for_random_test.size() < MAX_TEST_MODELS_NONRAND) {
|
|
for (int m = 0; m < (int)tested_models_threads[i].size(); m++) {
|
|
models_for_random_test.emplace_back(tested_models_threads[i][m].clone());
|
|
samples_for_random_test.emplace_back(tested_samples_threads[i][m]);
|
|
if ((int)models_for_random_test.size() == MAX_TEST_MODELS_NONRAND)
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
if (IS_NON_RAND_TEST && num_lambdas > 0 && avg_lambda > 0)
|
|
lambda_non_random_all_inliers = avg_lambda / num_lambdas;
|
|
}
|
|
if (best_model.empty()) {
|
|
ransac_output = RansacOutput::create(best_model, std::vector<bool>(), best_score.inlier_number, final_iters, ModelConfidence::RANDOM, std::vector<float>());
|
|
return false;
|
|
}
|
|
if (last_model_from_LO && IS_FUNDAMENTAL && K1.empty() && K2.empty()) {
|
|
Score new_score; Mat new_model;
|
|
const double INL_THR = 0.80;
|
|
if (parallel)
|
|
_quality->getInliers(best_model, best_inliers_mask);
|
|
// run additional degeneracy check for F:
|
|
if (_degeneracy.dynamicCast<FundamentalDegeneracy>()->verifyFundamental(best_model, best_score, best_inliers_mask, new_model, new_score)) {
|
|
// so-far-the-best F is degenerate
|
|
// Update best F using non-degenerate F or the one which is not from LO
|
|
if (new_score.isBetter(best_score_model_not_from_LO) && new_score.inlier_number > INL_THR*best_score.inlier_number) {
|
|
best_score = new_score;
|
|
new_model.copyTo(best_model);
|
|
} else if (best_score_model_not_from_LO.inlier_number > INL_THR*best_score.inlier_number) {
|
|
best_score = best_score_model_not_from_LO;
|
|
best_model_not_from_LO.copyTo(best_model);
|
|
}
|
|
} else { // so-far-the-best F is not degenerate
|
|
if (new_score.isBetter(best_score)) {
|
|
// if new model is better then update
|
|
best_score = new_score;
|
|
new_model.copyTo(best_model);
|
|
}
|
|
}
|
|
}
|
|
if (params->getFinalPolisher() != PolishingMethod::NONE_POLISHER) {
|
|
Mat polished_model;
|
|
Score polisher_score;
|
|
if (polisher->polishSoFarTheBestModel(best_model, best_score, // polish final model
|
|
polished_model, polisher_score) && polisher_score.isBetter(best_score)) {
|
|
best_score = polisher_score;
|
|
polished_model.copyTo(best_model);
|
|
}
|
|
}
|
|
|
|
///////////////// get inliers of the best model and points' residuals ///////////////
|
|
std::vector<bool> inliers_mask; std::vector<float> residuals;
|
|
if (params->isMaskRequired()) {
|
|
inliers_mask = std::vector<bool>(points_size);
|
|
residuals = _error->getErrors(best_model);
|
|
_quality->getInliers(residuals, inliers_mask, threshold);
|
|
}
|
|
|
|
ModelConfidence model_conf = ModelConfidence::UNKNOWN;
|
|
if (IS_NON_RAND_TEST) {
|
|
std::vector<int> temp_inliers(points_size);
|
|
const int non_random_inls_best_model = getIndependentInliers(best_model, best_sample, temp_inliers,
|
|
_quality->getInliers(best_model, temp_inliers));
|
|
// quick test on lambda from all inliers (= upper bound of independent inliers)
|
|
// if model with independent inliers is not random for Poisson with all inliers then it is not random using independent inliers too
|
|
if (pow(Utils::getPoissonCDF(lambda_non_random_all_inliers, non_random_inls_best_model), num_total_tested_models) < 0.9999) {
|
|
std::vector<int> inliers_list(models_for_random_test.size());
|
|
for (int m = 0; m < (int)models_for_random_test.size(); m++)
|
|
inliers_list[m] = getIndependentInliers(models_for_random_test[m], samples_for_random_test[m],
|
|
temp_inliers, _quality->getInliers(models_for_random_test[m], temp_inliers));
|
|
int min_non_rand_inliers;
|
|
const double lambda = getLambda(inliers_list, 1.644, points_size, sample_size, true, min_non_rand_inliers);
|
|
const double cdf_lambda = Utils::getPoissonCDF(lambda, non_random_inls_best_model), cdf_N = pow(cdf_lambda, num_total_tested_models);
|
|
model_conf = cdf_N < 0.9999 ? ModelConfidence ::RANDOM : ModelConfidence ::NON_RANDOM;
|
|
} else model_conf = ModelConfidence ::NON_RANDOM;
|
|
}
|
|
ransac_output = RansacOutput::create(best_model, inliers_mask, best_score.inlier_number, final_iters, model_conf, residuals);
|
|
return true;
|
|
}
|
|
};
|
|
|
|
/*
|
|
* pts1, pts2 are matrices either N x a, N x b or a x N or b x N, where N > a and N > b
|
|
* pts1 are image points, if pnp pts2 are object points otherwise - image points as well.
|
|
* output is matrix of size N x (a + b)
|
|
* return points_size = N
|
|
*/
|
|
int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp) {
|
|
Mat pts1 = pts1_.getMat(), pts2 = pts2_.getMat();
|
|
auto convertPoints = [] (Mat &points, int pt_dim) {
|
|
points.convertTo(points, CV_32F); // convert points to have float precision
|
|
if (points.channels() > 1)
|
|
points = points.reshape(1, (int)points.total()); // convert point to have 1 channel
|
|
if (points.rows < points.cols)
|
|
transpose(points, points); // transpose so points will be in rows
|
|
CV_CheckGE(points.cols, pt_dim, "Invalid dimension of point");
|
|
if (points.cols != pt_dim) // in case when image points are 3D convert them to 2D
|
|
points = points.colRange(0, pt_dim);
|
|
};
|
|
|
|
convertPoints(pts1, 2); // pts1 are always image points
|
|
convertPoints(pts2, ispnp ? 3 : 2); // for PnP points are 3D
|
|
|
|
// points are of size [Nx2 Nx2] = Nx4 for H, F, E
|
|
// points are of size [Nx2 Nx3] = Nx5 for PnP
|
|
hconcat(pts1, pts2, pts);
|
|
return pts.rows;
|
|
}
|
|
|
|
void saveMask (OutputArray mask, const std::vector<bool> &inliers_mask) {
|
|
if (mask.needed()) {
|
|
const int points_size = (int) inliers_mask.size();
|
|
Mat tmp_mask(points_size, 1, CV_8U);
|
|
auto * maskptr = tmp_mask.ptr<uchar>();
|
|
for (int i = 0; i < points_size; i++)
|
|
maskptr[i] = (uchar) inliers_mask[i];
|
|
tmp_mask.copyTo(mask);
|
|
}
|
|
}
|
|
void setParameters (Ptr<Model> ¶ms, EstimationMethod estimator, const UsacParams &usac_params,
|
|
bool mask_needed) {
|
|
params = Model::create(usac_params.threshold, estimator, usac_params.sampler,
|
|
usac_params.confidence, usac_params.maxIterations, usac_params.score);
|
|
params->setLocalOptimization(usac_params.loMethod);
|
|
params->setLOSampleSize(usac_params.loSampleSize);
|
|
params->setLOIterations(usac_params.loIterations);
|
|
params->setParallel(usac_params.isParallel);
|
|
params->setNeighborsType(usac_params.neighborsSearch);
|
|
params->setRandomGeneratorState(usac_params.randomGeneratorState);
|
|
params->maskRequired(mask_needed);
|
|
}
|
|
|
|
void setParameters (int flag, Ptr<Model> ¶ms, EstimationMethod estimator, double thr,
|
|
int max_iters, double conf, bool mask_needed) {
|
|
switch (flag) {
|
|
case USAC_DEFAULT:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
|
|
break;
|
|
case USAC_MAGSAC:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MAGSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_SIGMA);
|
|
params->setLOSampleSize(params->isHomography() ? 75 : 50);
|
|
params->setLOIterations(params->isHomography() ? 15 : 10);
|
|
break;
|
|
case USAC_PARALLEL:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setParallel(true);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
|
|
break;
|
|
case USAC_ACCURATE:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_GC);
|
|
params->setLOSampleSize(20);
|
|
params->setLOIterations(25);
|
|
break;
|
|
case USAC_FAST:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
|
|
params->setLOIterations(5);
|
|
params->setLOIterativeIters(3);
|
|
break;
|
|
case USAC_PROSAC:
|
|
params = Model::create(thr, estimator, SamplingMethod::SAMPLING_PROSAC, conf, max_iters,
|
|
ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
|
|
break;
|
|
case USAC_FM_8PTS:
|
|
params = Model::create(thr, EstimationMethod::FUNDAMENTAL8,SamplingMethod::SAMPLING_UNIFORM,
|
|
conf, max_iters,ScoreMethod::SCORE_METHOD_MSAC);
|
|
params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
|
|
break;
|
|
default: CV_Error(cv::Error::StsBadFlag, "Incorrect flag for USAC!");
|
|
}
|
|
// do not do too many iterations for PnP
|
|
if (estimator == EstimationMethod::P3P) {
|
|
if (params->getLOInnerMaxIters() > 10)
|
|
params->setLOIterations(10);
|
|
params->setLOIterativeIters(0);
|
|
params->setFinalLSQ(3);
|
|
}
|
|
|
|
params->maskRequired(mask_needed);
|
|
}
|
|
|
|
Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, double thr,
|
|
OutputArray mask, const int max_iters, const double confidence) {
|
|
Ptr<Model> params;
|
|
setParameters(method, params, EstimationMethod::HOMOGRAPHY, thr, max_iters, confidence, mask.needed());
|
|
Ptr<RansacOutput> ransac_output;
|
|
if (run(params, srcPoints, dstPoints,
|
|
ransac_output, noArray(), noArray(), noArray(), noArray())) {
|
|
saveMask(mask, ransac_output->getInliersMask());
|
|
return ransac_output->getModel() / ransac_output->getModel().at<double>(2,2);
|
|
}
|
|
if (mask.needed()){
|
|
mask.create(std::max(srcPoints.getMat().rows, srcPoints.getMat().cols), 1, CV_8U);
|
|
mask.setTo(Scalar::all(0));
|
|
}
|
|
return Mat();
|
|
}
|
|
|
|
Mat findFundamentalMat( InputArray points1, InputArray points2, int method, double thr,
|
|
double confidence, int max_iters, OutputArray mask ) {
|
|
Ptr<Model> params;
|
|
setParameters(method, params, EstimationMethod::FUNDAMENTAL, thr, max_iters, confidence, mask.needed());
|
|
Ptr<RansacOutput> ransac_output;
|
|
if (run(params, points1, points2,
|
|
ransac_output, noArray(), noArray(), noArray(), noArray())) {
|
|
saveMask(mask, ransac_output->getInliersMask());
|
|
return ransac_output->getModel();
|
|
}
|
|
if (mask.needed()){
|
|
mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
|
|
mask.setTo(Scalar::all(0));
|
|
}
|
|
return Mat();
|
|
}
|
|
|
|
Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1,
|
|
int method, double prob, double thr, OutputArray mask, int maxIters) {
|
|
Ptr<Model> params;
|
|
setParameters(method, params, EstimationMethod::ESSENTIAL, thr, maxIters, prob, mask.needed());
|
|
Ptr<RansacOutput> ransac_output;
|
|
if (run(params, points1, points2,
|
|
ransac_output, cameraMatrix1, cameraMatrix1, noArray(), noArray())) {
|
|
saveMask(mask, ransac_output->getInliersMask());
|
|
return ransac_output->getModel();
|
|
}
|
|
if (mask.needed()){
|
|
mask.create(std::max(points1.getMat().rows, points1.getMat().cols), 1, CV_8U);
|
|
mask.setTo(Scalar::all(0));
|
|
}
|
|
return Mat();
|
|
}
|
|
|
|
bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
|
|
InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,
|
|
bool /*useExtrinsicGuess*/, int max_iters, float thr, double conf,
|
|
OutputArray inliers, int method) {
|
|
Ptr<Model> params;
|
|
setParameters(method, params, cameraMatrix.empty() ? EstimationMethod ::P6P : EstimationMethod ::P3P,
|
|
thr, max_iters, conf, inliers.needed());
|
|
Ptr<RansacOutput> ransac_output;
|
|
if (run(params, imagePoints, objectPoints,
|
|
ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
|
|
if (inliers.needed()) {
|
|
const auto &inliers_mask = ransac_output->getInliersMask();
|
|
Mat inliers_;
|
|
for (int i = 0; i < (int)inliers_mask.size(); i++)
|
|
if (inliers_mask[i])
|
|
inliers_.push_back(i);
|
|
inliers_.copyTo(inliers);
|
|
}
|
|
const Mat &model = ransac_output->getModel();
|
|
model.col(0).copyTo(rvec);
|
|
model.col(1).copyTo(tvec);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int method,
|
|
double thr, int max_iters, double conf, int /*refineIters*/) {
|
|
Ptr<Model> params;
|
|
setParameters(method, params, EstimationMethod::AFFINE, thr, max_iters, conf, mask.needed());
|
|
Ptr<RansacOutput> ransac_output;
|
|
if (run(params, from, to,
|
|
ransac_output, noArray(), noArray(), noArray(), noArray())) {
|
|
saveMask(mask, ransac_output->getInliersMask());
|
|
return ransac_output->getModel().rowRange(0,2);
|
|
}
|
|
if (mask.needed()){
|
|
mask.create(std::max(from.getMat().rows, from.getMat().cols), 1, CV_8U);
|
|
mask.setTo(Scalar::all(0));
|
|
}
|
|
return Mat();
|
|
}
|
|
|
|
class ModelImpl : public Model {
|
|
private:
|
|
// main parameters:
|
|
double threshold;
|
|
EstimationMethod estimator;
|
|
SamplingMethod sampler;
|
|
double confidence;
|
|
int max_iterations;
|
|
ScoreMethod score;
|
|
int sample_size;
|
|
|
|
// Larsson parameters
|
|
bool is_larsson_optimization = true;
|
|
int larsson_leven_marq_iters_lo = 10, larsson_leven_marq_iters_fo = 15;
|
|
|
|
// solver for a null-space extraction
|
|
MethodSolver null_solver = GEM_SOLVER;
|
|
|
|
// prosac
|
|
int prosac_max_samples = 200000;
|
|
|
|
// for neighborhood graph
|
|
int k_nearest_neighbors = 8;//, flann_search_params = 5, num_kd_trees = 1; // for FLANN
|
|
int cell_size = 50; // pixels, for grid neighbors searching
|
|
int radius = 30; // pixels, for radius-search neighborhood graph
|
|
NeighborSearchMethod neighborsType = NeighborSearchMethod::NEIGH_GRID;
|
|
|
|
// Local Optimization parameters
|
|
LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_LO;
|
|
int lo_sample_size=12, lo_inner_iterations=20, lo_iterative_iterations=8,
|
|
lo_thr_multiplier=10, lo_iter_sample_size = 30;
|
|
|
|
// Graph cut parameters
|
|
const double spatial_coherence_term = 0.975;
|
|
|
|
// apply polisher for final RANSAC model
|
|
PolishingMethod polisher = PolishingMethod ::COV_POLISHER;
|
|
|
|
// preemptive verification test
|
|
VerificationMethod verifier = VerificationMethod ::ASPRT;
|
|
|
|
// sprt parameters
|
|
// lower bound estimate is 2% of inliers
|
|
// model estimation to verification time = ratio of time needed to estimate model
|
|
// to verification of one point wrt the model
|
|
double sprt_eps = 0.02, sprt_delta = 0.008, avg_num_models, model_est_to_ver_time;
|
|
|
|
// estimator error
|
|
ErrorMetric est_error;
|
|
|
|
// progressive napsac
|
|
double relax_coef = 0.1;
|
|
// for building neighborhood graphs
|
|
const std::vector<int> grid_cell_number = {10, 5, 2};
|
|
|
|
//for final least squares polisher
|
|
int final_lsq_iters = 7;
|
|
|
|
bool need_mask = true, // do we need inlier mask in the end
|
|
is_parallel = false, // use parallel RANSAC
|
|
is_nonrand_test = false; // is test for the final model non-randomness
|
|
|
|
// state of pseudo-random number generator
|
|
int random_generator_state = 0;
|
|
|
|
// number of iterations of plane-and-parallax in DEGENSAC^+
|
|
int plane_and_parallax_max_iters = 300;
|
|
|
|
// magsac parameters:
|
|
int DoF = 2;
|
|
double sigma_quantile = 3.04, upper_incomplete_of_sigma_quantile = 0.00419,
|
|
lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5;
|
|
double k_mlesac = 2.25; // parameter for MLESAC model evaluation
|
|
public:
|
|
ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_,
|
|
int max_iterations_, ScoreMethod score_) :
|
|
threshold(threshold_), estimator(estimator_), sampler(sampler_), confidence(confidence_), max_iterations(max_iterations_), score(score_) {
|
|
switch (estimator_) {
|
|
case (EstimationMethod::AFFINE):
|
|
avg_num_models = 1; model_est_to_ver_time = 50;
|
|
sample_size = 3; est_error = ErrorMetric ::FORW_REPR_ERR; break;
|
|
case (EstimationMethod::HOMOGRAPHY):
|
|
avg_num_models = 0.8; model_est_to_ver_time = 200;
|
|
sample_size = 4; est_error = ErrorMetric ::FORW_REPR_ERR; break;
|
|
case (EstimationMethod::FUNDAMENTAL):
|
|
DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012;
|
|
maximum_thr = 2.5;
|
|
avg_num_models = 1.5; model_est_to_ver_time = 200;
|
|
sample_size = 7; est_error = ErrorMetric ::SAMPSON_ERR; break;
|
|
case (EstimationMethod::FUNDAMENTAL8):
|
|
avg_num_models = 1; model_est_to_ver_time = 100; maximum_thr = 2.5;
|
|
sample_size = 8; est_error = ErrorMetric ::SAMPSON_ERR; break;
|
|
case (EstimationMethod::ESSENTIAL):
|
|
DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012;
|
|
avg_num_models = 3.93; model_est_to_ver_time = 1000; maximum_thr = 2;
|
|
sample_size = 5; est_error = ErrorMetric ::SAMPSON_ERR; break;
|
|
case (EstimationMethod::P3P):
|
|
avg_num_models = 1.38; model_est_to_ver_time = 800;
|
|
sample_size = 3; est_error = ErrorMetric ::RERPOJ; break;
|
|
case (EstimationMethod::P6P):
|
|
avg_num_models = 1; model_est_to_ver_time = 300;
|
|
sample_size = 6; est_error = ErrorMetric ::RERPOJ; break;
|
|
default: CV_Error(cv::Error::StsNotImplemented, "Estimator has not implemented yet!");
|
|
}
|
|
|
|
if (score_ == ScoreMethod::SCORE_METHOD_MAGSAC)
|
|
polisher = PolishingMethod::MAGSAC;
|
|
|
|
// for PnP problem we can use only KNN graph
|
|
if (estimator_ == EstimationMethod::P3P || estimator_ == EstimationMethod::P6P) {
|
|
polisher = LSQ_POLISHER;
|
|
neighborsType = NeighborSearchMethod::NEIGH_FLANN_KNN;
|
|
k_nearest_neighbors = 2;
|
|
}
|
|
}
|
|
|
|
// setters
|
|
void setNonRandomnessTest (bool set) override { is_nonrand_test = set; }
|
|
void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; }
|
|
void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; }
|
|
void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; }
|
|
void setError (ErrorMetric error_) override { est_error = error_; }
|
|
void setLocalOptimization (LocalOptimMethod lo_) override { lo = lo_; }
|
|
void setKNearestNeighhbors (int knn_) override { k_nearest_neighbors = knn_; }
|
|
void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; }
|
|
void setCellSize (int cell_size_) override { cell_size = cell_size_; }
|
|
void setLOIterations (int iters) override { lo_inner_iterations = iters; }
|
|
void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; }
|
|
void maskRequired (bool need_mask_) override { need_mask = need_mask_; }
|
|
void setRandomGeneratorState (int state) override { random_generator_state = state; }
|
|
void setLOIterativeIters (int iters) override { lo_iterative_iterations = iters; }
|
|
void setFinalLSQ (int iters) override { final_lsq_iters = iters; }
|
|
|
|
// getters
|
|
int getProsacMaxSamples() const override { return prosac_max_samples; }
|
|
int getLevMarqIters () const override { return larsson_leven_marq_iters_fo; }
|
|
int getLevMarqItersLO () const override { return larsson_leven_marq_iters_lo; }
|
|
bool isNonRandomnessTest () const override { return is_nonrand_test; }
|
|
bool isMaskRequired () const override { return need_mask; }
|
|
NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; }
|
|
int getKNN () const override { return k_nearest_neighbors; }
|
|
ErrorMetric getError () const override { return est_error; }
|
|
EstimationMethod getEstimator () const override { return estimator; }
|
|
int getSampleSize () const override { return sample_size; }
|
|
int getFinalLSQIterations () const override { return final_lsq_iters; }
|
|
int getDegreesOfFreedom () const override { return DoF; }
|
|
double getSigmaQuantile () const override { return sigma_quantile; }
|
|
double getUpperIncompleteOfSigmaQuantile () const override {
|
|
return upper_incomplete_of_sigma_quantile;
|
|
}
|
|
double getLowerIncompleteOfSigmaQuantile () const override {
|
|
return lower_incomplete_of_sigma_quantile;
|
|
}
|
|
double getC () const override { return C; }
|
|
double getKmlesac () const override { return k_mlesac; }
|
|
double getMaximumThreshold () const override { return maximum_thr; }
|
|
double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; }
|
|
int getLOSampleSize () const override { return lo_sample_size; }
|
|
MethodSolver getRansacSolver () const override { return null_solver; }
|
|
PolishingMethod getFinalPolisher () const override { return polisher; }
|
|
int getLOThresholdMultiplier() const override { return lo_thr_multiplier; }
|
|
int getLOIterativeSampleSize() const override { return lo_iter_sample_size; }
|
|
int getLOIterativeMaxIters() const override { return lo_iterative_iterations; }
|
|
int getLOInnerMaxIters() const override { return lo_inner_iterations; }
|
|
int getPlaneAndParallaxIters () const override { return plane_and_parallax_max_iters; }
|
|
LocalOptimMethod getLO () const override { return lo; }
|
|
ScoreMethod getScore () const override { return score; }
|
|
int getMaxIters () const override { return max_iterations; }
|
|
double getConfidence () const override { return confidence; }
|
|
double getThreshold () const override { return threshold; }
|
|
VerificationMethod getVerifier () const override { return verifier; }
|
|
SamplingMethod getSampler () const override { return sampler; }
|
|
int getRandomGeneratorState () const override { return random_generator_state; }
|
|
double getSPRTdelta () const override { return sprt_delta; }
|
|
double getSPRTepsilon () const override { return sprt_eps; }
|
|
double getSPRTavgNumModels () const override { return avg_num_models; }
|
|
int getCellSize () const override { return cell_size; }
|
|
int getGraphRadius() const override { return radius; }
|
|
double getTimeForModelEstimation () const override { return model_est_to_ver_time; }
|
|
double getRelaxCoef () const override { return relax_coef; }
|
|
const std::vector<int> &getGridCellNumber () const override { return grid_cell_number; }
|
|
bool isLarssonOptimization () const override { return is_larsson_optimization; }
|
|
bool isParallel () const override { return is_parallel; }
|
|
bool isFundamental () const override {
|
|
return estimator == EstimationMethod::FUNDAMENTAL ||
|
|
estimator == EstimationMethod::FUNDAMENTAL8;
|
|
}
|
|
bool isHomography () const override { return estimator == EstimationMethod::HOMOGRAPHY; }
|
|
bool isEssential () const override { return estimator == EstimationMethod::ESSENTIAL; }
|
|
bool isPnP() const override {
|
|
return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P;
|
|
}
|
|
};
|
|
|
|
Ptr<Model> Model::create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
|
|
double confidence_, int max_iterations_, ScoreMethod score_) {
|
|
return makePtr<ModelImpl>(threshold_, estimator_, sampler_, confidence_,
|
|
max_iterations_, score_);
|
|
}
|
|
|
|
bool run (const Ptr<const Model> ¶ms, InputArray points1, InputArray points2,
|
|
Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
|
|
InputArray dist_coeff1, InputArray dist_coeff2) {
|
|
Ransac ransac (params, points1, points2, K1_, K2_, dist_coeff1, dist_coeff2);
|
|
if (ransac.run(ransac_output)) {
|
|
if (params->isPnP()) {
|
|
// convert R to rodrigues and back and recalculate inliers which due to numerical
|
|
// issues can differ
|
|
Mat out, newP;
|
|
Matx33d R, newR, K1;
|
|
Vec3d t, rvec;
|
|
if (K1_.empty()) {
|
|
usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t);
|
|
Rodrigues(R, rvec);
|
|
hconcat(rvec, t, out);
|
|
hconcat(out, K1, out);
|
|
} else {
|
|
K1 = ransac.K1;
|
|
const Mat Rt = Mat(Matx33d(K1).inv() * Matx34d(ransac_output->getModel()));
|
|
t = Rt.col(3);
|
|
Rodrigues(Rt.colRange(0,3), rvec);
|
|
hconcat(rvec, t, out);
|
|
}
|
|
// Matx33d _K1(K1);
|
|
Rodrigues(rvec, newR);
|
|
hconcat(K1 * Matx33d(newR), K1 * Vec3d(t), newP);
|
|
std::vector<bool> inliers_mask(ransac.points_size);
|
|
ransac._quality->getInliers(newP, inliers_mask);
|
|
ransac_output = RansacOutput::create(out, inliers_mask, ransac_output->getNumberOfInliers(),
|
|
ransac_output->getNumberOfIters(), ransac_output->getConfidence(), ransac_output->getResiduals());
|
|
}
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
}}
|