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
8 namespace cv { namespace usac {
9 ////////////////////////////////// STANDARD TERMINATION ///////////////////////////////////////////
10 class StandardTerminationCriteriaImpl : public StandardTerminationCriteria {
11 private:
12 const double log_confidence;
13 const int points_size, sample_size, MAX_ITERATIONS;
14 public:
StandardTerminationCriteriaImpl(double confidence,int points_size_,int sample_size_,int max_iterations_)15 StandardTerminationCriteriaImpl (double confidence, int points_size_,
16 int sample_size_, int max_iterations_) :
17 log_confidence(log(1 - confidence)), points_size (points_size_),
18 sample_size (sample_size_), MAX_ITERATIONS(max_iterations_) {}
19
20 /*
21 * Get upper bound iterations for any sample number
22 * n is points size, w is inlier ratio, p is desired probability, k is expceted number of iterations.
23 * 1 - p = (1 - w^n)^k,
24 * k = log_(1-w^n) (1-p)
25 * k = ln (1-p) / ln (1-w^n)
26 *
27 * w^n is probability that all N points are inliers.
28 * (1 - w^n) is probability that at least one point of N is outlier.
29 * 1 - p = (1-w^n)^k is probability that in K steps of getting at least one outlier is 1% (5%).
30 */
update(const Mat &,int inlier_number)31 int update (const Mat &/*model*/, int inlier_number) override {
32 const double predicted_iters = log_confidence / log(1 - std::pow
33 (static_cast<double>(inlier_number) / points_size, sample_size));
34
35 // if inlier_prob == 1 then log(0) = -inf, predicted_iters == -0
36 // if inlier_prob == 0 then log(1) = 0 , predicted_iters == (+-) inf
37
38 if (! std::isinf(predicted_iters) && predicted_iters < MAX_ITERATIONS)
39 return static_cast<int>(predicted_iters);
40 return MAX_ITERATIONS;
41 }
42
clone() const43 Ptr<TerminationCriteria> clone () const override {
44 return makePtr<StandardTerminationCriteriaImpl>(1-exp(log_confidence), points_size,
45 sample_size, MAX_ITERATIONS);
46 }
47 };
create(double confidence,int points_size_,int sample_size_,int max_iterations_)48 Ptr<StandardTerminationCriteria> StandardTerminationCriteria::create(double confidence,
49 int points_size_, int sample_size_, int max_iterations_) {
50 return makePtr<StandardTerminationCriteriaImpl>(confidence, points_size_,
51 sample_size_, max_iterations_);
52 }
53
54 /////////////////////////////////////// SPRT TERMINATION //////////////////////////////////////////
55 class SPRTTerminationImpl : public SPRTTermination {
56 private:
57 const std::vector<SPRT_history> &sprt_histories;
58 const double log_eta_0;
59 const int points_size, sample_size, MAX_ITERATIONS;
60 public:
SPRTTerminationImpl(const std::vector<SPRT_history> & sprt_histories_,double confidence,int points_size_,int sample_size_,int max_iterations_)61 SPRTTerminationImpl (const std::vector<SPRT_history> &sprt_histories_, double confidence,
62 int points_size_, int sample_size_, int max_iterations_)
63 : sprt_histories (sprt_histories_), log_eta_0(log(1-confidence)),
64 points_size (points_size_), sample_size (sample_size_),MAX_ITERATIONS(max_iterations_){}
65
66 /*
67 * Termination criterion:
68 * l is number of tests
69 * n(l) = Product from i = 0 to l ( 1 - P_g (1 - A(i)^(-h(i)))^k(i) )
70 * log n(l) = sum from i = 0 to l k(i) * ( 1 - P_g (1 - A(i)^(-h(i))) )
71 *
72 * log (n0) - log (n(l-1))
73 * k(l) = ----------------------- (9)
74 * log (1 - P_g*A(l)^-1)
75 *
76 * A is decision threshold
77 * P_g is probability of good model.
78 * k(i) is number of samples verified by i-th sprt.
79 * n0 is typically set to 0.05
80 * this equation does not have to be evaluated before nR < n0
81 * nR = (1 - P_g)^k
82 */
update(const Mat &,int inlier_size)83 int update (const Mat &/*model*/, int inlier_size) override {
84 if (sprt_histories.empty())
85 return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size));
86
87 const double epsilon = static_cast<double>(inlier_size) / points_size; // inlier probability
88 const double P_g = pow (epsilon, sample_size); // probability of good sample
89
90 double log_eta_lmin1 = 0;
91
92 int total_number_of_tested_samples = 0;
93 const int sprts_size_min1 = static_cast<int>(sprt_histories.size())-1;
94 if (sprts_size_min1 < 0) return getStandardUpperBound(inlier_size);
95 // compute log n(l-1), l is number of tests
96 for (int test = 0; test < sprts_size_min1; test++) {
97 log_eta_lmin1 += log (1 - P_g * (1 - pow (sprt_histories[test].A,
98 -computeExponentH(sprt_histories[test].epsilon, epsilon,sprt_histories[test].delta))))
99 * sprt_histories[test].tested_samples;
100 total_number_of_tested_samples += sprt_histories[test].tested_samples;
101 }
102
103 // Implementation note: since η > ηR the equation (9) does not have to be evaluated
104 // before ηR < η0 is satisfied.
105 if (std::pow(1 - P_g, total_number_of_tested_samples) < log_eta_0)
106 return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size));
107 // use decision threshold A for last test (l-th)
108 const double predicted_iters_sprt = (log_eta_0 - log_eta_lmin1) /
109 log (1 - P_g * (1 - 1 / sprt_histories[sprts_size_min1].A)); // last A
110 if (std::isnan(predicted_iters_sprt) || std::isinf(predicted_iters_sprt))
111 return getStandardUpperBound(inlier_size);
112
113 if (predicted_iters_sprt < 0) return 0;
114 // compare with standard upper bound
115 if (predicted_iters_sprt < MAX_ITERATIONS)
116 return std::min(static_cast<int>(predicted_iters_sprt),
117 getStandardUpperBound(inlier_size));
118 return getStandardUpperBound(inlier_size);
119 }
120
clone() const121 Ptr<TerminationCriteria> clone () const override {
122 return makePtr<SPRTTerminationImpl>(sprt_histories, 1-exp(log_eta_0), points_size,
123 sample_size, MAX_ITERATIONS);
124 }
125 private:
getStandardUpperBound(int inlier_size) const126 inline int getStandardUpperBound(int inlier_size) const {
127 const double predicted_iters = log_eta_0 / log(1 - std::pow
128 (static_cast<double>(inlier_size) / points_size, sample_size));
129 return (! std::isinf(predicted_iters) && predicted_iters < MAX_ITERATIONS) ?
130 static_cast<int>(predicted_iters) : MAX_ITERATIONS;
131 }
132 /*
133 * h(i) must hold
134 *
135 * δ(i) 1 - δ(i)
136 * ε (-----)^h(i) + (1 - ε) (--------)^h(i) = 1
137 * ε(i) 1 - ε(i)
138 *
139 * ε * a^h + (1 - ε) * b^h = 1
140 * Has numerical solution.
141 */
computeExponentH(double epsilon,double epsilon_new,double delta)142 static double computeExponentH (double epsilon, double epsilon_new, double delta) {
143 const double a = log (delta / epsilon); // log likelihood ratio
144 const double b = log ((1 - delta) / (1 - epsilon));
145
146 const double x0 = log (1 / (1 - epsilon_new)) / b;
147 const double v0 = epsilon_new * exp (x0 * a);
148 const double x1 = log ((1 - 2*v0) / (1 - epsilon_new)) / b;
149 const double v1 = epsilon_new * exp (x1 * a) + (1 - epsilon_new) * exp(x1 * b);
150 const double h = x0 - (x0 - x1) / (1 + v0 - v1) * v0;
151
152 if (std::isnan(h))
153 // The equation always has solution for h = 0
154 // ε * a^0 + (1 - ε) * b^0 = 1
155 // ε + 1 - ε = 1 -> 1 = 1
156 return 0;
157 return h;
158 }
159 };
create(const std::vector<SPRT_history> & sprt_histories_,double confidence,int points_size_,int sample_size_,int max_iterations_)160 Ptr<SPRTTermination> SPRTTermination::create(const std::vector<SPRT_history> &sprt_histories_,
161 double confidence, int points_size_, int sample_size_, int max_iterations_) {
162 return makePtr<SPRTTerminationImpl>(sprt_histories_, confidence, points_size_, sample_size_,
163 max_iterations_);
164 }
165
166 ///////////////////////////// PROGRESSIVE-NAPSAC-SPRT TERMINATION /////////////////////////////////
167 class SPRTPNapsacTerminationImpl : public SPRTPNapsacTermination {
168 private:
169 SPRTTerminationImpl sprt_termination;
170 const std::vector<SPRT_history> &sprt_histories;
171 const double relax_coef, log_confidence;
172 const int points_size, sample_size, MAX_ITERS;
173 public:
174
SPRTPNapsacTerminationImpl(const std::vector<SPRT_history> & sprt_histories_,double confidence,int points_size_,int sample_size_,int max_iterations_,double relax_coef_)175 SPRTPNapsacTerminationImpl (const std::vector<SPRT_history> &sprt_histories_,
176 double confidence, int points_size_, int sample_size_,
177 int max_iterations_, double relax_coef_)
178 : sprt_termination (sprt_histories_, confidence, points_size_, sample_size_,
179 max_iterations_), sprt_histories (sprt_histories_),
180 relax_coef (relax_coef_), log_confidence(log(1-confidence)),
181 points_size (points_size_), sample_size (sample_size_),
182 MAX_ITERS (max_iterations_) {}
183
update(const Mat & model,int inlier_number)184 int update (const Mat &model, int inlier_number) override {
185 int predicted_iterations = sprt_termination.update(model, inlier_number);
186
187 const double inlier_prob = static_cast<double>(inlier_number) / points_size + relax_coef;
188 if (inlier_prob >= 1)
189 return 0;
190
191 const double predicted_iters = log_confidence / log(1 - std::pow(inlier_prob, sample_size));
192
193 if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations)
194 return static_cast<int>(predicted_iters);
195 return predicted_iterations;
196 }
clone() const197 Ptr<TerminationCriteria> clone () const override {
198 return makePtr<SPRTPNapsacTerminationImpl>(sprt_histories, 1-exp(log_confidence),
199 points_size, sample_size, MAX_ITERS, relax_coef);
200 }
201 };
create(const std::vector<SPRT_history> & sprt_histories_,double confidence,int points_size_,int sample_size_,int max_iterations_,double relax_coef_)202 Ptr<SPRTPNapsacTermination> SPRTPNapsacTermination::create(const std::vector<SPRT_history>&
203 sprt_histories_, double confidence, int points_size_, int sample_size_,
204 int max_iterations_, double relax_coef_) {
205 return makePtr<SPRTPNapsacTerminationImpl>(sprt_histories_, confidence, points_size_,
206 sample_size_, max_iterations_, relax_coef_);
207 }
208 ////////////////////////////////////// PROSAC TERMINATION /////////////////////////////////////////
209
210 class ProsacTerminationCriteriaImpl : public ProsacTerminationCriteria {
211 private:
212 const double log_confidence, beta, non_randomness_phi, inlier_threshold;
213 const int MAX_ITERATIONS, points_size, min_termination_length, sample_size;
214 const Ptr<ProsacSampler> sampler;
215
216 std::vector<int> non_random_inliers;
217
218 const Ptr<Error> error;
219 public:
ProsacTerminationCriteriaImpl(const Ptr<Error> & error_,int points_size_,int sample_size_,double confidence,int max_iterations,int min_termination_length_,double beta_,double non_randomness_phi_,double inlier_threshold_)220 ProsacTerminationCriteriaImpl (const Ptr<Error> &error_, int points_size_,int sample_size_,
221 double confidence, int max_iterations, int min_termination_length_, double beta_,
222 double non_randomness_phi_, double inlier_threshold_) : log_confidence
223 (log(1-confidence)), beta(beta_), non_randomness_phi(non_randomness_phi_),
224 inlier_threshold(inlier_threshold_), MAX_ITERATIONS(max_iterations),
225 points_size (points_size_), min_termination_length (min_termination_length_),
226 sample_size(sample_size_), error (error_) { init(); }
227
ProsacTerminationCriteriaImpl(const Ptr<ProsacSampler> & sampler_,const Ptr<Error> & error_,int points_size_,int sample_size_,double confidence,int max_iterations,int min_termination_length_,double beta_,double non_randomness_phi_,double inlier_threshold_)228 ProsacTerminationCriteriaImpl (const Ptr<ProsacSampler> &sampler_,const Ptr<Error> &error_,
229 int points_size_, int sample_size_, double confidence, int max_iterations,
230 int min_termination_length_, double beta_, double non_randomness_phi_,
231 double inlier_threshold_) : log_confidence(log(1-confidence)), beta(beta_),
232 non_randomness_phi(non_randomness_phi_), inlier_threshold(inlier_threshold_),
233 MAX_ITERATIONS(max_iterations), points_size (points_size_),
234 min_termination_length (min_termination_length_), sample_size(sample_size_),
235 sampler(sampler_), error (error_) { init(); }
236
init()237 void init () {
238 // m is sample_size
239 // N is points_size
240
241 // non-randomness constraint
242 // The non-randomness requirement prevents PROSAC
243 // from selecting a solution supported by outliers that are
244 // by chance consistent with it. The constraint is typically
245 // checked ex-post in standard approaches [1]. The distribution
246 // of the cardinalities of sets of random ‘inliers’ is binomial
247 // i-th entry - inlier counts for termination up to i-th point (term length = i+1)
248
249 // ------------------------------------------------------------------------
250 // initialize the data structures that determine stopping
251 // see probabilities description below.
252
253 non_random_inliers = std::vector<int>(points_size, 0);
254 std::vector<double> pn_i_arr(points_size);
255 const double beta2compl_beta = beta / (1-beta);
256 const int step_n = 50, max_n = std::min(points_size, 1200);
257 for (int n = sample_size; n <= points_size; n+=step_n) {
258 if (n > max_n) {
259 // skip expensive calculation
260 break;
261 }
262
263 // P^R_n(i) = β^(i−m) (1−β)^(n−i+m) (n−m i−m). (7) i = m,...,N
264 // initial value for i = m = sample_size
265 // P^R_n(i=m) = β^(0) (1−β)^(n) (n-m 0) = (1-β)^(n)
266 // P^R_n(i=m+1) = β^(1) (1−β)^(n−1) (n−m 1) = P^R_n(i=m) * β / (1-β) * (n-m) / 1
267 // P^R_n(i=m+2) = β^(2) (1−β)^(n−2) (n−m 2) = P^R_n(i=m) * β^2 / (1-β)^2 * (n-m-1)(n-m) / 2
268 // So, for each i=m+1.., P^R_n(i+1) must be calculated as P^R_n(i) * β / (1-β) * (n-i+1) / (i-m)
269
270 pn_i_arr[sample_size-1] = std::pow(1-beta, n);
271 double pn_i = pn_i_arr[sample_size-1]; // prob of random inlier set of size i for subset size n
272 for (int i = sample_size+1; i <= n; i++) {
273 // use recurrent relation to fulfill remaining values
274 pn_i *= beta2compl_beta * static_cast<double>(n-i+1) / (i-sample_size);
275 // update
276 pn_i_arr[i-1] = pn_i;
277 }
278
279 // find minimum number of inliers satisfying the non-randomness constraint
280 // Imin n = min{j : n∑i=j P^R_n(i) < Ψ }. (8)
281 double acc = 0;
282 int i_min = sample_size; // there is always sample_size inliers
283 for (int i = n; i >= sample_size; i--) {
284 acc += pn_i_arr[i-1];
285 if (acc < non_randomness_phi) i_min = i;
286 else break;
287 }
288 non_random_inliers[n-1] = i_min;
289 }
290
291 // approximate values of binomial distribution
292 for (int n = sample_size; n <= points_size; n+=step_n) {
293 if (n-1+step_n >= max_n) {
294 // copy rest of the values
295 std::fill(&non_random_inliers[0]+n-1, &non_random_inliers[0]+points_size, non_random_inliers[n-1]);
296 break;
297 }
298 const int non_rand_n = non_random_inliers[n-1];
299 const double step = (double)(non_random_inliers[n-1+step_n] - non_rand_n) / (double)step_n;
300 for (int i = 0; i < step_n-1; i++)
301 non_random_inliers[n+i] = (int)(non_rand_n + (i+1)*step);
302 }
303 }
304 /*
305 * The PROSAC algorithm terminates if the number of inliers I_n*
306 * within the set U_n* satisfies the following conditions:
307 *
308 * • non-randomness – the probability that I_n* out of n* (termination_length)
309 * data points are by chance inliers to an arbitrary incorrect model
310 * is smaller than Ψ (typically set to 5%)
311 *
312 * • maximality – the probability that a solution with more than
313 * In* inliers in U_n* exists and was not found after k
314 * samples is smaller than η0 (typically set to 5%).
315 */
update(const Mat & model,int inliers_size)316 int update (const Mat &model, int inliers_size) override {
317 int predicted_iterations = MAX_ITERATIONS;
318 /*
319 * The termination length n* is chosen to minimize k_n*(η0) subject to I_n* ≥ I_min n*;
320 * k_n*(η0) >= log(η0) / log(1 - (I_n* / n*)^m)
321 * g(k) <= n, I_n is number of inliers under termination length n.
322 */
323 const auto &errors = error->getErrors(model);
324
325 // find number of inliers under g(k)
326 int num_inliers_under_termination_len = 0;
327 for (int pt = 0; pt < min_termination_length; pt++)
328 if (errors[pt] < inlier_threshold)
329 num_inliers_under_termination_len++;
330
331 for (int termination_len = min_termination_length; termination_len < points_size;termination_len++){
332 if (errors[termination_len /* = point*/] < inlier_threshold) {
333 num_inliers_under_termination_len++;
334
335 // non-random constraint must satisfy I_n* ≥ I_min n*.
336 if (num_inliers_under_termination_len < non_random_inliers[termination_len])
337 continue;
338
339 // add 1 to termination length since num_inliers_under_termination_len is updated
340 const double new_max_samples = log_confidence / log(1 -
341 std::pow(static_cast<double>(num_inliers_under_termination_len)
342 / (termination_len+1), sample_size));
343
344 if (! std::isinf(new_max_samples) && predicted_iterations > new_max_samples) {
345 predicted_iterations = static_cast<int>(new_max_samples);
346 if (predicted_iterations == 0) break;
347 if (sampler != nullptr)
348 sampler->setTerminationLength(termination_len);
349 }
350 }
351 }
352
353 // compare also when termination length = points_size,
354 // so inliers under termination length is total number of inliers:
355 const double predicted_iters = log_confidence / log(1 - std::pow
356 (static_cast<double>(inliers_size) / points_size, sample_size));
357
358 if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations)
359 return static_cast<int>(predicted_iters);
360 return predicted_iterations;
361 }
362
clone() const363 Ptr<TerminationCriteria> clone () const override {
364 return makePtr<ProsacTerminationCriteriaImpl>(error->clone(),
365 points_size, sample_size, 1-exp(log_confidence), MAX_ITERATIONS,
366 min_termination_length, beta, non_randomness_phi, inlier_threshold);
367 }
368 };
369
370 Ptr<ProsacTerminationCriteria>
create(const Ptr<ProsacSampler> & sampler,const Ptr<Error> & error,int points_size_,int sample_size_,double confidence,int max_iterations,int min_termination_length_,double beta,double non_randomness_phi,double inlier_thresh)371 ProsacTerminationCriteria::create(const Ptr<ProsacSampler> &sampler, const Ptr<Error> &error,
372 int points_size_, int sample_size_, double confidence, int max_iterations,
373 int min_termination_length_, double beta, double non_randomness_phi, double inlier_thresh) {
374 return makePtr<ProsacTerminationCriteriaImpl> (sampler, error, points_size_, sample_size_,
375 confidence, max_iterations, min_termination_length_,
376 beta, non_randomness_phi, inlier_thresh);
377 }
378 }}
379