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.PROSACRobustEstimator;
23 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
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 PROSAC algorithm.
34 */
35 public class PROSACImageOfAbsoluteConicRobustEstimator extends ImageOfAbsoluteConicRobustEstimator {
36
37 /**
38 * Constant defining default threshold to determine whether homographies are
39 * inliers or not.
40 * Threshold is defined by equations h1'*IAC*h2 = 0 and
41 * h1'*IAC*h1 = h2'*IAC*h2 --< h1'*IAC*h1 - h2'*IAC*h2 = 0, where
42 * h1 and h2 are the 1st and 2nd columns of an homography (2D
43 * transformation).
44 * These equations are derived from the fact that rotation matrices are
45 * orthonormal.
46 */
47 public static final double DEFAULT_THRESHOLD = 1e-6;
48
49 /**
50 * Minimum value that can be set as threshold.
51 * Threshold must be strictly greater than 0.0.
52 */
53 public static final double MIN_THRESHOLD = 0.0;
54
55 /**
56 * Threshold to determine whether homographies are inliers or not when
57 * testing possible estimation solutions.
58 * The threshold refers to the amount of error a possible solution has on
59 * the ortho-normality assumption of rotation matrices.
60 */
61 private double threshold;
62
63 /**
64 * Quality scores corresponding to each provided homography.
65 * The larger the score value the better the quality of the sample.
66 */
67 private double[] qualityScores;
68
69 /**
70 * Constructor.
71 */
72 public PROSACImageOfAbsoluteConicRobustEstimator() {
73 super();
74 threshold = DEFAULT_THRESHOLD;
75 }
76
77 /**
78 * Constructor.
79 *
80 * @param listener listener to be notified of events such as when
81 * estimation starts, ends or its progress significantly changes.
82 */
83 public PROSACImageOfAbsoluteConicRobustEstimator(final ImageOfAbsoluteConicRobustEstimatorListener listener) {
84 super(listener);
85 threshold = DEFAULT_THRESHOLD;
86 }
87
88 /**
89 * Constructor.
90 *
91 * @param homographies list of homographies (2D transformations) used to
92 * estimate the image of absolute conic (IAC), which can be used to obtain
93 * pinhole camera intrinsic parameters.
94 * @throws IllegalArgumentException if not enough homographies are provided
95 * for default settings. Hence, at least 1 homography must be provided.
96 */
97 public PROSACImageOfAbsoluteConicRobustEstimator(final List<Transformation2D> homographies) {
98 super(homographies);
99 threshold = DEFAULT_THRESHOLD;
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param homographies list of homographies (2D transformations) used to
106 * estimate the image of absolute conic (IAC), which can be used to obtain
107 * pinhole camera intrinsic parameters.
108 * @param listener listener to be notified of events such as when estimation
109 * starts, ends or estimation progress changes.
110 * @throws IllegalArgumentException if not enough homographies are provided
111 * for default settings. Hence, at least 1 homography must be provided.
112 */
113 public PROSACImageOfAbsoluteConicRobustEstimator(
114 final List<Transformation2D> homographies, final ImageOfAbsoluteConicRobustEstimatorListener listener) {
115 super(homographies, listener);
116 threshold = DEFAULT_THRESHOLD;
117 }
118
119 /**
120 * Constructor.
121 *
122 * @param qualityScores quality scores corresponding to each provided
123 * homography.
124 * @throws IllegalArgumentException if provided quality scores length is
125 * smaller than required number of homographies for default settings
126 * (i.e. 1 homography).
127 */
128 public PROSACImageOfAbsoluteConicRobustEstimator(final double[] qualityScores) {
129 this();
130 internalSetQualityScores(qualityScores);
131 }
132
133 /**
134 * Constructor.
135 *
136 * @param qualityScores quality scores corresponding to each provided
137 * homography.
138 * @param listener listener to be notified of events such as when
139 * estimation starts, ends or its progress significantly changes.
140 * @throws IllegalArgumentException if provided quality scores length is
141 * smaller than required number of homographies for default settings
142 * (i.e. 1 homography).
143 */
144 public PROSACImageOfAbsoluteConicRobustEstimator(
145 final double[] qualityScores, final ImageOfAbsoluteConicRobustEstimatorListener listener) {
146 this(listener);
147 internalSetQualityScores(qualityScores);
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param homographies list of homographies (2D transformations) used to
154 * estimate the image of absolute conic (IAC), which can be used to obtain
155 * pinhole camera intrinsic parameters.
156 * @param qualityScores quality scores corresponding to each provided
157 * homography.
158 * @throws IllegalArgumentException if not enough homographies are provided
159 * for default settings (i.e. 1 homography) or quality scores and
160 * homographies don't have the same size.
161 */
162 public PROSACImageOfAbsoluteConicRobustEstimator(
163 final List<Transformation2D> homographies, final double[] qualityScores) {
164 this(homographies);
165 internalSetQualityScores(qualityScores);
166 }
167
168 /**
169 * Constructor.
170 *
171 * @param homographies list of homographies (2D transformations) used to
172 * estimate the image of absolute conic (IAC), which can be used to obtain
173 * pinhole camera intrinsic parameters.
174 * @param qualityScores quality scores corresponding to each provided
175 * homography.
176 * @param listener listener to be notified of events such as when estimation
177 * starts, ends or estimation progress changes.
178 * @throws IllegalArgumentException if not enough homographies are provided
179 * for default settings (i.e. 1 homography) or quality scores and
180 * homographies don't have the same size.
181 */
182 public PROSACImageOfAbsoluteConicRobustEstimator(
183 final List<Transformation2D> homographies, final double[] qualityScores,
184 final ImageOfAbsoluteConicRobustEstimatorListener listener) {
185 this(homographies, listener);
186 internalSetQualityScores(qualityScores);
187 }
188
189 /**
190 * Returns threshold to determine whether homographies are inliers or not
191 * when testing possible estimation solutions.
192 * The threshold refers to the amount of error a possible solution has on
193 * the ortho-normality assumption of rotation matrices.
194 *
195 * @return threshold to determine whether homographies are inliers or not
196 * when testing possible estimation solutions.
197 */
198 public double getThreshold() {
199 return threshold;
200 }
201
202 /**
203 * Sets threshold to determine whether homographies are inliers or not when
204 * testing possible estimation solutions.
205 * The threshold refers to the amount of error a possible solution has on
206 * the ortho-normality assumption of rotation matrices.
207 *
208 * @param threshold threshold to determine whether homographies are inliers
209 * or not when testing possible estimation solutions.
210 * @throws IllegalArgumentException if provided value is equal or less than
211 * zero.
212 * @throws LockedException if robust estimator is locked because an
213 * estimation is already in progress.
214 */
215 public void setThreshold(final double threshold) throws LockedException {
216 if (isLocked()) {
217 throw new LockedException();
218 }
219 if (threshold <= MIN_THRESHOLD) {
220 throw new IllegalArgumentException();
221 }
222 this.threshold = threshold;
223 }
224
225 /**
226 * Returns quality scores corresponding to each provided homography.
227 * The larger the score value the better the quality of the sampled
228 * homography
229 *
230 * @return quality scores corresponding to each homography.
231 */
232 @Override
233 public double[] getQualityScores() {
234 return qualityScores;
235 }
236
237 /**
238 * Sets quality scores corresponding to each provided homography.
239 * The larger the score value the better the quality of the sampled
240 * homography
241 *
242 * @param qualityScores quality scores corresponding to each homography
243 * @throws LockedException if robust estimator is locked because an
244 * estimation is already in progress.
245 * @throws IllegalArgumentException if provided quality scores length is
246 * smaller than minimum required number of homographies (i.e. 1
247 * homography)
248 */
249 @Override
250 public void setQualityScores(final double[] qualityScores) throws LockedException {
251 if (isLocked()) {
252 throw new LockedException();
253 }
254 internalSetQualityScores(qualityScores);
255 }
256
257 /**
258 * Indicates if estimator is ready to start the IAC estimation.
259 * This is true when input data (i.e. homographies) is provided and list
260 * contains at least the minimum number of required homographies, and
261 * also quality scores are provided.
262 *
263 * @return true if estimator is ready, false otherwise.
264 */
265 @Override
266 public boolean isReady() {
267 return super.isReady() && qualityScores != null && qualityScores.length == homographies.size();
268 }
269
270 /**
271 * Estimates Image of Absolute Conic (IAC).
272 *
273 * @return estimated IAC.
274 * @throws LockedException if robust estimator is locked because an
275 * estimation is already in progress.
276 * @throws NotReadyException if provided input data is not enough to start
277 * the estimation.
278 * @throws RobustEstimatorException if estimation fails for any reason
279 * (i.e. numerical instability, no solution available, etc).
280 */
281 @SuppressWarnings("DuplicatedCode")
282 @Override
283 public ImageOfAbsoluteConic estimate() throws LockedException, NotReadyException, RobustEstimatorException {
284 if (isLocked()) {
285 throw new LockedException();
286 }
287 if (!isReady()) {
288 throw new NotReadyException();
289 }
290
291 final var innerEstimator = new PROSACRobustEstimator<ImageOfAbsoluteConic>(
292 new PROSACRobustEstimatorListener<>() {
293
294 // subset of homographies picked on each iteration
295 private final List<Transformation2D> subsetHomographies = new ArrayList<>();
296
297 @Override
298 public double getThreshold() {
299 return threshold;
300 }
301
302 @Override
303 public int getTotalSamples() {
304 return homographies.size();
305 }
306
307 @Override
308 public int getSubsetSize() {
309 return iacEstimator.getMinNumberOfRequiredHomographies();
310 }
311
312 @Override
313 public void estimatePreliminarSolutions(
314 final int[] samplesIndices, final List<ImageOfAbsoluteConic> solutions) {
315 subsetHomographies.clear();
316 for (final var samplesIndex : samplesIndices) {
317 subsetHomographies.add(homographies.get(samplesIndex));
318 }
319
320 try {
321 iacEstimator.setLMSESolutionAllowed(false);
322 iacEstimator.setHomographies(subsetHomographies);
323
324 final var iac = iacEstimator.estimate();
325 solutions.add(iac);
326 } catch (final Exception e) {
327 // if anything fails, no solution is added
328 }
329 }
330
331 @Override
332 public double computeResidual(final ImageOfAbsoluteConic currentEstimation, final int i) {
333 return residual(currentEstimation, homographies.get(i));
334 }
335
336 @Override
337 public boolean isReady() {
338 return PROSACImageOfAbsoluteConicRobustEstimator.this.isReady();
339 }
340
341 @Override
342 public void onEstimateStart(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
343 if (listener != null) {
344 listener.onEstimateStart(PROSACImageOfAbsoluteConicRobustEstimator.this);
345 }
346 }
347
348 @Override
349 public void onEstimateEnd(final RobustEstimator<ImageOfAbsoluteConic> estimator) {
350 if (listener != null) {
351 listener.onEstimateEnd(PROSACImageOfAbsoluteConicRobustEstimator.this);
352 }
353 }
354
355 @Override
356 public void onEstimateNextIteration(
357 final RobustEstimator<ImageOfAbsoluteConic> estimator, final int iteration) {
358 if (listener != null) {
359 listener.onEstimateNextIteration(
360 PROSACImageOfAbsoluteConicRobustEstimator.this, iteration);
361 }
362 }
363
364 @Override
365 public void onEstimateProgressChange(
366 final RobustEstimator<ImageOfAbsoluteConic> estimator, final float progress) {
367 if (listener != null) {
368 listener.onEstimateProgressChange(
369 PROSACImageOfAbsoluteConicRobustEstimator.this, progress);
370 }
371 }
372
373 @Override
374 public double[] getQualityScores() {
375 return qualityScores;
376 }
377 });
378
379 try {
380 locked = true;
381 innerEstimator.setConfidence(confidence);
382 innerEstimator.setMaxIterations(maxIterations);
383 innerEstimator.setProgressDelta(progressDelta);
384 return innerEstimator.estimate();
385 } catch (final com.irurueta.numerical.LockedException e) {
386 throw new LockedException(e);
387 } catch (final com.irurueta.numerical.NotReadyException e) {
388 throw new NotReadyException(e);
389 } finally {
390 locked = false;
391 }
392 }
393
394 /**
395 * Returns method being used for robust estimation
396 *
397 * @return method being used for robust estimation
398 */
399 @Override
400 public RobustEstimatorMethod getMethod() {
401 return RobustEstimatorMethod.PROSAC;
402 }
403
404 /**
405 * Sets quality scores corresponding to each homography.
406 * This method is used internally and does not check whether instance is
407 * locked or not
408 *
409 * @param qualityScores quality scores to be set
410 * @throws IllegalArgumentException if provided quality scores length is
411 * smaller than the minimum number of required homographies for current
412 * settings.
413 */
414 private void internalSetQualityScores(final double[] qualityScores) {
415 if (qualityScores.length < iacEstimator.getMinNumberOfRequiredHomographies()) {
416 throw new IllegalArgumentException();
417 }
418 this.qualityScores = qualityScores;
419 }
420 }