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