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.RadialDistortion;
19 import com.irurueta.geometry.CoordinatesType;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.estimators.LockedException;
22 import com.irurueta.geometry.estimators.NotReadyException;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimator;
24 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.ArrayList;
30 import java.util.List;
31 import java.util.logging.Level;
32 import java.util.logging.Logger;
33
34 /**
35 * Finds the best radial distortion for provided collections of 2D points using
36 * PROMedS algorithm.
37 */
38 public class PROMedSRadialDistortionRobustEstimator extends RadialDistortionRobustEstimator {
39
40 /**
41 * Default value to be used for stop threshold. Stop threshold can be used
42 * to keep the algorithm iterating in case that best estimated threshold
43 * using median of residuals is not small enough. Once a solution is found
44 * that generates a threshold below this value, the algorithm will stop.
45 * The stop threshold can be used to prevent the LMedS algorithm iterating
46 * too many times in cases where samples have a very similar accuracy.
47 * For instance, in cases where proportion of outliers is very small (close
48 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
49 * iterate for a long time trying to find the best solution when indeed
50 * there is no need to do that if a reasonable threshold has already been
51 * reached.
52 * Because of this behaviour the stop threshold can be set to a value much
53 * lower than the one typically used in RANSAC, and yet the algorithm could
54 * still produce even smaller thresholds in estimated results.
55 */
56 public static final double DEFAULT_STOP_THRESHOLD = 1e-3;
57
58 /**
59 * Minimum allowed stop threshold value.
60 */
61 public static final double MIN_STOP_THRESHOLD = 0.0;
62
63 /**
64 * Threshold to be used to keep the algorithm iterating in case that best
65 * estimated threshold using median of residuals is not small enough. Once
66 * a solution is found that generates a threshold below this value, the
67 * algorithm will stop.
68 * The stop threshold can be used to prevent the LMedS algorithm iterating
69 * too many times in cases where samples have a very similar accuracy.
70 * For instance, in cases where proportion of outliers is very small (close
71 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
72 * iterate for a long time trying to find the best solution when indeed
73 * there is no need to do that if a reasonable threshold has already been
74 * reached.
75 * Because of this behaviour the stop threshold can be set to a value much
76 * lower than the one typically used in RANSAC, and yet the algorithm could
77 * still produce even smaller thresholds in estimated results.
78 */
79 private double stopThreshold;
80
81 /**
82 * Quality scores corresponding to each provided point.
83 * The larger the score value the better the quality of the sample.
84 */
85 private double[] qualityScores;
86
87 /**
88 * Constructor.
89 */
90 public PROMedSRadialDistortionRobustEstimator() {
91 super();
92 stopThreshold = DEFAULT_STOP_THRESHOLD;
93 }
94
95 /**
96 * Constructor.
97 *
98 * @param listener listener to be notified of events such as when
99 * estimation starts, ends or its progress significantly changes.
100 */
101 public PROMedSRadialDistortionRobustEstimator(final RadialDistortionRobustEstimatorListener listener) {
102 super(listener);
103 stopThreshold = DEFAULT_STOP_THRESHOLD;
104 }
105
106 /**
107 * Constructor.
108 *
109 * @param distortedPoints list of distorted points. Distorted points are
110 * obtained after radial distortion is applied to an undistorted point.
111 * @param undistortedPoints list of undistorted points.
112 * @throws IllegalArgumentException if provided lists of points don't have
113 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
114 */
115 public PROMedSRadialDistortionRobustEstimator(
116 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints) {
117 super(distortedPoints, undistortedPoints);
118 stopThreshold = DEFAULT_STOP_THRESHOLD;
119 }
120
121 /**
122 * Constructor.
123 *
124 * @param distortedPoints list of distorted points. Distorted points are
125 * obtained after radial distortion is applied to an undistorted point.
126 * @param undistortedPoints list of undistorted points.
127 * @param listener listener to be notified of events such as when
128 * estimation starts, ends or its progress significantly changes.
129 * @throws IllegalArgumentException if provided lists of points don't have
130 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
131 */
132 public PROMedSRadialDistortionRobustEstimator(
133 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints,
134 final RadialDistortionRobustEstimatorListener listener) {
135 super(distortedPoints, undistortedPoints, listener);
136 stopThreshold = DEFAULT_STOP_THRESHOLD;
137 }
138
139 /**
140 * Constructor.
141 *
142 * @param distortedPoints list of distorted points. Distorted points are
143 * obtained after radial distortion is applied to an undistorted point.
144 * @param undistortedPoints list of undistorted points.
145 * @param distortionCenter radial distortion center. If null it is assumed
146 * to be the origin of coordinates, otherwise this is typically equal to
147 * the camera principal point.
148 * @throws IllegalArgumentException if provided lists of points don't have
149 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
150 */
151 public PROMedSRadialDistortionRobustEstimator(
152 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints,
153 final Point2D distortionCenter) {
154 super(distortedPoints, undistortedPoints, distortionCenter);
155 stopThreshold = DEFAULT_STOP_THRESHOLD;
156 }
157
158 /**
159 * Constructor.
160 *
161 * @param distortedPoints list of distorted points. Distorted points are
162 * obtained after radial distortion is applied to an undistorted point.
163 * @param undistortedPoints list of undistorted points.
164 * @param distortionCenter radial distortion center. If null it is assumed
165 * to be the origin of coordinates, otherwise this is typically equal to
166 * the camera principal point.
167 * @param listener listener to be notified of events such as when
168 * estimation starts, ends or its progress significantly changes.
169 * @throws IllegalArgumentException if provided lists of points don't have
170 * the same size or their size is smaller than MIN_NUMBER_OF_POINTS.
171 */
172 public PROMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
173 final List<Point2D> undistortedPoints,
174 final Point2D distortionCenter,
175 final RadialDistortionRobustEstimatorListener listener) {
176 super(distortedPoints, undistortedPoints, distortionCenter, listener);
177 stopThreshold = DEFAULT_STOP_THRESHOLD;
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param qualityScores quality scores corresponding to each provided point.
184 * @throws IllegalArgumentException if provided quality scores length is
185 * smaller than required size (i.e. 2 points).
186 */
187 public PROMedSRadialDistortionRobustEstimator(final double[] qualityScores) {
188 this();
189 internalSetQualityScores(qualityScores);
190 }
191
192 /**
193 * Constructor.
194 *
195 * @param qualityScores quality scores corresponding to each provided point.
196 * @param listener listener to be notified of events such as when
197 * estimation starts, ends or its progress significantly changes.
198 * @throws IllegalArgumentException if provided quality scores length is
199 * smaller than required size (i.e. 2 points).
200 */
201 public PROMedSRadialDistortionRobustEstimator(
202 final double[] qualityScores, final RadialDistortionRobustEstimatorListener listener) {
203 this(listener);
204 internalSetQualityScores(qualityScores);
205 }
206
207 /**
208 * Constructor.
209 *
210 * @param distortedPoints list of distorted points. Distorted points are
211 * obtained after radial distortion is applied to an undistorted point.
212 * @param undistortedPoints list of undistorted points.
213 * @param qualityScores quality scores corresponding to each provided point.
214 * @throws IllegalArgumentException if provided lists of points and quality
215 * scores don't have the same size or their size is smaller than
216 * MIN_NUMBER_OF_POINTS (i.e. 2 points).
217 */
218 public PROMedSRadialDistortionRobustEstimator(
219 final List<Point2D> distortedPoints, final List<Point2D> undistortedPoints, final double[] qualityScores) {
220 this(distortedPoints, undistortedPoints);
221 internalSetQualityScores(qualityScores);
222 }
223
224 /**
225 * Constructor.
226 *
227 * @param distortedPoints list of distorted points. Distorted points are
228 * obtained after radial distortion is applied to an undistorted point.
229 * @param undistortedPoints list of undistorted points.
230 * @param qualityScores quality scores corresponding to each provided point.
231 * @param listener listener to be notified of events such as when
232 * estimation starts, ends or its progress significantly changes.
233 * @throws IllegalArgumentException if provided lists of points or quality
234 * scores don't have the same size or their size is smaller than
235 * MIN_NUMBER_OF_POINTS (i.e. 2 points).
236 */
237 public PROMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
238 final List<Point2D> undistortedPoints,
239 final double[] qualityScores,
240 final RadialDistortionRobustEstimatorListener listener) {
241 this(distortedPoints, undistortedPoints, listener);
242 internalSetQualityScores(qualityScores);
243 }
244
245 /**
246 * Constructor.
247 *
248 * @param distortedPoints list of distorted points. Distorted points are
249 * obtained after radial distortion is applied to an undistorted point.
250 * @param undistortedPoints list of undistorted points.
251 * @param qualityScores quality scores corresponding to each provided point.
252 * @param distortionCenter radial distortion center. If null it is assumed
253 * to be the origin of coordinates, otherwise this is typically equal to
254 * the camera principal point.
255 * @throws IllegalArgumentException if provided lists of points or quality
256 * scores don't have the same size or their size is smaller than
257 * MIN_NUMBER_OF_POINTS (i.e. 2 points).
258 */
259 public PROMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
260 final List<Point2D> undistortedPoints,
261 final double[] qualityScores,
262 final Point2D distortionCenter) {
263 this(distortedPoints, undistortedPoints, distortionCenter);
264 internalSetQualityScores(qualityScores);
265 }
266
267 /**
268 * Constructor.
269 *
270 * @param distortedPoints list of distorted points. Distorted points are
271 * obtained after radial distortion is applied to an undistorted point.
272 * @param undistortedPoints list of undistorted points.
273 * @param qualityScores quality scores corresponding to each provided point.
274 * @param distortionCenter radial distortion center. If null it is assumed
275 * to be the origin of coordinates, otherwise this is typically equal to
276 * the camera principal point.
277 * @param listener listener to be notified of events such as when
278 * estimation starts, ends or its progress significantly changes.
279 * @throws IllegalArgumentException if provided lists of points or quality
280 * scores don't have the same size or their size is smaller than
281 * MIN_NUMBER_OF_POINTS (i.e. 2 points).
282 */
283 public PROMedSRadialDistortionRobustEstimator(final List<Point2D> distortedPoints,
284 final List<Point2D> undistortedPoints,
285 final double[] qualityScores,
286 final Point2D distortionCenter,
287 final RadialDistortionRobustEstimatorListener listener) {
288 this(distortedPoints, undistortedPoints, distortionCenter, listener);
289 internalSetQualityScores(qualityScores);
290 }
291
292 /**
293 * Returns threshold to be used to keep the algorithm iterating in case that
294 * best estimated threshold using median of residuals is not small enough.
295 * Once a solution is found that generates a threshold below this value, the
296 * algorithm will stop.
297 * The stop threshold can be used to prevent the LMedS algorithm iterating
298 * too many times in cases where samples have a very similar accuracy.
299 * For instance, in cases where proportion of outliers is very small (close
300 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
301 * iterate for a long time trying to find the best solution when indeed
302 * there is no need to do that if a reasonable threshold has already been
303 * reached.
304 * Because of this behaviour the stop threshold can be set to a value much
305 * lower than the one typically used in RANSAC, and yet the algorithm could
306 * still produce even smaller thresholds in estimated results.
307 *
308 * @return stop threshold to stop the algorithm prematurely when a certain
309 * accuracy has been reached.
310 */
311 public double getStopThreshold() {
312 return stopThreshold;
313 }
314
315 /**
316 * Sets threshold to be used to keep the algorithm iterating in case that
317 * best estimated threshold using median of residuals is not small enough.
318 * Once a solution is found that generates a threshold below this value, the
319 * algorithm will stop.
320 * The stop threshold can be used to prevent the LMedS algorithm iterating
321 * too many times in cases where samples have a very similar accuracy.
322 * For instance, in cases where proportion of outliers is very small (close
323 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
324 * iterate for a long time trying to find the best solution when indeed
325 * there is no need to do that if a reasonable threshold has already been
326 * reached.
327 * Because of this behaviour the stop threshold can be set to a value much
328 * lower than the one typically used in RANSAC, and yet the algorithm could
329 * still produce even smaller thresholds in estimated results.
330 *
331 * @param stopThreshold stop threshold to stop the algorithm prematurely
332 * when a certain accuracy has been reached.
333 * @throws IllegalArgumentException if provided value is zero or negative.
334 * @throws LockedException if robust estimator is locked because an
335 * estimation is already in progress.
336 */
337 public void setStopThreshold(final double stopThreshold) throws LockedException {
338 if (isLocked()) {
339 throw new LockedException();
340 }
341 if (stopThreshold <= MIN_STOP_THRESHOLD) {
342 throw new IllegalArgumentException();
343 }
344
345 this.stopThreshold = stopThreshold;
346 }
347
348 /**
349 * Returns quality scores corresponding to each provided point.
350 * The larger the score value the better the quality of the sampled point.
351 *
352 * @return quality scores corresponding to each point.
353 */
354 @Override
355 public double[] getQualityScores() {
356 return qualityScores;
357 }
358
359 /**
360 * Sets quality scores corresponding to each provided point.
361 * The larger the score value the better the quality of the sampled point.
362 *
363 * @param qualityScores quality scores corresponding to each point.
364 * @throws LockedException if robust estimator is locked because an
365 * estimation is already in progress.
366 * @throws IllegalArgumentException if provided quality scores length is
367 * smaller than MINIMUM_SIZE (i.e. 3 samples).
368 */
369 @Override
370 public void setQualityScores(final double[] qualityScores) throws LockedException {
371 if (isLocked()) {
372 throw new LockedException();
373 }
374 internalSetQualityScores(qualityScores);
375 }
376
377 /**
378 * Indicates if estimator is ready to start the radial distortion
379 * estimation.
380 * This is true when input data (i.e. 2D points and quality scores) are
381 * provided and a minimum of 2 points are available.
382 *
383 * @return true if estimator is ready, false otherwise.
384 */
385 @Override
386 public boolean isReady() {
387 return super.isReady() && qualityScores != null && qualityScores.length == distortedPoints.size();
388 }
389
390 /**
391 * Estimates a radial distortion using a robust estimator and
392 * the best set of matched 2D points found using the robust estimator.
393 *
394 * @return a radial distortion.
395 * @throws LockedException if robust estimator is locked because an
396 * estimation is already in progress.
397 * @throws NotReadyException if provided input data is not enough to start
398 * the estimation.
399 * @throws RobustEstimatorException if estimation fails for any reason
400 * (i.e. numerical instability, no solution available, etc).
401 */
402 @SuppressWarnings("DuplicatedCode")
403 @Override
404 public RadialDistortion estimate() throws LockedException, NotReadyException, RobustEstimatorException {
405 if (isLocked()) {
406 throw new LockedException();
407 }
408 if (!isReady()) {
409 throw new NotReadyException();
410 }
411
412 final var innerEstimator = new PROMedSRobustEstimator<RadialDistortion>(new PROMedSRobustEstimatorListener<>() {
413
414 // point to be reused when computing residuals
415 private final Point2D testPoint = Point2D.create(CoordinatesType.INHOMOGENEOUS_COORDINATES);
416
417 // non-robust radial distortion estimator
418 private final LMSERadialDistortionEstimator radialDistortionEstimator = new LMSERadialDistortionEstimator();
419
420 // subset of distorted (i.e. measured) points
421 private final List<Point2D> subsetDistorted = new ArrayList<>();
422
423 // subset of undistorted (i.e. ideal) points
424 private final List<Point2D> subsetUndistorted = new ArrayList<>();
425
426 @Override
427 public double getThreshold() {
428 return stopThreshold;
429 }
430
431 @Override
432 public int getTotalSamples() {
433 return distortedPoints.size();
434 }
435
436 @Override
437 public int getSubsetSize() {
438 return RadialDistortionRobustEstimator.MIN_NUMBER_OF_POINTS;
439 }
440
441 @Override
442 public void estimatePreliminarSolutions(
443 final int[] samplesIndices, final List<RadialDistortion> solutions) {
444 subsetDistorted.clear();
445 subsetDistorted.add(distortedPoints.get(samplesIndices[0]));
446 subsetDistorted.add(distortedPoints.get(samplesIndices[1]));
447
448 subsetUndistorted.clear();
449 subsetUndistorted.add(undistortedPoints.get(samplesIndices[0]));
450 subsetUndistorted.add(undistortedPoints.get(samplesIndices[1]));
451
452 try {
453 radialDistortionEstimator.setPoints(distortedPoints, undistortedPoints);
454 radialDistortionEstimator.setPoints(subsetDistorted, subsetUndistorted);
455
456 final var distortion = radialDistortionEstimator.estimate();
457 solutions.add(distortion);
458 } catch (final Exception e) {
459 // if anything fails, no solution is added
460 }
461 }
462
463 @Override
464 public double computeResidual(final RadialDistortion currentEstimation, final int i) {
465 final var distortedPoint = distortedPoints.get(i);
466 final var undistortedPoint = undistortedPoints.get(i);
467
468 currentEstimation.distort(undistortedPoint, testPoint);
469
470 return testPoint.distanceTo(distortedPoint);
471 }
472
473 @Override
474 public boolean isReady() {
475 return PROMedSRadialDistortionRobustEstimator.this.isReady();
476 }
477
478 @Override
479 public void onEstimateStart(final RobustEstimator<RadialDistortion> estimator) {
480 try {
481 radialDistortionEstimator.setLMSESolutionAllowed(false);
482 radialDistortionEstimator.setIntrinsic(getIntrinsic());
483 } catch (final Exception e) {
484 Logger.getLogger(PROMedSRadialDistortionRobustEstimator.class.getName()).log(Level.WARNING,
485 "Could not set intrinsic parameters on radial distortion estimator", e);
486 }
487
488 if (listener != null) {
489 listener.onEstimateStart(PROMedSRadialDistortionRobustEstimator.this);
490 }
491 }
492
493 @Override
494 public void onEstimateEnd(final RobustEstimator<RadialDistortion> estimator) {
495 if (listener != null) {
496 listener.onEstimateEnd(PROMedSRadialDistortionRobustEstimator.this);
497 }
498 }
499
500 @Override
501 public void onEstimateNextIteration(
502 final RobustEstimator<RadialDistortion> estimator, final int iteration) {
503 if (listener != null) {
504 listener.onEstimateNextIteration(PROMedSRadialDistortionRobustEstimator.this, iteration);
505 }
506 }
507
508 @Override
509 public void onEstimateProgressChange(
510 final RobustEstimator<RadialDistortion> estimator, final float progress) {
511 if (listener != null) {
512 listener.onEstimateProgressChange(PROMedSRadialDistortionRobustEstimator.this, progress);
513 }
514 }
515
516 @Override
517 public double[] getQualityScores() {
518 return qualityScores;
519 }
520 });
521
522 try {
523 locked = true;
524 innerEstimator.setConfidence(confidence);
525 innerEstimator.setMaxIterations(maxIterations);
526 innerEstimator.setProgressDelta(progressDelta);
527 return innerEstimator.estimate();
528 } catch (final com.irurueta.numerical.LockedException e) {
529 throw new LockedException(e);
530 } catch (final com.irurueta.numerical.NotReadyException e) {
531 throw new NotReadyException(e);
532 } finally {
533 locked = false;
534 }
535 }
536
537 /**
538 * Returns method being used for robust estimation
539 *
540 * @return method being used for robust estimation
541 */
542 @Override
543 public RobustEstimatorMethod getMethod() {
544 return RobustEstimatorMethod.PROMEDS;
545 }
546
547 /**
548 * Sets quality scores corresponding to each provided point.
549 * This method is used internally and does not check whether instance is
550 * locked or not.
551 *
552 * @param qualityScores quality scores to be set.
553 * @throws IllegalArgumentException if provided quality scores length is
554 * smaller than MINIMUM_SIZE.
555 */
556 private void internalSetQualityScores(final double[] qualityScores) {
557 if (qualityScores.length < MIN_NUMBER_OF_POINTS) {
558 throw new IllegalArgumentException();
559 }
560
561 this.qualityScores = qualityScores;
562 }
563 }