1	// This file is part of OpenCV project.
2	// It is subject to the license terms in the LICENSE file found in the top-level directory
3	// of this distribution and at http://opencv.org/license.html.
4
5	#include "../precomp.hpp"
6	#include "../usac.hpp"
7	#include <atomic>
8
9	namespace cv {
10	UsacParams::UsacParams() {
11	confidence=0.99;
12	isParallel=false;
13	loIterations=5;
14	loMethod=LOCAL_OPTIM_INNER_LO;
15	loSampleSize=14;
16	maxIterations=5000;
17	neighborsSearch=NEIGH_GRID;
18	randomGeneratorState=0;
19	sampler=SAMPLING_UNIFORM;
20	score=SCORE_METHOD_MSAC;
21	threshold=1.5;
22	final_polisher=COV_POLISHER;
23	final_polisher_iterations=3;
24	}
25
26	namespace usac {
27	int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp);
28	void setParameters (int flag, Ptr<Model> &params, EstimationMethod estimator, double thr,
29	int max_iters, double conf, bool mask_needed);
30
31	class RansacOutputImpl : public RansacOutput {
32	private:
33	std::vector<int> inliers;
34	cv::Mat model, K1, K2;
35	// vector of number_inliers size
36	// vector of points size, true if inlier, false - outlier
37	std::vector<bool> inliers_mask;
38	// vector of points size, value of i-th index corresponds to error of i-th point if i is inlier.
39	std::vector<float> residuals;
40	int number_inliers, number_iterations;
41	ModelConfidence conf;
42	public:
43	RansacOutputImpl (const cv::Mat &model_, const std::vector<bool> &inliers_mask_, int number_inliers_,
44	int number_iterations_, ModelConfidence conf_, const std::vector<float> &errors_) {
45	model_.copyTo(m: model);
46	inliers_mask = inliers_mask_;
47	number_inliers = number_inliers_;
48	number_iterations = number_iterations_;
49	residuals = errors_;
50	conf = conf_;
51	}
52
53	// Return inliers' indices of size = number of inliers
54	const std::vector<int> &getInliers() override {
55	if (inliers.empty()) {
56	inliers.reserve(n: number_inliers);
57	int pt_cnt = 0;
58	for (bool is_inlier : inliers_mask) {
59	if (is_inlier)
60	inliers.emplace_back(args&: pt_cnt);
61	pt_cnt++;
62	}
63	}
64	return inliers;
65	}
66	const std::vector<bool> &getInliersMask() const override {
67	return inliers_mask;
68	}
69	int getNumberOfInliers() const override {
70	return number_inliers;
71	}
72	const Mat &getModel() const override {
73	return model;
74	}
75	int getNumberOfIters() const override {
76	return number_iterations;
77	}
78	ModelConfidence getConfidence() const override {
79	return conf;
80	}
81	const std::vector<float> &getResiduals() const override {
82	return residuals;
83	}
84	};
85
86	Ptr<RansacOutput> RansacOutput::create(const cv::Mat &model_, const std::vector<bool> &inliers_mask_, int number_inliers_,
87	int number_iterations_, ModelConfidence conf, const std::vector<float> &errors_) {
88	return makePtr<RansacOutputImpl>(a1: model_, a1: inliers_mask_, a1: number_inliers_,
89	a1: number_iterations_, a1: conf, a1: errors_);
90	}
91
92	double getLambda (std::vector<int> &supports, double cdf_thr, int points_size,
93	int sample_size, bool is_independent, int &min_non_random_inliers) {
94	std::sort(first: supports.begin(), last: supports.end());
95	double lambda = supports.size() % 2 ? (supports[supports.size()/2] + supports[supports.size()/2+1])*0.5 : supports[supports.size()/2];
96	const double cdf = lambda + cdf_thr*sqrt(x: lambda * (1 - lambda / (is_independent ? points_size - sample_size : points_size)));
97	int lower_than_cdf = 0; lambda = 0;
98	for (const auto &inl : supports)
99	if (inl < cdf) {
100	lambda += inl; lower_than_cdf++;
101	} else break; // list is sorted
102	lambda /= lower_than_cdf;
103	if (lambda < 1 || lower_than_cdf == 0) lambda = 1;
104	// use 0.9999 quantile https://keisan.casio.com/exec/system/14060745333941
105	if (! is_independent) // do not calculate it for all inliers
106	min_non_random_inliers = (int)(lambda + 2.32*sqrt(x: lambda * (1 - lambda / points_size))) + 1;
107	return lambda;
108	}
109
110	class Ransac {
111	public:
112	const Ptr<const Model> params;
113	Ptr<Estimator> _estimator;
114	Ptr<Error> _error;
115	Ptr<Quality> _quality;
116	Ptr<Sampler> _sampler;
117	Ptr<Termination> _termination;
118	Ptr<ModelVerifier> _model_verifier;
119	Ptr<Degeneracy> _degeneracy;
120	Ptr<LocalOptimization> _local_optimization;
121	Ptr<FinalModelPolisher> polisher;
122	Ptr<GammaValues> _gamma_generator;
123	Ptr<MinimalSolver> _min_solver;
124	Ptr<NonMinimalSolver> _lo_solver, _fo_solver;
125	Ptr<RandomGenerator> _lo_sampler;
126	Ptr<WeightFunction> _weight_fnc;
127
128	int points_size, _state, filtered_points_size;
129	double threshold, max_thr;
130	bool parallel;
131
132	Matx33d T1, T2;
133	Mat points, K1, K2, calib_points, image_points, norm_points, filtered_points;
134	Ptr<NeighborhoodGraph> graph;
135	std::vector<Ptr<NeighborhoodGraph>> layers;
136
137	Ransac (const Ptr<const Model> &params_, cv::InputArray points1, cv::InputArray points2,
138	cv::InputArray K1_, cv::InputArray K2_, cv::InputArray dist_coeff1, cv::InputArray dist_coeff2) : params(params_) {
139	_state = params->getRandomGeneratorState();
140	threshold = params->getThreshold();
141	max_thr = std::max(a: threshold, b: params->getMaximumThreshold());
142	parallel = params->isParallel();
143	Mat undist_points1, undist_points2;
144	if (params->isPnP()) {
145	if (! K1_.empty()) {
146	K1 = K1_.getMat().clone(); K1.convertTo(m: K1, CV_64F);
147	if (! dist_coeff1.empty()) {
148	// undistortPoints also calibrate points using K
149	undistortPoints(src: points1.isContinuous() ? points1 : points1.getMat().clone(), dst: undist_points1, cameraMatrix: K1_, distCoeffs: dist_coeff1);
150	points_size = mergePoints(pts1_: undist_points1, pts2_: points2, pts&: points, ispnp: true);
151	Utils::normalizeAndDecalibPointsPnP (K: K1, pts&: points, calib_norm_pts&: calib_points);
152	} else {
153	points_size = mergePoints(pts1_: points1, pts2_: points2, pts&: points, ispnp: true);
154	Utils::calibrateAndNormalizePointsPnP(K: K1, pts: points, calib_norm_pts&: calib_points);
155	}
156	} else points_size = mergePoints(pts1_: points1, pts2_: points2, pts&: points, ispnp: true);
157	} else {
158	if (params->isEssential()) {
159	CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!");
160	K1 = K1_.getMat(); K1.convertTo(m: K1, CV_64F);
161	K2 = K2_.getMat(); K2.convertTo(m: K2, CV_64F);
162	if (! dist_coeff1.empty() || ! dist_coeff2.empty()) {
163	// undistortPoints also calibrate points using K
164	if (! dist_coeff1.empty()) undistortPoints(src: points1.isContinuous() ? points1 : points1.getMat().clone(), dst: undist_points1, cameraMatrix: K1_, distCoeffs: dist_coeff1);
165	else undist_points1 = points1.getMat();
166	if (! dist_coeff2.empty()) undistortPoints(src: points2.isContinuous() ? points2 : points2.getMat().clone(), dst: undist_points2, cameraMatrix: K2_, distCoeffs: dist_coeff2);
167	else undist_points2 = points2.getMat();
168	points_size = mergePoints(pts1_: undist_points1, pts2_: undist_points2, pts&: calib_points, ispnp: false);
169	} else {
170	points_size = mergePoints(pts1_: points1, pts2_: points2, pts&: points, ispnp: false);
171	Utils::calibratePoints(K1, K2, points, norm_points&: calib_points);
172	}
173	threshold = Utils::getCalibratedThreshold(threshold, K1, K2);
174	max_thr = Utils::getCalibratedThreshold(threshold: max_thr, K1, K2);
175	} else {
176	points_size = mergePoints(pts1_: points1, pts2_: points2, pts&: points, ispnp: false);
177	if (params->isFundamental() && ! K1_.empty() && ! K2_.empty()) {
178	K1 = K1_.getMat(); K1.convertTo(m: K1, CV_64F);
179	K2 = K2_.getMat(); K2.convertTo(m: K2, CV_64F);
180	Utils::calibratePoints(K1, K2, points, norm_points&: calib_points);
181	}
182	}
183	}
184
185	if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) {
186	if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) {
187	graph = GridNeighborhoodGraph::create(points, points_size,
188	cell_size_x_img1_: params->getCellSize(), cell_size_y_img1_: params->getCellSize(), cell_size_x_img2_: params->getCellSize(), cell_size_y_img2_: params->getCellSize(), max_neighbors: 10);
189	} else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) {
190	graph = FlannNeighborhoodGraph::create(points, points_size,k_nearest_neighbors_: params->getKNN(), get_distances: false, flann_search_params: 5, num_kd_trees: 1);
191	} else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) {
192	graph = RadiusSearchNeighborhoodGraph::create(points, points_size, radius_: params->getGraphRadius(), flann_search_params: 5, num_kd_trees: 1);
193	} else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!");
194	}
195
196	if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
197	CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!");
198	const auto &cell_number_per_layer = params->getGridCellNumber();
199	layers.reserve(n: cell_number_per_layer.size());
200	const auto * const pts = (float *) points.data;
201	float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0;
202	for (int i = 0; i < 4 * points_size; i += 4) {
203	if (pts[i ] > img1_width ) img1_width = pts[i ];
204	if (pts[i + 1] > img1_height) img1_height = pts[i + 1];
205	if (pts[i + 2] > img2_width ) img2_width = pts[i + 2];
206	if (pts[i + 3] > img2_height) img2_height = pts[i + 3];
207	}
208	// Create grid graphs (overlapping layes of given cell numbers)
209	for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) {
210	const int cell_number = cell_number_per_layer[layer_idx];
211	if (layer_idx > 0)
212	if (cell_number_per_layer[layer_idx-1] <= cell_number)
213	CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: "
214	"Cell number in layers must be in decreasing order!");
215	layers.emplace_back(args: GridNeighborhoodGraph::create(points, points_size,
216	cell_size_x_img1_: (int)(img1_width / (float)cell_number), cell_size_y_img1_: (int)(img1_height / (float)cell_number),
217	cell_size_x_img2_: (int)(img2_width / (float)cell_number), cell_size_y_img2_: (int)(img2_height / (float)cell_number), max_neighbors: 10));
218	}
219	}
220
221	// update points by calibrated for Essential matrix after graph is calculated
222	if (params->isEssential()) {
223	image_points = points;
224	points = calib_points;
225	// if maximum calibrated threshold significanlty differs threshold then set upper bound
226	if (max_thr > 10*threshold)
227	max_thr = 10*threshold;
228	}
229
230	// Since error function output squared error distance, so make
231	// threshold squared as well
232	threshold *= threshold;
233
234	if ((params->isHomography() || (params->isFundamental() && (K1.empty() || K2.empty() || !params->isLarssonOptimization())) ||
235	params->getEstimator() == EstimationMethod::AFFINE) && (params->getLO() != LOCAL_OPTIM_NULL || params->getFinalPolisher() == COV_POLISHER)) {
236	const auto normTr = NormTransform::create(points);
237	std::vector<int> sample (points_size);
238	for (int i = 0; i < points_size; i++) sample[i] = i;
239	normTr->getNormTransformation(norm_points, sample, sample_number: points_size, T1, T2);
240	}
241
242	if (params->getScore() == SCORE_METHOD_MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA || params->getFinalPolisher() == MAGSAC)
243	_gamma_generator = GammaValues::create(DoF: params->getDegreesOfFreedom()); // is thread safe
244	initialize (state: _state, min_solver&: _min_solver, non_min_solver&: _lo_solver, error&: _error, estimator&: _estimator, degeneracy&: _degeneracy, quality&: _quality,
245	verifier&: _model_verifier, lo&: _local_optimization, termination&: _termination, sampler&: _sampler, lo_sampler&: _lo_sampler, weight_fnc&: _weight_fnc, parallel_call: false/*parallel*/);
246	if (params->getFinalPolisher() != NONE_POLISHER) {
247	polisher = NonMinimalPolisher::create(quality_: _quality, solver_: _fo_solver,
248	weight_fnc_: params->getFinalPolisher() == MAGSAC ? _weight_fnc : nullptr, max_iters_: params->getFinalLSQIterations(), iou_thr_: 0.99);
249	}
250	}
251
252	void initialize (int state, Ptr<MinimalSolver> &min_solver, Ptr<NonMinimalSolver> &non_min_solver,
253	Ptr<Error> &error, Ptr<Estimator> &estimator, Ptr<Degeneracy> &degeneracy, Ptr<Quality> &quality,
254	Ptr<ModelVerifier> &verifier, Ptr<LocalOptimization> &lo, Ptr<Termination> &termination,
255	Ptr<Sampler> &sampler, Ptr<RandomGenerator> &lo_sampler, Ptr<WeightFunction> &weight_fnc, bool parallel_call) {
256
257	const int min_sample_size = params->getSampleSize(), prosac_termination_length = std::min(a: (int)(.5*points_size), b: 100);
258	// inner inlier threshold will be used in LO to obtain inliers
259	// additionally in DEGENSAC for F
260	double inner_inlier_thr_sqr = threshold;
261	if (params->isHomography() && inner_inlier_thr_sqr < 5.25) inner_inlier_thr_sqr = 5.25; // at least 2.5 px
262	else if (params->isFundamental() && inner_inlier_thr_sqr < 4) inner_inlier_thr_sqr = 4; // at least 2 px
263
264	if (params->getFinalPolisher() == MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA)
265	weight_fnc = MagsacWeightFunction::create(gamma_generator_: _gamma_generator, DoF_: params->getDegreesOfFreedom(), upper_incomplete_of_sigma_quantile: params->getUpperIncompleteOfSigmaQuantile(), C_: params->getC(), max_sigma_: params->getMaximumThreshold());
266	else weight_fnc = nullptr;
267
268	switch (params->getError()) {
269	case ErrorMetric::SYMM_REPR_ERR:
270	error = ReprojectionErrorSymmetric::create(points); break;
271	case ErrorMetric::FORW_REPR_ERR:
272	if (params->getEstimator() == EstimationMethod::AFFINE)
273	error = ReprojectionErrorAffine::create(points);
274	else error = ReprojectionErrorForward::create(points);
275	break;
276	case ErrorMetric::SAMPSON_ERR:
277	error = SampsonError::create(points); break;
278	case ErrorMetric::SGD_ERR:
279	error = SymmetricGeometricDistance::create(points); break;
280	case ErrorMetric::RERPOJ:
281	error = ReprojectionErrorPmatrix::create(points); break;
282	default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!");
283	}
284
285	const double k_mlesac = params->getKmlesac ();
286	switch (params->getScore()) {
287	case ScoreMethod::SCORE_METHOD_RANSAC :
288	quality = RansacQuality::create(points_size_: points_size, threshold_: threshold, error_: error); break;
289	case ScoreMethod::SCORE_METHOD_MSAC :
290	quality = MsacQuality::create(points_size_: points_size, threshold_: threshold, error_: error, k_msac: k_mlesac); break;
291	case ScoreMethod::SCORE_METHOD_MAGSAC :
292	quality = MagsacQuality::create(maximum_thr: max_thr, points_size_: points_size, error_: error, gamma_generator: _gamma_generator,
293	tentative_inlier_threshold_: threshold, DoF: params->getDegreesOfFreedom(), sigma_quantile: params->getSigmaQuantile(),
294	upper_incomplete_of_sigma_quantile: params->getUpperIncompleteOfSigmaQuantile()); break;
295	case ScoreMethod::SCORE_METHOD_LMEDS :
296	quality = LMedsQuality::create(points_size_: points_size, threshold_: threshold, error_: error); break;
297	default: CV_Error(cv::Error::StsNotImplemented, "Score is not implemented!");
298	}
299
300	const auto is_ge_solver = params->getRansacSolver() == GEM_SOLVER;
301	if (params->isHomography()) {
302	degeneracy = HomographyDegeneracy::create(points_: points);
303	min_solver = HomographyMinimalSolver4pts::create(points, use_ge: is_ge_solver);
304	non_min_solver = HomographyNonMinimalSolver::create(points_: norm_points, T1, T2, use_ge: true);
305	estimator = HomographyEstimator::create(min_solver_: min_solver, non_min_solver_: non_min_solver, degeneracy_: degeneracy);
306	if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
307	if (params->getFinalPolisher() == COV_POLISHER)
308	_fo_solver = CovarianceHomographySolver::create(points: norm_points, T1, T2);
309	else _fo_solver = HomographyNonMinimalSolver::create(points_: points);
310	}
311	} else if (params->isFundamental()) {
312	if (K1.empty() || K2.empty()) {
313	degeneracy = FundamentalDegeneracy::create(state: state++, quality_: quality, points_: points, sample_size_: min_sample_size,
314	max_iters_plane_and_parallax: params->getPlaneAndParallaxIters(), homography_threshold: std::max(a: threshold, b: 8.) /*sqr homogr thr*/, f_inlier_thr_sqr: inner_inlier_thr_sqr, true_K1: K1, true_K2: K2);
315	} else degeneracy = FundamentalDegeneracyViaE::create(quality, pts: points, calib_pts: calib_points, K1, K2, is_f_objective: true/*is F*/);
316	if (min_sample_size == 7) {
317	min_solver = FundamentalMinimalSolver7pts::create(points, use_ge: is_ge_solver);
318	} else min_solver = FundamentalMinimalSolver8pts::create(points_: points);
319	if (params->isLarssonOptimization() && !K1.empty() && !K2.empty()) {
320	non_min_solver = LarssonOptimizer::create(calib_points_: calib_points, K1_: K1, K2_: K2, max_iters_: params->getLevMarqItersLO(), is_fundamental_: true/*F*/);
321	} else {
322	if (weight_fnc)
323	non_min_solver = EpipolarNonMinimalSolver::create(points_: points, is_fundamental: true);
324	else
325	non_min_solver = EpipolarNonMinimalSolver::create(points_: norm_points, T1, T2, use_ge: true);
326	}
327	estimator = FundamentalEstimator::create(min_solver_: min_solver, non_min_solver_: non_min_solver, degeneracy_: degeneracy);
328	if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
329	if (params->isLarssonOptimization() && !K1.empty() && !K2.empty())
330	_fo_solver = LarssonOptimizer::create(calib_points_: calib_points, K1_: K1, K2_: K2, max_iters_: params->getLevMarqIters(), is_fundamental_: true/*F*/);
331	else if (params->getFinalPolisher() == COV_POLISHER)
332	_fo_solver = CovarianceEpipolarSolver::create(points: norm_points, T1, T2);
333	else _fo_solver = EpipolarNonMinimalSolver::create(points_: points, is_fundamental: true);
334	}
335	} else if (params->isEssential()) {
336	if (params->getEstimator() == EstimationMethod::ESSENTIAL) {
337	min_solver = EssentialMinimalSolver5pts::create(points, use_svd: !is_ge_solver, is_nister: true/*Nister*/);
338	degeneracy = EssentialDegeneracy::create(points, sample_size: min_sample_size);
339	}
340	non_min_solver = LarssonOptimizer::create(calib_points_: calib_points, K1_: K1, K2_: K2, max_iters_: params->getLevMarqItersLO(), is_fundamental_: false/*E*/);
341	estimator = EssentialEstimator::create(min_solver_: min_solver, non_min_solver_: non_min_solver, degeneracy_: degeneracy);
342	if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER)
343	_fo_solver = LarssonOptimizer::create(calib_points_: calib_points, K1_: K1, K2_: K2, max_iters_: params->getLevMarqIters(), is_fundamental_: false/*E*/);
344	} else if (params->isPnP()) {
345	degeneracy = makePtr<Degeneracy>();
346	if (min_sample_size == 3) {
347	min_solver = P3PSolver::create(points_: points, calib_norm_pts: calib_points, K: K1);
348	non_min_solver = DLSPnP::create(points_: points, calib_norm_pts: calib_points, K: K1);
349	} else {
350	if (is_ge_solver)
351	min_solver = PnPMinimalSolver6Pts::create(points_: points);
352	else min_solver = PnPSVDSolver::create(points);
353	non_min_solver = PnPNonMinimalSolver::create(points);
354	}
355	estimator = PnPEstimator::create(min_solver_: min_solver, non_min_solver_: non_min_solver);
356	if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) _fo_solver = non_min_solver;
357	} else if (params->getEstimator() == EstimationMethod::AFFINE) {
358	degeneracy = makePtr<Degeneracy>();
359	min_solver = AffineMinimalSolver::create(points_: points);
360	non_min_solver = AffineNonMinimalSolver::create(points, T1: cv::noArray(), T2: cv::noArray());
361	estimator = AffineEstimator::create(min_solver_: min_solver, non_min_solver_: non_min_solver);
362	if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) {
363	if (params->getFinalPolisher() == COV_POLISHER)
364	_fo_solver = CovarianceAffineSolver::create(points);
365	else _fo_solver = non_min_solver;
366	}
367	} else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!");
368
369	switch (params->getSampler()) {
370	case SamplingMethod::SAMPLING_UNIFORM:
371	sampler = UniformSampler::create(state: state++, sample_size_: min_sample_size, points_size_: points_size);
372	break;
373	case SamplingMethod::SAMPLING_PROSAC:
374	if (!parallel_call) // for parallel only one PROSAC sampler
375	sampler = ProsacSampler::create(state: state++, points_size_: points_size, sample_size_: min_sample_size, growth_max_samples: params->getProsacMaxSamples());
376	break;
377	case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC:
378	sampler = ProgressiveNapsac::create(state: state++, points_size_: points_size, sample_size_: min_sample_size, layers, sampler_length: 20); break;
379	case SamplingMethod::SAMPLING_NAPSAC:
380	sampler = NapsacSampler::create(state: state++, points_size_: points_size, sample_size_: min_sample_size, neighborhood_graph_: graph); break;
381	default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!");
382	}
383
384	const bool is_sprt = params->getVerifier() == VerificationMethod::SPRT_VERIFIER || params->getVerifier() == VerificationMethod::ASPRT;
385	if (is_sprt)
386	verifier = AdaptiveSPRT::create(state: state++, quality, points_size_: points_size, inlier_threshold_: params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold,
387	prob_pt_of_good_model: params->getSPRTepsilon(), prob_pt_of_bad_model: params->getSPRTdelta(), time_sample: params->getTimeForModelEstimation(),
388	avg_num_models: params->getSPRTavgNumModels(), score_type_: params->getScore(), k_mlesac, is_adaptive: params->getVerifier() == VerificationMethod::ASPRT);
389	else if (params->getVerifier() == VerificationMethod::NULL_VERIFIER)
390	verifier = ModelVerifier::create(qualtiy: quality);
391	else CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!");
392
393	if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) {
394	if (parallel_call) {
395	termination = ProsacTerminationCriteria::create(sampler_: nullptr, error_: error,
396	points_size_: points_size, sample_size: min_sample_size, confidence: params->getConfidence(), max_iters: params->getMaxIters(), min_termination_length: prosac_termination_length, beta: 0.05, non_randomness_phi: 0.05, inlier_thresh: threshold,
397	non_rand_inliers: _termination.dynamicCast<ProsacTerminationCriteria>()->getNonRandomInliers());
398	} else {
399	termination = ProsacTerminationCriteria::create(sampler_: sampler.dynamicCast<ProsacSampler>(), error_: error,
400	points_size_: points_size, sample_size: min_sample_size, confidence: params->getConfidence(), max_iters: params->getMaxIters(), min_termination_length: prosac_termination_length, beta: 0.05, non_randomness_phi: 0.05, inlier_thresh: threshold,
401	non_rand_inliers: std::vector<int>());
402	}
403	} else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) {
404	if (is_sprt)
405	termination = SPRTPNapsacTermination::create(sprt: verifier.dynamicCast<AdaptiveSPRT>(),
406	confidence: params->getConfidence(), points_size_: points_size, sample_size_: min_sample_size,
407	max_iterations_: params->getMaxIters(), relax_coef_: params->getRelaxCoef());
408	else termination = StandardTerminationCriteria::create (confidence: params->getConfidence(),
409	points_size_: points_size, sample_size_: min_sample_size, max_iterations_: params->getMaxIters());
410	} else if (is_sprt && params->getLO() == LocalOptimMethod::LOCAL_OPTIM_NULL) {
411	termination = SPRTTermination::create(sprt: verifier.dynamicCast<AdaptiveSPRT>(),
412	confidence: params->getConfidence(), points_size_: points_size, sample_size_: min_sample_size, max_iterations_: params->getMaxIters());
413	} else {
414	termination = StandardTerminationCriteria::create
415	(confidence: params->getConfidence(), points_size_: points_size, sample_size_: min_sample_size, max_iterations_: params->getMaxIters());
416	}
417
418	// if normal ransac or parallel call, avoid redundant init
419	if ((! params->isParallel() || parallel_call) && params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) {
420	lo_sampler = UniformRandomGenerator::create(state, max_range: points_size, subset_size_: params->getLOSampleSize());
421	const auto lo_termination = StandardTerminationCriteria::create(confidence: params->getConfidence(), points_size_: points_size, sample_size_: min_sample_size, max_iterations_: params->getMaxIters());
422	switch (params->getLO()) {
423	case LocalOptimMethod::LOCAL_OPTIM_INNER_LO: case LocalOptimMethod::LOCAL_OPTIM_SIGMA:
424	lo = SimpleLocalOptimization::create(quality_: quality, estimator_: non_min_solver, termination_: lo_termination, random_gen: lo_sampler,
425	weight_fnc_: weight_fnc, max_lo_iters_: params->getLOInnerMaxIters(), inlier_thr_sqr: inner_inlier_thr_sqr, updated_lo: true); break;
426	case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO:
427	lo = InnerIterativeLocalOptimization::create(estimator_: estimator, quality_: quality, lo_sampler_: lo_sampler,
428	pts_size: points_size, threshold_: threshold, is_iterative_: true, lo_iter_sample_size_: params->getLOIterativeSampleSize(),
429	lo_inner_iterations: params->getLOInnerMaxIters(), lo_iter_max_iterations: params->getLOIterativeMaxIters(),
430	threshold_multiplier: params->getLOThresholdMultiplier()); break;
431	case LocalOptimMethod::LOCAL_OPTIM_GC:
432	lo = GraphCut::create(estimator_: estimator, quality_: quality, neighborhood_graph_: graph, lo_sampler_: lo_sampler, threshold_: threshold,
433	spatial_coherence_term: params->getGraphCutSpatialCoherenceTerm(), gc_iters: params->getLOInnerMaxIters(), termination_: lo_termination); break;
434	default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!");
435	}
436	}
437	}
438
439	int getIndependentInliers (const Mat &model_, const std::vector<int> &sample,
440	std::vector<int> &inliers, const int num_inliers_) {
441	bool is_F = params->isFundamental();
442	Mat model = model_;
443	int sample_size = 0;
444	if (is_F) sample_size = 7;
445	else if (params->isHomography()) sample_size = 4;
446	else if (params->isEssential()) {
447	is_F = true;
448	// convert E to F
449	model = Mat(Matx33d(K2).inv().t() * Matx33d(model) * Matx33d(K1).inv());
450	sample_size = 5;
451	} else if (params->isPnP() || params->getEstimator() == EstimationMethod::AFFINE) sample_size = 3;
452	else
453	CV_Error(cv::Error::StsNotImplemented, "Method for independent inliers is not implemented for this problem");
454	if (num_inliers_ <= sample_size) return 0; // minimal sample size generates model
455	model.convertTo(m: model, CV_32F);
456	int num_inliers = num_inliers_, num_pts_near_ep = 0,
457	num_pts_validatin_or_constr = 0, pt1 = 0;
458	const auto * const pts = params->isEssential() ? (float *) image_points.data : (float *) points.data;
459	// scale for thresholds should be used
460	const float ep_thr_sqr = 0.000001f, line_thr = 0.01f, neigh_thr = 4.0f;
461	float sign1=0,a1=0, b1=0, c1=0, a2=0, b2=0, c2=0, ep1_x, ep1_y, ep2_x, ep2_y;
462	const auto * const m = (float *) model.data;
463	Vec3f ep1;
464	bool do_or_test = false, ep1_inf = false, ep2_inf = false;
465	if (is_F) { // compute epipole and sign of the first point for orientation test
466	model *= (1/norm(src1: model));
467	ep1 = Utils::getRightEpipole(F: model);
468	const Vec3f ep2 = Utils::getLeftEpipole(F: model);
469	if (fabsf(x: ep1[2]) < DBL_EPSILON) {
470	ep1_inf = true;
471	} else {
472	ep1_x = ep1[0] / ep1[2];
473	ep1_y = ep1[1] / ep1[2];
474	}
475	if (fabsf(x: ep2[2]) < DBL_EPSILON) {
476	ep2_inf = true;
477	} else {
478	ep2_x = ep2[0] / ep2[2];
479	ep2_y = ep2[1] / ep2[2];
480	}
481	}
482	const auto * const e1 = ep1.val; // of size 3x1
483
484	// we move sample points to the end, so every inlier will be checked by sample point
485	int num_sample_in_inliers = 0;
486	if (!sample.empty()) {
487	num_sample_in_inliers = 0;
488	int temp_idx = num_inliers;
489	for (int i = 0; i < temp_idx; i++) {
490	const int inl = inliers[i];
491	for (int s : sample) {
492	if (inl == s) {
493	std::swap(a&: inliers[i], b&: inliers[--temp_idx]);
494	i--; // we need to check inlier that we just swapped
495	num_sample_in_inliers++;
496	break;
497	}
498	}
499	}
500	}
501
502	if (is_F) {
503	int MIN_TEST = std::min(a: 15, b: num_inliers);
504	for (int i = 0; i < MIN_TEST; i++) {
505	pt1 = 4*inliers[i];
506	sign1 = (m[0]*pts[pt1+2]+m[3]*pts[pt1+3]+m[6])*(e1[1]-e1[2]*pts[pt1+1]);
507	int validate = 0;
508	for (int j = 0; j < MIN_TEST; j++) {
509	if (i == j) continue;
510	const int inl_idx = 4*inliers[j];
511	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)
512	validate++;
513	}
514	if (validate < MIN_TEST/2) {
515	do_or_test = true; break;
516	}
517	}
518	}
519
520	// verification does not include sample points as they are surely random
521	const int max_verify = num_inliers - num_sample_in_inliers;
522	if (max_verify <= 0)
523	return 0;
524	int num_non_random_inliers = num_inliers - sample_size;
525	auto removeDependentPoints = [&] (bool do_orient_test, bool check_epipoles) {
526	for (int i = 0; i < max_verify; i++) {
527	// checks over inliers if they are dependent to other inliers
528	const int inl_idx = 4*inliers[i];
529	const auto x1 = pts[inl_idx], y1 = pts[inl_idx+1], x2 = pts[inl_idx+2], y2 = pts[inl_idx+3];
530	if (is_F) {
531	// epipolar line on image 2 = l2
532	a2 = m[0] * x1 + m[1] * y1 + m[2];
533	b2 = m[3] * x1 + m[4] * y1 + m[5];
534	c2 = m[6] * x1 + m[7] * y1 + m[8];
535	// epipolar line on image 1 = l1
536	a1 = m[0] * x2 + m[3] * y2 + m[6];
537	b1 = m[1] * x2 + m[4] * y2 + m[7];
538	c1 = m[2] * x2 + m[5] * y2 + m[8];
539	if ((!ep1_inf && fabsf(x: x1-ep1_x)+fabsf(x: y1-ep1_y) < neigh_thr) ||
540	(!ep2_inf && fabsf(x: x2-ep2_x)+fabsf(x: y2-ep2_y) < neigh_thr)) {
541	num_non_random_inliers--;
542	num_pts_near_ep++;
543	continue; // is dependent, continue to the next point
544	} else if (check_epipoles) {
545	if (a2 * a2 + b2 * b2 + c2 * c2 < ep_thr_sqr ||
546	a1 * a1 + b1 * b1 + c1 * c1 < ep_thr_sqr) {
547	num_non_random_inliers--;
548	num_pts_near_ep++;
549	continue; // is dependent, continue to the next point
550	}
551	}
552	else if (do_orient_test && pt1 != inl_idx && sign1*(m[0]*x2+m[3]*y2+m[6])*(e1[1]-e1[2]*y1)<0) {
553	num_non_random_inliers--;
554	num_pts_validatin_or_constr++;
555	continue;
556	}
557	const auto mag2 = 1 / sqrt(x: a2 * a2 + b2 * b2), mag1 = 1/sqrt(x: a1 * a1 + b1 * b1);
558	a2 *= mag2; b2 *= mag2; c2 *= mag2;
559	a1 *= mag1; b1 *= mag1; c1 *= mag1;
560	}
561
562	for (int j = i+1; j < num_inliers; j++) {// verify through all including sample points
563	const int inl_idx_j = 4*inliers[j];
564	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];
565	// use L1 norm instead of L2 for faster evaluation
566	if (fabsf(x: X1-x1) + fabsf(x: Y1 - y1) < neigh_thr || fabsf(x: X2-x2) + fabsf(x: Y2 - y2) < neigh_thr) {
567	num_non_random_inliers--;
568	// num_pts_bad_conditioning++;
569	break; // is dependent stop verification
570	} else if (is_F) {
571	if (fabsf(x: a2 * X2 + b2 * Y2 + c2) < line_thr && //|| // xj'^T F xi
572	fabsf(x: a1 * X1 + b1 * Y1 + c1) < line_thr) { // xj^T F^T xi'
573	num_non_random_inliers--;
574	break; // is dependent stop verification
575	}
576	}
577	}
578	}
579	};
580	if (params->isPnP()) {
581	for (int i = 0; i < max_verify; i++) {
582	const int inl_idx = 5*inliers[i];
583	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];
584	for (int j = i+1; j < num_inliers; j++) {
585	const int inl_idx_j = 5*inliers[j];
586	if (fabsf(x: x-pts[inl_idx_j ]) + fabsf(x: y-pts[inl_idx_j+1]) < neigh_thr ||
587	fabsf(x: X-pts[inl_idx_j+2]) + fabsf(x: Y-pts[inl_idx_j+3]) + fabsf(x: Z-pts[inl_idx_j+4]) < neigh_thr) {
588	num_non_random_inliers--;
589	break;
590	}
591	}
592	}
593	} else {
594	removeDependentPoints(do_or_test, !ep1_inf && !ep2_inf);
595	if (is_F) {
596	const bool is_pts_vald_constr_normal = (double)num_pts_validatin_or_constr / num_inliers < 0.6;
597	const bool is_pts_near_ep_normal = (double)num_pts_near_ep / num_inliers < 0.6;
598	if (!is_pts_near_ep_normal || !is_pts_vald_constr_normal) {
599	num_non_random_inliers = num_inliers-sample_size;
600	num_pts_near_ep = 0; num_pts_validatin_or_constr = 0;
601	removeDependentPoints(is_pts_vald_constr_normal, is_pts_near_ep_normal);
602	}
603	}
604	}
605	return num_non_random_inliers;
606	}
607
608	bool run(Ptr<RansacOutput> &ransac_output) {
609	if (points_size < params->getSampleSize())
610	return false;
611	const bool LO = params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL,
612	IS_FUNDAMENTAL = params->isFundamental(), IS_NON_RAND_TEST = params->isNonRandomnessTest();
613	const int MAX_MODELS_ADAPT = 21, MAX_ITERS_ADAPT = MAX_MODELS_ADAPT/*assume at least 1 model from 1 sample*/,
614	sample_size = params->getSampleSize();
615	const double IOU_SIMILARITY_THR = 0.80;
616	std::vector<int> non_degen_sample, best_sample;
617
618	double lambda_non_random_all_inliers = -1;
619	int final_iters, num_total_tested_models = 0;
620
621	// non-random
622	const int MAX_TEST_MODELS_NONRAND = IS_NON_RAND_TEST ? MAX_MODELS_ADAPT : 0;
623	std::vector<Mat> models_for_random_test; models_for_random_test.reserve(n: MAX_TEST_MODELS_NONRAND);
624	std::vector<std::vector<int>> samples_for_random_test; samples_for_random_test.reserve(n: MAX_TEST_MODELS_NONRAND);
625
626	bool last_model_from_LO = false;
627	Mat best_model, best_model_not_from_LO, K1_approx, K2_approx;
628	Score best_score, best_score_model_not_from_LO;
629	std::vector<bool> best_inliers_mask(points_size);
630	if (! parallel) {
631	// adaptive sprt test
632	double IoU = 0, mean_num_est_models = 0;
633	bool adapt = IS_NON_RAND_TEST || params->getVerifier() == VerificationMethod ::ASPRT, was_LO_run = false;
634	int min_non_random_inliers = 30, iters = 0, num_estimations = 0, max_iters = params->getMaxIters();
635	Mat non_degenerate_model, lo_model;
636	Score current_score, non_degenerate_model_score, best_score_sample;
637	std::vector<bool> model_inliers_mask (points_size);
638	std::vector<Mat> models(_estimator->getMaxNumSolutions());
639	std::vector<int> sample(_estimator->getMinimalSampleSize()), supports;
640	supports.reserve(n: 3*MAX_MODELS_ADAPT); // store model supports during adaption
641	auto update_best = [&] (const Mat &new_model, const Score &new_score, bool from_lo=false) {
642	_quality->getInliers(model: new_model, inliers_mask&: model_inliers_mask);
643	IoU = Utils::intersectionOverUnion(a: best_inliers_mask, b: model_inliers_mask);
644	best_inliers_mask = model_inliers_mask;
645
646	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
647	// save old best model for non-randomness test if necessary
648	models_for_random_test.emplace_back(args: best_model.clone());
649	samples_for_random_test.emplace_back(args&: best_sample);
650	}
651
652	// update score, model, inliers and max iterations
653	best_score = new_score;
654	new_model.copyTo(m: best_model);
655
656	if (!from_lo) {
657	best_sample = sample;
658	if (IS_FUNDAMENTAL) { // avoid degeneracy after LO run
659	// save last model not from LO
660	best_model.copyTo(m: best_model_not_from_LO);
661	best_score_model_not_from_LO = best_score;
662	}
663	}
664
665	_model_verifier->update(score: best_score, iteration: iters);
666	max_iters = _termination->update(model: best_model, inlier_number: best_score.inlier_number);
667	// max_iters = std::max(max_iters, std::min(10, params->getMaxIters()));
668	if (!adapt) // update quality and verifier to save evaluation time of a model
669	_quality->setBestScore(best_score.score);
670	last_model_from_LO = from_lo;
671	};
672
673	auto run_lo = [&] (const Mat &_model, const Score &_score, bool force_lo) {
674	was_LO_run = true;
675	_local_optimization->setCurrentRANSACiter(force_lo ? iters : -1);
676	Score lo_score;
677	if (_local_optimization->refineModel(best_model: _model, best_model_score: _score, new_model&: lo_model, new_model_score&: lo_score) && lo_score.isBetter(score2: best_score))
678	update_best(lo_model, lo_score, true);
679	};
680
681	for (; iters < max_iters; iters++) {
682	_sampler->generateSample(sample);
683	int number_of_models;
684	if (adapt) {
685	number_of_models = _estimator->estimateModels(sample, models);
686	mean_num_est_models += number_of_models;
687	num_estimations++;
688	} else {
689	number_of_models = _estimator->estimateModels(sample, models);
690	}
691	for (int i = 0; i < number_of_models; i++) {
692	num_total_tested_models++;
693	if (adapt) {
694	current_score = _quality->getScore(model: models[i]);
695	supports.emplace_back(args&: current_score.inlier_number);
696	if (IS_NON_RAND_TEST && best_score_sample.isBetter(score2: current_score)) {
697	models_for_random_test.emplace_back(args: models[i].clone());
698	samples_for_random_test.emplace_back(args&: sample);
699	}
700	} else {
701	if (! _model_verifier->isModelGood(model: models[i], score&: current_score))
702	continue;
703	}
704	if (current_score.isBetter(score2: best_score_sample)) {
705	if (_degeneracy->recoverIfDegenerate(sample, best_model: models[i], score: current_score,
706	non_degenerate_model, non_degenerate_model_score)) {
707	// check if best non degenerate model is better than so far the best model
708	if (non_degenerate_model_score.isBetter(score2: best_score)) {
709	update_best(non_degenerate_model, non_degenerate_model_score);
710	best_score_sample = current_score.isBetter(score2: best_score) ? best_score : current_score;
711	} else continue;
712	} else {
713	best_score_sample = current_score;
714	update_best(models[i], current_score);
715	}
716
717	if (LO && ((iters < max_iters && best_score.inlier_number > min_non_random_inliers && IoU < IOU_SIMILARITY_THR)))
718	run_lo(best_model, best_score, false);
719
720	} // end of if so far the best score
721	} // end loop of number of models
722	if (adapt && iters >= MAX_ITERS_ADAPT && num_total_tested_models >= MAX_MODELS_ADAPT) {
723	adapt = false;
724	lambda_non_random_all_inliers = getLambda(supports, cdf_thr: 2.32, points_size, sample_size, is_independent: false, min_non_random_inliers);
725	_model_verifier->updateSPRT(time_model_est: params->getTimeForModelEstimation(), time_corr_ver: 1.0, new_avg_models: mean_num_est_models/num_estimations, new_delta: lambda_non_random_all_inliers/points_size,new_epsilon: (double)std::max(a: min_non_random_inliers, b: best_score.inlier_number)/points_size, best_score);
726	}
727	} // end main while loop
728	final_iters = iters;
729	if (! was_LO_run && !best_model.empty() && LO)
730	run_lo(best_model, best_score, true);
731	} else { // parallel VSAC
732	const int MAX_THREADS = getNumThreads(), growth_max_samples = params->getProsacMaxSamples();
733	const bool is_prosac = params->getSampler() == SamplingMethod::SAMPLING_PROSAC;
734	std::atomic_bool success(false);
735	std::atomic_int num_hypothesis_tested(0), thread_cnt(0), max_number_inliers(0), subset_size, termination_length;
736	std::atomic<float> best_score_all(std::numeric_limits<float>::max());
737	std::vector<Score> best_scores(MAX_THREADS), best_scores_not_LO;
738	std::vector<Mat> best_models(MAX_THREADS), best_models_not_LO, K1_apx, K2_apx;
739	std::vector<int> num_tested_models_threads(MAX_THREADS), growth_function, non_random_inliers;
740	std::vector<std::vector<Mat>> tested_models_threads(MAX_THREADS);
741	std::vector<std::vector<std::vector<int>>> tested_samples_threads(MAX_THREADS);
742	std::vector<std::vector<int>> best_samples_threads(MAX_THREADS);
743	std::vector<bool> last_model_from_LO_vec;
744	std::vector<double> lambda_non_random_all_inliers_vec(MAX_THREADS);
745	if (IS_FUNDAMENTAL) {
746	last_model_from_LO_vec = std::vector<bool>(MAX_THREADS);
747	best_models_not_LO = std::vector<Mat>(MAX_THREADS);
748	best_scores_not_LO = std::vector<Score>(MAX_THREADS);
749	K1_apx = std::vector<Mat>(MAX_THREADS);
750	K2_apx = std::vector<Mat>(MAX_THREADS);
751	}
752	if (is_prosac) {
753	growth_function = _sampler.dynamicCast<ProsacSampler>()->getGrowthFunction();
754	subset_size = 2*sample_size; // n, size of the current sampling pool
755	termination_length = points_size;
756	}
757	///////////////////////////////////////////////////////////////////////////////////////////////////////
758	parallel_for_(range: Range(0, MAX_THREADS), functor: [&](const Range & /*range*/) {
759	if (!success) { // cover all if not success to avoid thread creating new variables
760	const int thread_rng_id = thread_cnt++;
761	bool adapt = params->getVerifier() == VerificationMethod ::ASPRT || IS_NON_RAND_TEST;
762	int thread_state = _state + thread_rng_id, min_non_random_inliers = 0, num_tested_models = 0,
763	num_estimations = 0, mean_num_est_models = 0, iters, max_iters = params->getMaxIters();
764	double IoU = 0, lambda_non_random_all_inliers_thread = -1;
765	std::vector<Mat> tested_models_thread; tested_models_thread.reserve(n: MAX_TEST_MODELS_NONRAND);
766	std::vector<std::vector<int>> tested_samples_thread; tested_samples_thread.reserve(n: MAX_TEST_MODELS_NONRAND);
767	Ptr<UniformRandomGenerator> random_gen;
768	if (is_prosac) random_gen = UniformRandomGenerator::create(state: thread_state);
769	Ptr<Error> error;
770	Ptr<Estimator> estimator;
771	Ptr<Degeneracy> degeneracy;
772	Ptr<Quality> quality;
773	Ptr<ModelVerifier> model_verifier;
774	Ptr<Sampler> sampler;
775	Ptr<RandomGenerator> lo_sampler;
776	Ptr<Termination> termination;
777	Ptr<LocalOptimization> local_optimization;
778	Ptr<MinimalSolver> min_solver;
779	Ptr<NonMinimalSolver> non_min_solver;
780	Ptr<WeightFunction> weight_fnc;
781	initialize (state: thread_state, min_solver, non_min_solver, error, estimator, degeneracy, quality,
782	verifier&: model_verifier, lo&: local_optimization, termination, sampler, lo_sampler, weight_fnc, parallel_call: true);
783	bool is_last_from_LO_thread = false;
784	Mat best_model_thread, non_degenerate_model, lo_model, best_not_LO_thread;
785	Score best_score_thread, current_score, non_denegenerate_model_score, lo_score, best_score_all_threads, best_not_LO_score_thread;
786	std::vector<int> sample(estimator->getMinimalSampleSize()), best_sample_thread, supports;
787	supports.reserve(n: 3*MAX_MODELS_ADAPT); // store model supports
788	std::vector<bool> best_inliers_mask_local(points_size, false), model_inliers_mask(points_size, false);
789	std::vector<Mat> models(estimator->getMaxNumSolutions());
790	auto update_best = [&] (const Score &new_score, const Mat &new_model, bool from_LO=false) {
791	// update best score of all threads
792	if (max_number_inliers < new_score.inlier_number) max_number_inliers = new_score.inlier_number;
793	if (best_score_all > new_score.score)
794	best_score_all = new_score.score;
795	best_score_all_threads = Score(max_number_inliers, best_score_all);
796	//
797	quality->getInliers(model: new_model, inliers_mask&: model_inliers_mask);
798	IoU = Utils::intersectionOverUnion(a: best_inliers_mask_local, b: model_inliers_mask);
799	if (!best_model_thread.empty() && (int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND && IoU < IOU_SIMILARITY_THR) {
800	tested_models_thread.emplace_back(args: best_model_thread.clone());
801	tested_samples_thread.emplace_back(args&: best_sample_thread);
802	}
803	if (!adapt) { // update quality and verifier
804	quality->setBestScore(best_score_all);
805	model_verifier->update(score: best_score_all_threads, iteration: iters);
806	}
807	// copy new score to best score
808	best_score_thread = new_score;
809	best_sample_thread = sample;
810	best_inliers_mask_local = model_inliers_mask;
811	// remember best model
812	new_model.copyTo(m: best_model_thread);
813
814	// update upper bound of iterations
815	if (is_prosac) {
816	int new_termination_length;
817	max_iters = termination.dynamicCast<ProsacTerminationCriteria>()->
818	updateTerminationLength(model: best_model_thread, inliers_size: best_score_thread.inlier_number, found_termination_length&: new_termination_length);
819	// update termination length
820	if (new_termination_length < termination_length)
821	termination_length = new_termination_length;
822	} else max_iters = termination->update(model: best_model_thread, inlier_number: max_number_inliers);
823	if (IS_FUNDAMENTAL) {
824	is_last_from_LO_thread = from_LO;
825	if (!from_LO) {
826	best_model_thread.copyTo(m: best_not_LO_thread);
827	best_not_LO_score_thread = best_score_thread;
828	}
829	}
830	};
831	bool was_LO_run = false;
832	auto runLO = [&] (int current_ransac_iters) {
833	was_LO_run = true;
834	local_optimization->setCurrentRANSACiter(current_ransac_iters);
835	if (local_optimization->refineModel(best_model: best_model_thread, best_model_score: best_score_thread, new_model&: lo_model,
836	new_model_score&: lo_score) && lo_score.isBetter(score2: best_score_thread))
837	update_best(lo_score, lo_model, true);
838	};
839	for (iters = 0; iters < max_iters && !success; iters++) {
840	success = num_hypothesis_tested++ > max_iters;
841	if (iters % 10 && !adapt) {
842	// Synchronize threads. just to speed verification of model.
843	quality->setBestScore(std::min(a: best_score_thread.score, b: (float)best_score_all));
844	model_verifier->update(score: best_score_thread.inlier_number > max_number_inliers ? best_score_thread : best_score_all_threads, iteration: iters);
845	}
846
847	if (is_prosac) {
848	if (num_hypothesis_tested > growth_max_samples) {
849	// if PROSAC has not converged to solution then do uniform sampling.
850	random_gen->generateUniqueRandomSet(sample, subset_size: sample_size, max_range: points_size);
851	} else {
852	if (num_hypothesis_tested >= growth_function[subset_size-1] && subset_size < termination_length-MAX_THREADS) {
853	subset_size++;
854	if (subset_size >= points_size) subset_size = points_size-1;
855	}
856	if (growth_function[subset_size-1] < num_hypothesis_tested) {
857	// The sample contains m-1 points selected from U_(n-1) at random and u_n
858	random_gen->generateUniqueRandomSet(sample, subset_size: sample_size-1, max_range: subset_size-1);
859	sample[sample_size-1] = subset_size-1;
860	} else
861	// Select m points from U_n at random.
862	random_gen->generateUniqueRandomSet(sample, subset_size: sample_size, max_range: subset_size);
863	}
864	} else sampler->generateSample(sample); // use local sampler
865
866	const int number_of_models = estimator->estimateModels(sample, models);
867	if (adapt) {
868	num_estimations++; mean_num_est_models += number_of_models;
869	}
870	for (int i = 0; i < number_of_models; i++) {
871	num_tested_models++;
872	if (adapt) {
873	current_score = quality->getScore(model: models[i]);
874	supports.emplace_back(args&: current_score.inlier_number);
875	} else if (! model_verifier->isModelGood(model: models[i], score&: current_score))
876	continue;
877
878	if (current_score.isBetter(score2: best_score_all_threads)) {
879	if (degeneracy->recoverIfDegenerate(sample, best_model: models[i], score: current_score,
880	non_degenerate_model, non_degenerate_model_score&: non_denegenerate_model_score)) {
881	// check if best non degenerate model is better than so far the best model
882	if (non_denegenerate_model_score.isBetter(score2: best_score_thread))
883	update_best(non_denegenerate_model_score, non_degenerate_model);
884	else continue;
885	} else update_best(current_score, models[i]);
886	if (LO && num_hypothesis_tested < max_iters && IoU < IOU_SIMILARITY_THR &&
887	best_score_thread.inlier_number > min_non_random_inliers)
888	runLO(iters);
889	} // end of if so far the best score
890	else if ((int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND) {
891	tested_models_thread.emplace_back(args: models[i].clone());
892	tested_samples_thread.emplace_back(args&: sample);
893	}
894	if (num_hypothesis_tested > max_iters) {
895	success = true; break;
896	}
897	} // end loop of number of models
898	if (adapt && iters >= MAX_ITERS_ADAPT && num_tested_models >= MAX_MODELS_ADAPT) {
899	adapt = false;
900	lambda_non_random_all_inliers_thread = getLambda(supports, cdf_thr: 2.32, points_size, sample_size, is_independent: false, min_non_random_inliers);
901	model_verifier->updateSPRT(time_model_est: params->getTimeForModelEstimation(), time_corr_ver: 1, new_avg_models: (double)mean_num_est_models/num_estimations, new_delta: lambda_non_random_all_inliers_thread/points_size,
902	new_epsilon: (double)std::max(a: min_non_random_inliers, b: best_score.inlier_number)/points_size, best_score: best_score_all_threads);
903	}
904	if (!adapt && LO && num_hypothesis_tested < max_iters && !was_LO_run && !best_model_thread.empty() &&
905	best_score_thread.inlier_number > min_non_random_inliers)
906	runLO(iters);
907	} // end of loop over iters
908	if (! was_LO_run && !best_model_thread.empty() && LO)
909	runLO(-1 /*use full iterations of LO*/);
910	best_model_thread.copyTo(m: best_models[thread_rng_id]);
911	best_scores[thread_rng_id] = best_score_thread;
912	num_tested_models_threads[thread_rng_id] = num_tested_models;
913	tested_models_threads[thread_rng_id] = tested_models_thread;
914	tested_samples_threads[thread_rng_id] = tested_samples_thread;
915	best_samples_threads[thread_rng_id] = best_sample_thread;
916	if (IS_FUNDAMENTAL) {
917	best_scores_not_LO[thread_rng_id] = best_not_LO_score_thread;
918	best_not_LO_thread.copyTo(m: best_models_not_LO[thread_rng_id]);
919	last_model_from_LO_vec[thread_rng_id] = is_last_from_LO_thread;
920	}
921	lambda_non_random_all_inliers_vec[thread_rng_id] = lambda_non_random_all_inliers_thread;
922	}}); // end parallel
923	///////////////////////////////////////////////////////////////////////////////////////////////////////
924	// find best model from all threads' models
925	best_score = best_scores[0];
926	int best_thread_idx = 0;
927	for (int i = 1; i < MAX_THREADS; i++)
928	if (best_scores[i].isBetter(score2: best_score)) {
929	best_score = best_scores[i];
930	best_thread_idx = i;
931	}
932	best_model = best_models[best_thread_idx];
933	if (IS_FUNDAMENTAL) {
934	last_model_from_LO = last_model_from_LO_vec[best_thread_idx];
935	K1_approx = K1_apx[best_thread_idx];
936	K2_approx = K2_apx[best_thread_idx];
937	}
938	final_iters = num_hypothesis_tested;
939	best_sample = best_samples_threads[best_thread_idx];
940	int num_lambdas = 0;
941	double avg_lambda = 0;
942	for (int i = 0; i < MAX_THREADS; i++) {
943	if (IS_FUNDAMENTAL && best_scores_not_LO[i].isBetter(score2: best_score_model_not_from_LO)) {
944	best_score_model_not_from_LO = best_scores_not_LO[i];
945	best_models_not_LO[i].copyTo(m: best_model_not_from_LO);
946	}
947	if (IS_NON_RAND_TEST && lambda_non_random_all_inliers_vec[i] > 0) {
948	num_lambdas ++;
949	avg_lambda += lambda_non_random_all_inliers_vec[i];
950	}
951	num_total_tested_models += num_tested_models_threads[i];
952	if ((int)models_for_random_test.size() < MAX_TEST_MODELS_NONRAND) {
953	for (int m = 0; m < (int)tested_models_threads[i].size(); m++) {
954	models_for_random_test.emplace_back(args: tested_models_threads[i][m].clone());
955	samples_for_random_test.emplace_back(args&: tested_samples_threads[i][m]);
956	if ((int)models_for_random_test.size() == MAX_TEST_MODELS_NONRAND)
957	break;
958	}
959	}
960	}
961	if (IS_NON_RAND_TEST && num_lambdas > 0 && avg_lambda > 0)
962	lambda_non_random_all_inliers = avg_lambda / num_lambdas;
963	}
964	if (best_model.empty()) {
965	ransac_output = RansacOutput::create(model_: best_model, inliers_mask_: std::vector<bool>(), number_inliers_: best_score.inlier_number, number_iterations_: final_iters, conf: ModelConfidence::RANDOM, errors_: std::vector<float>());
966	return false;
967	}
968	if (last_model_from_LO && IS_FUNDAMENTAL && K1.empty() && K2.empty()) {
969	Score new_score; Mat new_model;
970	const double INL_THR = 0.80;
971	if (parallel)
972	_quality->getInliers(model: best_model, inliers_mask&: best_inliers_mask);
973	// run additional degeneracy check for F:
974	if (_degeneracy.dynamicCast<FundamentalDegeneracy>()->verifyFundamental(F_best: best_model, F_score: best_score, inliers_mask: best_inliers_mask, F_new&: new_model, new_score)) {
975	// so-far-the-best F is degenerate
976	// Update best F using non-degenerate F or the one which is not from LO
977	if (new_score.isBetter(score2: best_score_model_not_from_LO) && new_score.inlier_number > INL_THR*best_score.inlier_number) {
978	best_score = new_score;
979	new_model.copyTo(m: best_model);
980	} else if (best_score_model_not_from_LO.inlier_number > INL_THR*best_score.inlier_number) {
981	best_score = best_score_model_not_from_LO;
982	best_model_not_from_LO.copyTo(m: best_model);
983	}
984	} else { // so-far-the-best F is not degenerate
985	if (new_score.isBetter(score2: best_score)) {
986	// if new model is better then update
987	best_score = new_score;
988	new_model.copyTo(m: best_model);
989	}
990	}
991	}
992	if (params->getFinalPolisher() != PolishingMethod::NONE_POLISHER) {
993	Mat polished_model;
994	Score polisher_score;
995	if (polisher->polishSoFarTheBestModel(model: best_model, best_model_score: best_score, // polish final model
996	new_model&: polished_model, new_model_score&: polisher_score) && polisher_score.isBetter(score2: best_score)) {
997	best_score = polisher_score;
998	polished_model.copyTo(m: best_model);
999	}
1000	}
1001
1002	///////////////// get inliers of the best model and points' residuals ///////////////
1003	std::vector<bool> inliers_mask; std::vector<float> residuals;
1004	if (params->isMaskRequired()) {
1005	inliers_mask = std::vector<bool>(points_size);
1006	residuals = _error->getErrors(model: best_model);
1007	_quality->getInliers(errors: residuals, inliers&: inliers_mask, threshold);
1008	}
1009
1010	ModelConfidence model_conf = ModelConfidence::UNKNOWN;
1011	if (IS_NON_RAND_TEST) {
1012	std::vector<int> temp_inliers(points_size);
1013	const int non_random_inls_best_model = getIndependentInliers(model_: best_model, sample: best_sample, inliers&: temp_inliers,
1014	num_inliers_: _quality->getInliers(model: best_model, inliers&: temp_inliers));
1015	// quick test on lambda from all inliers (= upper bound of independent inliers)
1016	// if model with independent inliers is not random for Poisson with all inliers then it is not random using independent inliers too
1017	if (pow(x: Utils::getPoissonCDF(lambda: lambda_non_random_all_inliers, tentative_inliers: non_random_inls_best_model), y: num_total_tested_models) < 0.9999) {
1018	std::vector<int> inliers_list(models_for_random_test.size());
1019	for (int m = 0; m < (int)models_for_random_test.size(); m++)
1020	inliers_list[m] = getIndependentInliers(model_: models_for_random_test[m], sample: samples_for_random_test[m],
1021	inliers&: temp_inliers, num_inliers_: _quality->getInliers(model: models_for_random_test[m], inliers&: temp_inliers));
1022	int min_non_rand_inliers;
1023	const double lambda = getLambda(supports&: inliers_list, cdf_thr: 1.644, points_size, sample_size, is_independent: true, min_non_random_inliers&: min_non_rand_inliers);
1024	const double cdf_lambda = Utils::getPoissonCDF(lambda, tentative_inliers: non_random_inls_best_model), cdf_N = pow(x: cdf_lambda, y: num_total_tested_models);
1025	model_conf = cdf_N < 0.9999 ? ModelConfidence ::RANDOM : ModelConfidence ::NON_RANDOM;
1026	} else model_conf = ModelConfidence ::NON_RANDOM;
1027	}
1028	ransac_output = RansacOutput::create(model_: best_model, inliers_mask_: inliers_mask, number_inliers_: best_score.inlier_number, number_iterations_: final_iters, conf: model_conf, errors_: residuals);
1029	return true;
1030	}
1031	};
1032
1033	/*
1034	* 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
1035	* pts1 are image points, if pnp pts2 are object points otherwise - image points as well.
1036	* output is matrix of size N x (a + b)
1037	* return points_size = N
1038	*/
1039	int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp) {
1040	Mat pts1 = pts1_.getMat(), pts2 = pts2_.getMat();
1041	auto convertPoints = [] (Mat &points, int pt_dim) {
1042	points.convertTo(m: points, CV_32F); // convert points to have float precision
1043	if (points.channels() > 1)
1044	points = points.reshape(cn: 1, rows: (int)points.total()); // convert point to have 1 channel
1045	if (points.rows < points.cols)
1046	transpose(src: points, dst: points); // transpose so points will be in rows
1047	CV_CheckGE(points.cols, pt_dim, "Invalid dimension of point");
1048	if (points.cols != pt_dim) // in case when image points are 3D convert them to 2D
1049	points = points.colRange(startcol: 0, endcol: pt_dim);
1050	};
1051
1052	convertPoints(pts1, 2); // pts1 are always image points
1053	convertPoints(pts2, ispnp ? 3 : 2); // for PnP points are 3D
1054
1055	// points are of size [Nx2 Nx2] = Nx4 for H, F, E
1056	// points are of size [Nx2 Nx3] = Nx5 for PnP
1057	hconcat(src1: pts1, src2: pts2, dst: pts);
1058	return pts.rows;
1059	}
1060
1061	void saveMask (OutputArray mask, const std::vector<bool> &inliers_mask) {
1062	if (mask.needed()) {
1063	const int points_size = (int) inliers_mask.size();
1064	Mat tmp_mask(points_size, 1, CV_8U);
1065	auto * maskptr = tmp_mask.ptr<uchar>();
1066	for (int i = 0; i < points_size; i++)
1067	maskptr[i] = (uchar) inliers_mask[i];
1068	tmp_mask.copyTo(m: mask);
1069	}
1070	}
1071	void setParameters (Ptr<Model> &params, EstimationMethod estimator, const UsacParams &usac_params,
1072	bool mask_needed) {
1073	params = Model::create(threshold_: usac_params.threshold, estimator_: estimator, sampler_: usac_params.sampler,
1074	confidence_: usac_params.confidence, max_iterations_: usac_params.maxIterations, score_: usac_params.score);
1075	params->setLocalOptimization(usac_params.loMethod);
1076	params->setLOSampleSize(usac_params.loSampleSize);
1077	params->setLOIterations(usac_params.loIterations);
1078	params->setParallel(usac_params.isParallel);
1079	params->setNeighborsType(usac_params.neighborsSearch);
1080	params->setRandomGeneratorState(usac_params.randomGeneratorState);
1081	params->maskRequired(required: mask_needed);
1082	}
1083
1084	void setParameters (int flag, Ptr<Model> &params, EstimationMethod estimator, double thr,
1085	int max_iters, double conf, bool mask_needed) {
1086	switch (flag) {
1087	case USAC_DEFAULT:
1088	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_UNIFORM, confidence_: conf, max_iterations_: max_iters,
1089	score_: ScoreMethod::SCORE_METHOD_MSAC);
1090	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
1091	break;
1092	case USAC_MAGSAC:
1093	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_UNIFORM, confidence_: conf, max_iterations_: max_iters,
1094	score_: ScoreMethod::SCORE_METHOD_MAGSAC);
1095	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_SIGMA);
1096	params->setLOSampleSize(params->isHomography() ? 75 : 50);
1097	params->setLOIterations(params->isHomography() ? 15 : 10);
1098	break;
1099	case USAC_PARALLEL:
1100	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_UNIFORM, confidence_: conf, max_iterations_: max_iters,
1101	score_: ScoreMethod::SCORE_METHOD_MSAC);
1102	params->setParallel(true);
1103	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
1104	break;
1105	case USAC_ACCURATE:
1106	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_UNIFORM, confidence_: conf, max_iterations_: max_iters,
1107	score_: ScoreMethod::SCORE_METHOD_MSAC);
1108	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_GC);
1109	params->setLOSampleSize(20);
1110	params->setLOIterations(25);
1111	break;
1112	case USAC_FAST:
1113	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_UNIFORM, confidence_: conf, max_iterations_: max_iters,
1114	score_: ScoreMethod::SCORE_METHOD_MSAC);
1115	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO);
1116	params->setLOIterations(5);
1117	params->setLOIterativeIters(3);
1118	break;
1119	case USAC_PROSAC:
1120	params = Model::create(threshold_: thr, estimator_: estimator, sampler_: SamplingMethod::SAMPLING_PROSAC, confidence_: conf, max_iterations_: max_iters,
1121	score_: ScoreMethod::SCORE_METHOD_MSAC);
1122	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
1123	break;
1124	case USAC_FM_8PTS:
1125	params = Model::create(threshold_: thr, estimator_: EstimationMethod::FUNDAMENTAL8,sampler_: SamplingMethod::SAMPLING_UNIFORM,
1126	confidence_: conf, max_iterations_: max_iters,score_: ScoreMethod::SCORE_METHOD_MSAC);
1127	params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO);
1128	break;
1129	default: CV_Error(cv::Error::StsBadFlag, "Incorrect flag for USAC!");
1130	}
1131	// do not do too many iterations for PnP
1132	if (estimator == EstimationMethod::P3P) {
1133	if (params->getLOInnerMaxIters() > 10)
1134	params->setLOIterations(10);
1135	params->setLOIterativeIters(0);
1136	params->setFinalLSQ(3);
1137	}
1138
1139	params->maskRequired(required: mask_needed);
1140	}
1141
1142	Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, double thr,
1143	OutputArray mask, const int max_iters, const double confidence) {
1144	Ptr<Model> params;
1145	setParameters(flag: method, params, estimator: EstimationMethod::HOMOGRAPHY, thr, max_iters, conf: confidence, mask_needed: mask.needed());
1146	Ptr<RansacOutput> ransac_output;
1147	if (run(params, points1: srcPoints, points2: dstPoints,
1148	ransac_output, K1_: noArray(), K2_: noArray(), dist_coeff1: noArray(), dist_coeff2: noArray())) {
1149	saveMask(mask, inliers_mask: ransac_output->getInliersMask());
1150	return ransac_output->getModel() / ransac_output->getModel().at<double>(i0: 2,i1: 2);
1151	}
1152	if (mask.needed()){
1153	mask.create(rows: std::max(a: srcPoints.getMat().rows, b: srcPoints.getMat().cols), cols: 1, CV_8U);
1154	mask.setTo(value: Scalar::all(v0: 0));
1155	}
1156	return Mat();
1157	}
1158
1159	Mat findFundamentalMat( InputArray points1, InputArray points2, int method, double thr,
1160	double confidence, int max_iters, OutputArray mask ) {
1161	Ptr<Model> params;
1162	setParameters(flag: method, params, estimator: EstimationMethod::FUNDAMENTAL, thr, max_iters, conf: confidence, mask_needed: mask.needed());
1163	Ptr<RansacOutput> ransac_output;
1164	if (run(params, points1, points2,
1165	ransac_output, K1_: noArray(), K2_: noArray(), dist_coeff1: noArray(), dist_coeff2: noArray())) {
1166	saveMask(mask, inliers_mask: ransac_output->getInliersMask());
1167	return ransac_output->getModel();
1168	}
1169	if (mask.needed()){
1170	mask.create(rows: std::max(a: points1.getMat().rows, b: points1.getMat().cols), cols: 1, CV_8U);
1171	mask.setTo(value: Scalar::all(v0: 0));
1172	}
1173	return Mat();
1174	}
1175
1176	Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1,
1177	int method, double prob, double thr, OutputArray mask, int maxIters) {
1178	Ptr<Model> params;
1179	setParameters(flag: method, params, estimator: EstimationMethod::ESSENTIAL, thr, max_iters: maxIters, conf: prob, mask_needed: mask.needed());
1180	Ptr<RansacOutput> ransac_output;
1181	if (run(params, points1, points2,
1182	ransac_output, K1_: cameraMatrix1, K2_: cameraMatrix1, dist_coeff1: noArray(), dist_coeff2: noArray())) {
1183	saveMask(mask, inliers_mask: ransac_output->getInliersMask());
1184	return ransac_output->getModel();
1185	}
1186	if (mask.needed()){
1187	mask.create(rows: std::max(a: points1.getMat().rows, b: points1.getMat().cols), cols: 1, CV_8U);
1188	mask.setTo(value: Scalar::all(v0: 0));
1189	}
1190	return Mat();
1191	}
1192
1193	bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
1194	InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec,
1195	bool /*useExtrinsicGuess*/, int max_iters, float thr, double conf,
1196	OutputArray inliers, int method) {
1197	Ptr<Model> params;
1198	setParameters(flag: method, params, estimator: cameraMatrix.empty() ? EstimationMethod ::P6P : EstimationMethod ::P3P,
1199	thr, max_iters, conf, mask_needed: inliers.needed());
1200	Ptr<RansacOutput> ransac_output;
1201	if (run(params, points1: imagePoints, points2: objectPoints,
1202	ransac_output, K1_: cameraMatrix, K2_: noArray(), dist_coeff1: distCoeffs, dist_coeff2: noArray())) {
1203	if (inliers.needed()) {
1204	const auto &inliers_mask = ransac_output->getInliersMask();
1205	Mat inliers_;
1206	for (int i = 0; i < (int)inliers_mask.size(); i++)
1207	if (inliers_mask[i])
1208	inliers_.push_back(elem: i);
1209	inliers_.copyTo(m: inliers);
1210	}
1211	const Mat &model = ransac_output->getModel();
1212	model.col(x: 0).copyTo(m: rvec);
1213	model.col(x: 1).copyTo(m: tvec);
1214	return true;
1215	}
1216	return false;
1217	}
1218
1219	Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int method,
1220	double thr, int max_iters, double conf, int /*refineIters*/) {
1221	Ptr<Model> params;
1222	setParameters(flag: method, params, estimator: EstimationMethod::AFFINE, thr, max_iters, conf, mask_needed: mask.needed());
1223	Ptr<RansacOutput> ransac_output;
1224	if (run(params, points1: from, points2: to,
1225	ransac_output, K1_: noArray(), K2_: noArray(), dist_coeff1: noArray(), dist_coeff2: noArray())) {
1226	saveMask(mask, inliers_mask: ransac_output->getInliersMask());
1227	return ransac_output->getModel().rowRange(startrow: 0,endrow: 2);
1228	}
1229	if (mask.needed()){
1230	mask.create(rows: std::max(a: from.getMat().rows, b: from.getMat().cols), cols: 1, CV_8U);
1231	mask.setTo(value: Scalar::all(v0: 0));
1232	}
1233	return Mat();
1234	}
1235
1236	class ModelImpl : public Model {
1237	private:
1238	// main parameters:
1239	double threshold;
1240	EstimationMethod estimator;
1241	SamplingMethod sampler;
1242	double confidence;
1243	int max_iterations;
1244	ScoreMethod score;
1245	int sample_size;
1246
1247	// Larsson parameters
1248	bool is_larsson_optimization = true;
1249	int larsson_leven_marq_iters_lo = 10, larsson_leven_marq_iters_fo = 15;
1250
1251	// solver for a null-space extraction
1252	MethodSolver null_solver = GEM_SOLVER;
1253
1254	// prosac
1255	int prosac_max_samples = 200000;
1256
1257	// for neighborhood graph
1258	int k_nearest_neighbors = 8;//, flann_search_params = 5, num_kd_trees = 1; // for FLANN
1259	int cell_size = 50; // pixels, for grid neighbors searching
1260	int radius = 30; // pixels, for radius-search neighborhood graph
1261	NeighborSearchMethod neighborsType = NeighborSearchMethod::NEIGH_GRID;
1262
1263	// Local Optimization parameters
1264	LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_LO;
1265	int lo_sample_size=12, lo_inner_iterations=20, lo_iterative_iterations=8,
1266	lo_thr_multiplier=10, lo_iter_sample_size = 30;
1267
1268	// Graph cut parameters
1269	const double spatial_coherence_term = 0.975;
1270
1271	// apply polisher for final RANSAC model
1272	PolishingMethod polisher = PolishingMethod ::COV_POLISHER;
1273
1274	// preemptive verification test
1275	VerificationMethod verifier = VerificationMethod ::ASPRT;
1276
1277	// sprt parameters
1278	// lower bound estimate is 2% of inliers
1279	// model estimation to verification time = ratio of time needed to estimate model
1280	// to verification of one point wrt the model
1281	double sprt_eps = 0.02, sprt_delta = 0.008, avg_num_models, model_est_to_ver_time;
1282
1283	// estimator error
1284	ErrorMetric est_error;
1285
1286	// progressive napsac
1287	double relax_coef = 0.1;
1288	// for building neighborhood graphs
1289	const std::vector<int> grid_cell_number = {10, 5, 2};
1290
1291	//for final least squares polisher
1292	int final_lsq_iters = 7;
1293
1294	bool need_mask = true, // do we need inlier mask in the end
1295	is_parallel = false, // use parallel RANSAC
1296	is_nonrand_test = false; // is test for the final model non-randomness
1297
1298	// state of pseudo-random number generator
1299	int random_generator_state = 0;
1300
1301	// number of iterations of plane-and-parallax in DEGENSAC^+
1302	int plane_and_parallax_max_iters = 300;
1303
1304	// magsac parameters:
1305	int DoF = 2;
1306	double sigma_quantile = 3.04, upper_incomplete_of_sigma_quantile = 0.00419,
1307	lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5;
1308	double k_mlesac = 2.25; // parameter for MLESAC model evaluation
1309	public:
1310	ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_,
1311	int max_iterations_, ScoreMethod score_) :
1312	threshold(threshold_), estimator(estimator_), sampler(sampler_), confidence(confidence_), max_iterations(max_iterations_), score(score_) {
1313	switch (estimator_) {
1314	case (EstimationMethod::AFFINE):
1315	avg_num_models = 1; model_est_to_ver_time = 50;
1316	sample_size = 3; est_error = ErrorMetric ::FORW_REPR_ERR; break;
1317	case (EstimationMethod::HOMOGRAPHY):
1318	avg_num_models = 0.8; model_est_to_ver_time = 200;
1319	sample_size = 4; est_error = ErrorMetric ::FORW_REPR_ERR; break;
1320	case (EstimationMethod::FUNDAMENTAL):
1321	DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012;
1322	maximum_thr = 2.5;
1323	avg_num_models = 1.5; model_est_to_ver_time = 200;
1324	sample_size = 7; est_error = ErrorMetric ::SAMPSON_ERR; break;
1325	case (EstimationMethod::FUNDAMENTAL8):
1326	avg_num_models = 1; model_est_to_ver_time = 100; maximum_thr = 2.5;
1327	sample_size = 8; est_error = ErrorMetric ::SAMPSON_ERR; break;
1328	case (EstimationMethod::ESSENTIAL):
1329	DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012;
1330	avg_num_models = 3.93; model_est_to_ver_time = 1000; maximum_thr = 2;
1331	sample_size = 5; est_error = ErrorMetric ::SAMPSON_ERR; break;
1332	case (EstimationMethod::P3P):
1333	avg_num_models = 1.38; model_est_to_ver_time = 800;
1334	sample_size = 3; est_error = ErrorMetric ::RERPOJ; break;
1335	case (EstimationMethod::P6P):
1336	avg_num_models = 1; model_est_to_ver_time = 300;
1337	sample_size = 6; est_error = ErrorMetric ::RERPOJ; break;
1338	default: CV_Error(cv::Error::StsNotImplemented, "Estimator has not implemented yet!");
1339	}
1340
1341	if (score_ == ScoreMethod::SCORE_METHOD_MAGSAC)
1342	polisher = PolishingMethod::MAGSAC;
1343
1344	// for PnP problem we can use only KNN graph
1345	if (estimator_ == EstimationMethod::P3P || estimator_ == EstimationMethod::P6P) {
1346	polisher = LSQ_POLISHER;
1347	neighborsType = NeighborSearchMethod::NEIGH_FLANN_KNN;
1348	k_nearest_neighbors = 2;
1349	}
1350	}
1351
1352	// setters
1353	void setNonRandomnessTest (bool set) override { is_nonrand_test = set; }
1354	void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; }
1355	void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; }
1356	void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; }
1357	void setError (ErrorMetric error_) override { est_error = error_; }
1358	void setLocalOptimization (LocalOptimMethod lo_) override { lo = lo_; }
1359	void setKNearestNeighhbors (int knn_) override { k_nearest_neighbors = knn_; }
1360	void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; }
1361	void setCellSize (int cell_size_) override { cell_size = cell_size_; }
1362	void setLOIterations (int iters) override { lo_inner_iterations = iters; }
1363	void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; }
1364	void maskRequired (bool need_mask_) override { need_mask = need_mask_; }
1365	void setRandomGeneratorState (int state) override { random_generator_state = state; }
1366	void setLOIterativeIters (int iters) override { lo_iterative_iterations = iters; }
1367	void setFinalLSQ (int iters) override { final_lsq_iters = iters; }
1368
1369	// getters
1370	int getProsacMaxSamples() const override { return prosac_max_samples; }
1371	int getLevMarqIters () const override { return larsson_leven_marq_iters_fo; }
1372	int getLevMarqItersLO () const override { return larsson_leven_marq_iters_lo; }
1373	bool isNonRandomnessTest () const override { return is_nonrand_test; }
1374	bool isMaskRequired () const override { return need_mask; }
1375	NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; }
1376	int getKNN () const override { return k_nearest_neighbors; }
1377	ErrorMetric getError () const override { return est_error; }
1378	EstimationMethod getEstimator () const override { return estimator; }
1379	int getSampleSize () const override { return sample_size; }
1380	int getFinalLSQIterations () const override { return final_lsq_iters; }
1381	int getDegreesOfFreedom () const override { return DoF; }
1382	double getSigmaQuantile () const override { return sigma_quantile; }
1383	double getUpperIncompleteOfSigmaQuantile () const override {
1384	return upper_incomplete_of_sigma_quantile;
1385	}
1386	double getLowerIncompleteOfSigmaQuantile () const override {
1387	return lower_incomplete_of_sigma_quantile;
1388	}
1389	double getC () const override { return C; }
1390	double getKmlesac () const override { return k_mlesac; }
1391	double getMaximumThreshold () const override { return maximum_thr; }
1392	double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; }
1393	int getLOSampleSize () const override { return lo_sample_size; }
1394	MethodSolver getRansacSolver () const override { return null_solver; }
1395	PolishingMethod getFinalPolisher () const override { return polisher; }
1396	int getLOThresholdMultiplier() const override { return lo_thr_multiplier; }
1397	int getLOIterativeSampleSize() const override { return lo_iter_sample_size; }
1398	int getLOIterativeMaxIters() const override { return lo_iterative_iterations; }
1399	int getLOInnerMaxIters() const override { return lo_inner_iterations; }
1400	int getPlaneAndParallaxIters () const override { return plane_and_parallax_max_iters; }
1401	LocalOptimMethod getLO () const override { return lo; }
1402	ScoreMethod getScore () const override { return score; }
1403	int getMaxIters () const override { return max_iterations; }
1404	double getConfidence () const override { return confidence; }
1405	double getThreshold () const override { return threshold; }
1406	VerificationMethod getVerifier () const override { return verifier; }
1407	SamplingMethod getSampler () const override { return sampler; }
1408	int getRandomGeneratorState () const override { return random_generator_state; }
1409	double getSPRTdelta () const override { return sprt_delta; }
1410	double getSPRTepsilon () const override { return sprt_eps; }
1411	double getSPRTavgNumModels () const override { return avg_num_models; }
1412	int getCellSize () const override { return cell_size; }
1413	int getGraphRadius() const override { return radius; }
1414	double getTimeForModelEstimation () const override { return model_est_to_ver_time; }
1415	double getRelaxCoef () const override { return relax_coef; }
1416	const std::vector<int> &getGridCellNumber () const override { return grid_cell_number; }
1417	bool isLarssonOptimization () const override { return is_larsson_optimization; }
1418	bool isParallel () const override { return is_parallel; }
1419	bool isFundamental () const override {
1420	return estimator == EstimationMethod::FUNDAMENTAL ||
1421	estimator == EstimationMethod::FUNDAMENTAL8;
1422	}
1423	bool isHomography () const override { return estimator == EstimationMethod::HOMOGRAPHY; }
1424	bool isEssential () const override { return estimator == EstimationMethod::ESSENTIAL; }
1425	bool isPnP() const override {
1426	return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P;
1427	}
1428	};
1429
1430	Ptr<Model> Model::create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
1431	double confidence_, int max_iterations_, ScoreMethod score_) {
1432	return makePtr<ModelImpl>(a1: threshold_, a1: estimator_, a1: sampler_, a1: confidence_,
1433	a1: max_iterations_, a1: score_);
1434	}
1435
1436	bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2,
1437	Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
1438	InputArray dist_coeff1, InputArray dist_coeff2) {
1439	Ransac ransac (params, points1, points2, K1_, K2_, dist_coeff1, dist_coeff2);
1440	if (ransac.run(ransac_output)) {
1441	if (params->isPnP()) {
1442	// convert R to rodrigues and back and recalculate inliers which due to numerical
1443	// issues can differ
1444	Mat out, newP;
1445	Matx33d R, newR, K1;
1446	Vec3d t, rvec;
1447	if (K1_.empty()) {
1448	usac::Utils::decomposeProjection (P: ransac_output->getModel(), K_&: K1, R, t);
1449	Rodrigues(src: R, dst: rvec);
1450	hconcat(src1: rvec, src2: t, dst: out);
1451	hconcat(src1: out, src2: K1, dst: out);
1452	} else {
1453	K1 = ransac.K1;
1454	const Mat Rt = Mat(Matx33d(K1).inv() * Matx34d(ransac_output->getModel()));
1455	t = Rt.col(x: 3);
1456	Rodrigues(src: Rt.colRange(startcol: 0,endcol: 3), dst: rvec);
1457	hconcat(src1: rvec, src2: t, dst: out);
1458	}
1459	// Matx33d _K1(K1);
1460	Rodrigues(src: rvec, dst: newR);
1461	hconcat(src1: K1 * Matx33d(newR), src2: K1 * Vec3d(t), dst: newP);
1462	std::vector<bool> inliers_mask(ransac.points_size);
1463	ransac._quality->getInliers(model: newP, inliers_mask);
1464	ransac_output = RansacOutput::create(model_: out, inliers_mask_: inliers_mask, number_inliers_: ransac_output->getNumberOfInliers(),
1465	number_iterations_: ransac_output->getNumberOfIters(), conf: ransac_output->getConfidence(), errors_: ransac_output->getResiduals());
1466	}
1467	return true;
1468	}
1469	return false;
1470	}
1471	}}
1472