1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.ar.calibration.estimators;
17
18 import com.irurueta.ar.calibration.ImageOfAbsoluteConic;
19 import com.irurueta.geometry.Transformation2D;
20 import com.irurueta.geometry.estimators.LockedException;
21 import com.irurueta.geometry.estimators.NotReadyException;
22 import com.irurueta.numerical.robust.PROMedSRobustEstimator;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.ArrayList;
29 import java.util.List;
30
31 /**
32 * Finds the best image of absolute conic (IAC) for provided collection of
33 * homographies (2D transformations) using PROMedS algorithm.
34 */
35 public class PROMedSImageOfAbsoluteConicRobustEstimator extends ImageOfAbsoluteConicRobustEstimator {
36
37 /**
38 * Default value to be used for stop threshold. Stop threshold can be used
39 * to keep the algorithm iterating in case that best estimated threshold
40 * using median of residuals is not small enough. Once a solution is found
41 * that generates a threshold below this value, the algorithm will stop.
42 * Threshold is defined by equations h1'*IAC*h2 = 0 and
43 * h1'*IAC*h1 = h2'*IAC*h2 --< h1'*IAC*h1 - h2'*IAC*h2 = 0, where
44 * h1 and h2 are the 1st and 2nd columns of an homography (2D
45 * transformation).
46 * These equations are derived from the fact that rotation matrices are
47 * orthonormal.
48 */
49 public static final double DEFAULT_STOP_THRESHOLD = 1e-6;
50
51 /**
52 * Minimum value that can be set as stop threshold.
53 * Threshold must be strictly greater than 0.0.
54 */
55 public static final double MIN_STOP_THRESHOLD = 0.0;
56
57 /**
58 * Threshold to be used to keep the algorithm iterating in case that best
59 * estimated threshold using median of residuals is not small enough. Once
60 * a solution is found that generates a threshold below this value, the
61 * algorithm will stop.
62 * The stop threshold can be used to prevent the PROMedS algorithm iterating
63 * too many times in cases where samples have a very similar accuracy.
64 * For instance, in cases where proportion of outliers is very small (close
65 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
66 * iterate for a long time trying to find the best solution when indeed
67 * there is no need to do that if a reasonable threshold has already been
68 * reached.
69 * Because of this behaviour the stop threshold can be set to a value much
70 * lower than the one typically used in RANSAC, and yet the algorithm could
71 * still produce even smaller thresholds in estimated results.
72 */
73 private double stopThreshold;
74
75 /**
76 * Quality scores corresponding to each provided homography.
77 * The larger the score value the better the quality of the sample.
78 */
79 private double[] qualityScores;
80
81 /**
82 * Constructor.
83 */
84 public PROMedSImageOfAbsoluteConicRobustEstimator() {
85 super();
86 stopThreshold = DEFAULT_STOP_THRESHOLD;
87 }
88
89 /**
90 * Constructor.
91 *
92 * @param listener listener to be notified of events such as when
93 * estimation starts, ends or its progress significantly changes.
94 */
95 public PROMedSImageOfAbsoluteConicRobustEstimator(final ImageOfAbsoluteConicRobustEstimatorListener listener) {
96 super(listener);
97 stopThreshold = DEFAULT_STOP_THRESHOLD;
98 }
99
100 /**
101 * Constructor.
102 *
103 * @param homographies list of homographies (2D transformations) used to
104 * estimate the image of absolute conic (IAC), which can be used to obtain
105 * pinhole camera intrinsic parameters.
106 * @throws IllegalArgumentException if not enough homographies are provided
107 * for default settings. Hence, at least 1 homography must be provided.
108 */
109 public PROMedSImageOfAbsoluteConicRobustEstimator(final List<Transformation2D> homographies) {
110 super(homographies);
111 stopThreshold = DEFAULT_STOP_THRESHOLD;
112 }
113
114 /**
115 * Constructor.
116 *
117 * @param homographies list of homographies (2D transformations) used to
118 * estimate the image of absolute conic (IAC), which can be used to obtain
119 * pinhole camera intrinsic parameters.
120 * @param listener listener to be notified of events such as when estimation
121 * starts, ends or estimation progress changes.
122 * @throws IllegalArgumentException if not enough homographies are provided
123 * for default settings. Hence, at least 1 homography must be provided.
124 */
125 public PROMedSImageOfAbsoluteConicRobustEstimator(
126 final List<Transformation2D> homographies, final ImageOfAbsoluteConicRobustEstimatorListener listener) {
127 super(homographies, listener);
128 stopThreshold = DEFAULT_STOP_THRESHOLD;
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param qualityScores quality scores corresponding to each provided
135 * homography.
136 * @throws IllegalArgumentException if provided quality scores length is
137 * smaller than required number of homographies for default settings
138 * (i.e. 1 homography).
139 */
140 public PROMedSImageOfAbsoluteConicRobustEstimator(final double[] qualityScores) {
141 this();
142 internalSetQualityScores(qualityScores);
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param qualityScores quality scores corresponding to each provided
149 * homography.
150 * @param listener listener to be notified of events such as when
151 * estimation starts, ends or its progress significantly changes.
152 * @throws IllegalArgumentException if provided quality scores length is
153 * smaller than required number of homographies for default settings
154 * (i.e. 1 homography).
155 */
156 public PROMedSImageOfAbsoluteConicRobustEstimator(
157 final double[] qualityScores, final ImageOfAbsoluteConicRobustEstimatorListener listener) {
158 this(listener);
159 internalSetQualityScores(qualityScores);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param homographies list of homographies (2D transformations) used to
166 * estimate the image of absolute conic (IAC), which can be used to obtain
167 * pinhole camera intrinsic parameters.
168 * @param qualityScores quality scores corresponding to each provided
169 * homography.
170 * @throws IllegalArgumentException if not enough homographies are provided
171 * for default settings (i.e. 1 homography) or quality scores and
172 * homographies don't have the same size.
173 */
174 public PROMedSImageOfAbsoluteConicRobustEstimator(
175 final List<Transformation2D> homographies, final double[] qualityScores) {
176 this(homographies);
177 internalSetQualityScores(qualityScores);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param homographies list of homographies (2D transformations) used to
184 * estimate the image of absolute conic (IAC), which can be used to obtain
185 * pinhole camera intrinsic parameters.
186 * @param qualityScores quality scores corresponding to each provided
187 * homography.
188 * @param listener listener to be notified of events such as when estimation
189 * starts, ends or estimation progress changes.
190 * @throws IllegalArgumentException if not enough homographies are provided
191 * for default settings (i.e. 1 homography) or quality scores and
192 * homographies don't have the same size.
193 */
194 public PROMedSImageOfAbsoluteConicRobustEstimator(
195 final List<Transformation2D> homographies, final double[] qualityScores,
196 final ImageOfAbsoluteConicRobustEstimatorListener listener) {
197 this(homographies, listener);
198 internalSetQualityScores(qualityScores);
199 }
200
201 /**
202 * Returns threshold to be used to keep the algorithm iterating in case that
203 * best estimated threshold using median of residuals is not small enough.
204 * Once a solution is found that generates a threshold below this value, the
205 * algorithm will stop.
206 * The stop threshold can be used to prevent the PROMedS algorithm iterating
207 * too many times in cases where samples have a very similar accuracy.
208 * For instance, in cases where proportion of outliers is very small (close
209 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
210 * iterate for a long time trying to find the best solution when indeed
211 * there is no need to do that if a reasonable threshold has already been
212 * reached.
213 * Because of this behaviour the stop threshold can be set to a value much
214 * lower than the one typically used in RANSAC, and yet the algorithm could
215 * still produce even smaller thresholds in estimated results.
216 *
217 * @return stop threshold to stop the algorithm prematurely when a certain
218 * accuracy has been reached.
219 */
220 public double getStopThreshold() {
221 return stopThreshold;
222 }
223
224 /**
225 * Sets threshold to be used to keep the algorithm iterating in case that
226 * best estimated threshold using median of residuals is not small enough.
227 * Once a solution is found that generates a threshold below this value, the
228 * algorithm will stop.
229 * The stop threshold can be used to prevent the PROMedS algorithm iterating
230 * too many times in cases where samples have a very similar accuracy.
231 * For instance, in cases where proportion of outliers is very small (close
232 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
233 * iterate for a long time trying to find the best solution when indeed
234 * there is no need to do that if a reasonable threshold has already been
235 * reached.
236 * Because of this behaviour the stop threshold can be set to a value much
237 * lower than the one typically used in RANSAC, and yet the algorithm could
238 * still produce even smaller thresholds in estimated results.
239 *
240 * @param stopThreshold stop threshold to stop the algorithm prematurely
241 * when a certain accuracy has been reached.
242 * @throws IllegalArgumentException if provided value is zero or negative.
243 * @throws LockedException if robust estimator is locked because an
244 * estimation is already in progress.
245 */
246 public void setStopThreshold(final double stopThreshold) throws LockedException {
247 if (isLocked()) {
248 throw new LockedException();
249 }
250 if (stopThreshold <= MIN_STOP_THRESHOLD) {
251 throw new IllegalArgumentException();
252 }
253 this.stopThreshold = stopThreshold;
254 }
255
256 /**
257 * Returns quality scores corresponding to each provided homography.
258 * The larger the score value the better the quality of the sampled
259 * homography
260 *
261 * @return quality scores corresponding to each homography.
262 */
263 @Override
264 public double[] getQualityScores() {
265 return qualityScores;
266 }
267
268 /**
269 * Sets quality scores corresponding to each provided homography.
270 * The larger the score value the better the quality of the sampled
271 * homography.
272 *
273 * @param qualityScores quality scores corresponding to each homography.
274 * @throws LockedException if robust estimator is locked because an
275 * estimation is already in progress.
276 * @throws IllegalArgumentException if provided quality scores length is
277 * smaller than minimum required number of homographies (i.e. 1
278 * homography).
279 */
280 @Override
281 public void setQualityScores(final double[] qualityScores) throws LockedException {
282 if (isLocked()) {
283 throw new LockedException();
284 }
285 internalSetQualityScores(qualityScores);
286 }
287
288 /**
289 * Indicates if estimator is ready to start the IAC estimation.
290 * This is true when input data (i.e. homographies) is provided and list
291 * contains at least the minimum number of required homographies, and
292 * also quality scores are provided.
293 *
294 * @return true if estimator is ready, false otherwise.
295 */
296 @Override
297 public boolean isReady() {
298 return super.isReady() && qualityScores != null && qualityScores.length == homographies.size();
299 }
300
301 /**
302 * Estimates Image of Absolute Conic (IAC)
303 *
304 * @return estimated IAC
305 * @throws LockedException if robust estimator is locked because an
306 * estimation is already in progress
307 * @throws NotReadyException if provided input data is not enough to start
308 * the estimation
309 * @throws RobustEstimatorException if estimation fails for any reason
310 * (i.e. numerical instability, no solution available, etc)
311 */
312 @SuppressWarnings("DuplicatedCode")
313 @Override
314 public ImageOfAbsoluteConic estimate() throws LockedException, NotReadyException, RobustEstimatorException {
315 if (isLocked()) {
316 throw new LockedException();
317 }
318 if (!isReady()) {
319 throw new NotReadyException();
320 }
321
322 final var innerEstimator = new PROMedSRobustEstimator<ImageOfAbsoluteConic>(
323 new PROMedSRobustEstimatorListener<>() {
324
325 // subset of homographies picked on each iteration
326 private final List<Transformation2D> subsetHomographies = new ArrayList<>();
327
328 @Override
329 public double getThreshold() {
330 return stopThreshold;
331 }
332
333 @Override
334 public int getTotalSamples() {
335 return homographies.size();
336 }
337
338 @Override
339 public int getSubsetSize() {
340 return iacEstimator.getMinNumberOfRequiredHomographies();
341 }
342
343 @Override
344 public void estimatePreliminarSolutions(
345 final int[] samplesIndices, final List<ImageOfAbsoluteConic> solutions) {
346 subsetHomographies.clear();
347 for (final var samplesIndex : samplesIndices) {
348 subsetHomographies.add(homographies.get(samplesIndex));
349 }
350
351 try {
352 iacEstimator.setLMSESolutionAllowed(false);
353 iacEstimator.setHomographies(subsetHomographies);
354
355 final var iac = iacEstimator.estimate();
356 solutions.add(iac);
357 } catch (final Exception e) {
358 // if anything fails, no solution is added
359 }
360 }
361
362 @Override
363 public double computeResidual(final ImageOfAbsoluteConic currentEstimation, final int i) {
364 return residual(currentEstimation, homographies.get(i));
365 }
366
367 @Override
368 public boolean isReady() {
369 return PROMedSImageOfAbsoluteConicRobustEstimator.this.isReady();
370 }
371
372 @Override
373 public void onEstimateStart(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
374 if (listener != null) {
375 listener.onEstimateStart(PROMedSImageOfAbsoluteConicRobustEstimator.this);
376 }
377 }
378
379 @Override
380 public void onEstimateEnd(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
381 if (listener != null) {
382 listener.onEstimateEnd(PROMedSImageOfAbsoluteConicRobustEstimator.this);
383 }
384 }
385
386 @Override
387 public void onEstimateNextIteration(
388 final RobustEstimator<ImageOfAbsoluteConic> estimator, final int iteration) {
389 if (listener != null) {
390 listener.onEstimateNextIteration(
391 PROMedSImageOfAbsoluteConicRobustEstimator.this, iteration);
392 }
393 }
394
395 @Override
396 public void onEstimateProgressChange(
397 final RobustEstimator<ImageOfAbsoluteConic> estimator, final float progress) {
398 if (listener != null) {
399 listener.onEstimateProgressChange(
400 PROMedSImageOfAbsoluteConicRobustEstimator.this, progress);
401 }
402 }
403
404 @Override
405 public double[] getQualityScores() {
406 return qualityScores;
407 }
408 });
409
410 try {
411 locked = true;
412 innerEstimator.setConfidence(confidence);
413 innerEstimator.setMaxIterations(maxIterations);
414 innerEstimator.setProgressDelta(progressDelta);
415 return innerEstimator.estimate();
416 } catch (final com.irurueta.numerical.LockedException e) {
417 throw new LockedException(e);
418 } catch (final com.irurueta.numerical.NotReadyException e) {
419 throw new NotReadyException(e);
420 } finally {
421 locked = false;
422 }
423 }
424
425 /**
426 * Returns method being used for robust estimation
427 *
428 * @return method being used for robust estimation
429 */
430 @Override
431 public RobustEstimatorMethod getMethod() {
432 return RobustEstimatorMethod.PROMEDS;
433 }
434
435 /**
436 * Sets quality scores corresponding to each homography.
437 * This method is used internally and does not check whether instance is
438 * locked or not.
439 *
440 * @param qualityScores quality scores to be set.
441 * @throws IllegalArgumentException if provided quality scores length is
442 * smaller than the minimum number of required homographies for current
443 * settings.
444 */
445 private void internalSetQualityScores(final double[] qualityScores) {
446 if (qualityScores.length < iacEstimator.getMinNumberOfRequiredHomographies()) {
447 throw new IllegalArgumentException();
448 }
449
450 this.qualityScores = qualityScores;
451 }
452 }