1 /*
2 * Copyright (C) 2017 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.geometry.estimators;
17
18 import com.irurueta.geometry.CoordinatesType;
19 import com.irurueta.geometry.PinholeCamera;
20 import com.irurueta.geometry.Point2D;
21 import com.irurueta.geometry.Point3D;
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 pinhole camera for provided collections of matched 2D/3D
33 * points using PROMedS + UPnP algorithms.
34 */
35 @SuppressWarnings("DuplicatedCode")
36 public class PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator extends
37 UPnPPointCorrespondencePinholeCameraRobustEstimator {
38
39 /**
40 * Default value to be used for stop threshold. Stop threshold can be used
41 * to keep the algorithm iterating in case that best estimated threshold
42 * using median of residuals is not small enough. Once a solution is found
43 * that generates a threshold below this value, the algorithm will stop.
44 * The stop threshold can be used to prevent the LMedS algorithm iterating
45 * too many times in cases where samples have a very similar accuracy.
46 * For instance, in cases where proportion of outliers is very small (close
47 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
48 * iterate for a long time trying to find the best solution when indeed
49 * there is no need to do that if a reasonable threshold has already been
50 * reached.
51 * Because of this behaviour the stop threshold can be set to a value much
52 * lower than the one typically used in RANSAC, and yet the algorithm could
53 * still produce even smaller thresholds in estimated results.
54 */
55 public static final double DEFAULT_STOP_THRESHOLD = 1.0;
56
57 /**
58 * Minimum allowed stop threshold value.
59 */
60 public static final double MIN_STOP_THRESHOLD = 0.0;
61
62 /**
63 * Threshold to be used to keep the algorithm iterating in case that best
64 * estimated threshold using median of residuals is not small enough. Once
65 * a solution is found that generates a threshold below this value, the
66 * algorithm will stop.
67 * The stop threshold can be used to prevent the LMedS algorithm iterating
68 * too many times in cases where samples have a very similar accuracy.
69 * For instance, in cases where proportion of outliers is very small (close
70 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
71 * iterate for a long time trying to find the best solution when indeed
72 * there is no need to do that if a reasonable threshold has already been
73 * reached.
74 * Because of this behaviour the stop threshold can be set to a value much
75 * lower than the one typically used in RANSAC, and yet the algorithm could
76 * still produce even smaller thresholds in estimated results.
77 */
78 private double stopThreshold;
79
80 /**
81 * Quality scores corresponding to each pair of matched points.
82 * The larger the score value the better the quality of the matching.
83 */
84 private double[] qualityScores;
85
86 /**
87 * Constructor.
88 */
89 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator() {
90 super();
91 stopThreshold = DEFAULT_STOP_THRESHOLD;
92 }
93
94 /**
95 * Constructor with lists of points to be used to estimate a pinhole camera.
96 * Points in the list located at the same position are considered to be
97 * matched. Hence, both lists must have the same size, and their size must
98 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
99 *
100 * @param points3D list of 3D points used to estimate a pinhole camera.
101 * @param points2D list of corresponding projected 2D points used to
102 * estimate a pinhole camera.
103 * @throws IllegalArgumentException if provided lists of points don't have
104 * the same size or their size is smaller than required minimum size
105 * (6 correspondences).
106 */
107 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
108 final List<Point3D> points3D, final List<Point2D> points2D) {
109 super(points3D, points2D);
110 stopThreshold = DEFAULT_STOP_THRESHOLD;
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param listener listener to be notified of events such as when estimation
117 * starts, ends or its progress significantly changes.
118 */
119 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
120 final PinholeCameraRobustEstimatorListener listener) {
121 super(listener);
122 stopThreshold = DEFAULT_STOP_THRESHOLD;
123 }
124
125 /**
126 * Constructor with listener and lists of points to be used ot estimate a
127 * pinhole camera.
128 * Points in the list located at the same position are considered to be
129 * matched. Hence, both lists must have the same size, and their size must
130 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
131 *
132 * @param listener listener to be notified of events such as when estimation
133 * starts, ends or its progress significantly changes.
134 * @param points3D list of 3D points used to estimate a pinhole camera.
135 * @param points2D list of corresponding projected 2D points used to
136 * estimate a pinhole camera.
137 * @throws IllegalArgumentException if provided lists of points don't have
138 * the same size or their size is smaller than required minimum size
139 * (6 correspondences).
140 */
141 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
142 final PinholeCameraRobustEstimatorListener listener,
143 final List<Point3D> points3D, final List<Point2D> points2D) {
144 super(listener, points3D, points2D);
145 stopThreshold = DEFAULT_STOP_THRESHOLD;
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param qualityScores quality scores corresponding to each pair of matched
152 * points.
153 * @throws IllegalArgumentException if provided quality scores length is
154 * smaller than MINIMUM_SIZE (i.e. 3 samples).
155 */
156 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(final double[] qualityScores) {
157 super();
158 stopThreshold = DEFAULT_STOP_THRESHOLD;
159 internalSetQualityScores(qualityScores);
160 }
161
162 /**
163 * Constructor with lists of points to be used to estimate a pinhole camera.
164 * Points in the list located at the same position are considered to be
165 * matched. Hence, both lists must have the same size, and their size must
166 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
167 *
168 * @param points3D list of 3D points used to estimate a pinhole camera.
169 * @param points2D list of corresponding projected 2D points used to
170 * estimate a pinhole camera.
171 * @param qualityScores quality scores corresponding to each pair of matched
172 * points.
173 * @throws IllegalArgumentException if provided lists of points and array
174 * of quality scores don't have the same size or their size is smaller than
175 * 6 correspondences.
176 */
177 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
178 final List<Point3D> points3D, final List<Point2D> points2D, final double[] qualityScores) {
179 super(points3D, points2D);
180
181 if (qualityScores.length != points3D.size()) {
182 throw new IllegalArgumentException();
183 }
184
185 stopThreshold = DEFAULT_STOP_THRESHOLD;
186 internalSetQualityScores(qualityScores);
187 }
188
189 /**
190 * Constructor.
191 *
192 * @param listener listener to be notified of events such as when estimation
193 * starts, ends or its progress significantly changes.
194 * @param qualityScores quality scores corresponding to each pair of matched
195 * points.
196 * @throws IllegalArgumentException if provided quality scores length is
197 * smaller than MINIMUM_SIZE (i.e. 3 samples).
198 */
199 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
200 final PinholeCameraRobustEstimatorListener listener, final double[] qualityScores) {
201 super(listener);
202 stopThreshold = DEFAULT_STOP_THRESHOLD;
203 internalSetQualityScores(qualityScores);
204 }
205
206 /**
207 * Constructor with listener and lists of points to be used ot estimate a
208 * pinhole camera.
209 * Points in the list located at the same position are considered to be
210 * matched. Hence, both lists must have the same size, and their size must
211 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
212 *
213 * @param listener listener to be notified of events such as when estimation
214 * starts, ends or its progress significantly changes.
215 * @param points3D list of 3D points used to estimate a pinhole camera.
216 * @param points2D list of corresponding projected 2D points used to
217 * estimate a pinhole camera.
218 * @param qualityScores quality scores corresponding to each pair of matched
219 * points.
220 * @throws IllegalArgumentException if provided lists of points don't have
221 * the same size or their size is smaller than
222 * MIN_NUMBER_OF_POINT_CORRESPONDENCES.
223 */
224 public PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator(
225 final PinholeCameraRobustEstimatorListener listener,
226 final List<Point3D> points3D, final List<Point2D> points2D, final double[] qualityScores) {
227 super(listener, points3D, points2D);
228
229 if (qualityScores.length != points3D.size()) {
230 throw new IllegalArgumentException();
231 }
232
233 stopThreshold = DEFAULT_STOP_THRESHOLD;
234 internalSetQualityScores(qualityScores);
235 }
236
237 /**
238 * Returns threshold to be used to keep the algorithm iterating in case that
239 * best estimated threshold using median of residuals is not small enough.
240 * Once a solution is found that generates a threshold below this value, the
241 * algorithm will stop.
242 * As in LMedS, the stop threshold can be used to prevent the PROMedS
243 * algorithm iterating too many times in cases where samples have a very
244 * similar accuracy.
245 * For instance, in cases where proportion of outliers is very small (close
246 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
247 * iterate for a long time trying to find the best solution when indeed
248 * there is no need to do that if a reasonable threshold has already been
249 * reached.
250 * Because of this behaviour the stop threshold can be set to a value much
251 * lower than the one typically used in RANSAC, and yet the algorithm could
252 * still produce even smaller thresholds in estimated results.
253 *
254 * @return stop threshold to stop the algorithm prematurely when a certain
255 * accuracy has been reached.
256 */
257 public double getStopThreshold() {
258 return stopThreshold;
259 }
260
261 /**
262 * Sets threshold to be used to keep the algorithm iterating in case that
263 * best estimated threshold using median of residuals is not small enough.
264 * Once a solution is found that generates a threshold below this value, the
265 * algorithm will stop.
266 * As in LMedS, the stop threshold can be used to prevent the PROMedS
267 * algorithm iterating too many times in cases where samples have a very
268 * similar accuracy.
269 * For instance, in cases where proportion of outliers is very small (close
270 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
271 * iterate for a long time trying to find the best solution when indeed
272 * there is no need to do that if a reasonable threshold has already been
273 * reached.
274 * Because of this behaviour the stop threshold can be set to a value much
275 * lower than the one typically used in RANSAC, and yet the algorithm could
276 * still produce even smaller thresholds in estimated results.
277 *
278 * @param stopThreshold stop threshold to stop the algorithm prematurely
279 * when a certain accuracy has been reached.
280 * @throws IllegalArgumentException if provided value is zero or negative.
281 * @throws LockedException if robust estimator is locked because an
282 * estimation is already in progress.
283 */
284 public void setStopThreshold(final double stopThreshold) throws LockedException {
285 if (isLocked()) {
286 throw new LockedException();
287 }
288 if (stopThreshold <= MIN_STOP_THRESHOLD) {
289 throw new IllegalArgumentException();
290 }
291
292 this.stopThreshold = stopThreshold;
293 }
294
295 /**
296 * Returns quality scores corresponding to each pair of matched points.
297 * The larger the score value the better the quality of the matching.
298 *
299 * @return quality scores corresponding to each pair of matched points.
300 */
301 @Override
302 public double[] getQualityScores() {
303 return qualityScores;
304 }
305
306 /**
307 * Sets quality scores corresponding to each pair of matched points.
308 * The larger the score value the better the quality of the matching.
309 *
310 * @param qualityScores quality scores corresponding to each pair of matched
311 * points.
312 * @throws LockedException if robust estimator is locked because an
313 * estimation is already in progress.
314 * @throws IllegalArgumentException if provided quality scores length is
315 * smaller than MINIMUM_SIZE (i.e. 6 samples).
316 */
317 @Override
318 public void setQualityScores(final double[] qualityScores) throws LockedException {
319 if (isLocked()) {
320 throw new LockedException();
321 }
322 internalSetQualityScores(qualityScores);
323 }
324
325 /**
326 * Indicates if estimator is ready to start the affine 2D transformation
327 * estimation.
328 * This is true when input data (i.e. lists of matched points and quality
329 * scores) are provided and a minimum of MINIMUM_SIZE points are available.
330 *
331 * @return true if estimator is ready, false otherwise.
332 */
333 @Override
334 public boolean isReady() {
335 return super.isReady() && qualityScores != null && qualityScores.length == points3D.size();
336 }
337
338 /**
339 * Estimates an affine 2D transformation using a robust estimator and
340 * the best set of matched 2D point correspondences found using the robust
341 * estimator.
342 *
343 * @return an affine 2D transformation.
344 * @throws LockedException if robust estimator is locked because an
345 * estimation is already in progress.
346 * @throws NotReadyException if provided input data is not enough to start
347 * the estimation.
348 * @throws RobustEstimatorException if estimation fails for any reason
349 * (i.e. numerical instability, no solution available, etc).
350 */
351 @Override
352 public PinholeCamera estimate() throws LockedException, NotReadyException, RobustEstimatorException {
353 if (isLocked()) {
354 throw new LockedException();
355 }
356 if (!isReady()) {
357 throw new NotReadyException();
358 }
359
360 // pinhole camera estimator using UPnP (Uncalibrated Perspective-n-Point) algorithm
361 final var nonRobustEstimator = new UPnPPointCorrespondencePinholeCameraEstimator();
362
363 nonRobustEstimator.setPlanarConfigurationAllowed(planarConfigurationAllowed);
364 nonRobustEstimator.setNullspaceDimension2Allowed(nullspaceDimension2Allowed);
365 nonRobustEstimator.setPlanarThreshold(planarThreshold);
366 nonRobustEstimator.setSkewness(skewness);
367 nonRobustEstimator.setHorizontalPrincipalPoint(horizontalPrincipalPoint);
368 nonRobustEstimator.setVerticalPrincipalPoint(verticalPrincipalPoint);
369
370 // suggestions
371 nonRobustEstimator.setSuggestSkewnessValueEnabled(isSuggestSkewnessValueEnabled());
372 nonRobustEstimator.setSuggestedSkewnessValue(getSuggestedSkewnessValue());
373 nonRobustEstimator.setSuggestHorizontalFocalLengthEnabled(isSuggestHorizontalFocalLengthEnabled());
374 nonRobustEstimator.setSuggestedHorizontalFocalLengthValue(getSuggestedHorizontalFocalLengthValue());
375 nonRobustEstimator.setSuggestVerticalFocalLengthEnabled(isSuggestVerticalFocalLengthEnabled());
376 nonRobustEstimator.setSuggestedVerticalFocalLengthValue(getSuggestedVerticalFocalLengthValue());
377 nonRobustEstimator.setSuggestAspectRatioEnabled(isSuggestAspectRatioEnabled());
378 nonRobustEstimator.setSuggestedAspectRatioValue(getSuggestedAspectRatioValue());
379 nonRobustEstimator.setSuggestPrincipalPointEnabled(isSuggestPrincipalPointEnabled());
380 nonRobustEstimator.setSuggestedPrincipalPointValue(getSuggestedPrincipalPointValue());
381 nonRobustEstimator.setSuggestRotationEnabled(isSuggestRotationEnabled());
382 nonRobustEstimator.setSuggestedRotationValue(getSuggestedRotationValue());
383 nonRobustEstimator.setSuggestCenterEnabled(isSuggestCenterEnabled());
384 nonRobustEstimator.setSuggestedCenterValue(getSuggestedCenterValue());
385
386 final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PinholeCamera>() {
387
388 // point to be reused when computing residuals
389 private final Point2D testPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
390
391 // 3D points for a subset of samples
392 private final List<Point3D> subset3D = new ArrayList<>();
393
394 // 2D points for a subset of samples
395 private final List<Point2D> subset2D = new ArrayList<>();
396
397 @Override
398 public double getThreshold() {
399 return stopThreshold;
400 }
401
402 @Override
403 public int getTotalSamples() {
404 return points3D.size();
405 }
406
407 @Override
408 public int getSubsetSize() {
409 return PointCorrespondencePinholeCameraRobustEstimator.MIN_NUMBER_OF_POINT_CORRESPONDENCES;
410 }
411
412 @Override
413 public void estimatePreliminarSolutions(
414 final int[] samplesIndices, final List<PinholeCamera> solutions) {
415 subset3D.clear();
416 subset3D.add(points3D.get(samplesIndices[0]));
417 subset3D.add(points3D.get(samplesIndices[1]));
418 subset3D.add(points3D.get(samplesIndices[2]));
419 subset3D.add(points3D.get(samplesIndices[3]));
420 subset3D.add(points3D.get(samplesIndices[4]));
421 subset3D.add(points3D.get(samplesIndices[5]));
422
423 subset2D.clear();
424 subset2D.add(points2D.get(samplesIndices[0]));
425 subset2D.add(points2D.get(samplesIndices[1]));
426 subset2D.add(points2D.get(samplesIndices[2]));
427 subset2D.add(points2D.get(samplesIndices[3]));
428 subset2D.add(points2D.get(samplesIndices[4]));
429 subset2D.add(points2D.get(samplesIndices[5]));
430
431 try {
432 nonRobustEstimator.setLists(subset3D, subset2D);
433
434 final var cam = nonRobustEstimator.estimate();
435 solutions.add(cam);
436 } catch (final Exception e) {
437 // if points configuration is degenerate, no solution is
438 // added
439 }
440 }
441
442 @Override
443 public double computeResidual(final PinholeCamera currentEstimation, final int i) {
444 // pick i-th points
445 final var point3D = points3D.get(i);
446 final var point2D = points2D.get(i);
447
448 // project point3D into test point
449 currentEstimation.project(point3D, testPoint);
450
451 // compare test point and 2D point
452 return testPoint.distanceTo(point2D);
453 }
454
455 @Override
456 public boolean isReady() {
457 return PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator.this.isReady();
458 }
459
460 @Override
461 public void onEstimateStart(final RobustEstimator<PinholeCamera> estimator) {
462 if (listener != null) {
463 listener.onEstimateStart(
464 PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator.this);
465 }
466 }
467
468 @Override
469 public void onEstimateEnd(final RobustEstimator<PinholeCamera> estimator) {
470 if (listener != null) {
471 listener.onEstimateEnd(
472 PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator.this);
473 }
474 }
475
476 @Override
477 public void onEstimateNextIteration(
478 final RobustEstimator<PinholeCamera> estimator, final int iteration) {
479 if (listener != null) {
480 listener.onEstimateNextIteration(
481 PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator.this,
482 iteration);
483 }
484 }
485
486 @Override
487 public void onEstimateProgressChange(
488 final RobustEstimator<PinholeCamera> estimator, final float progress) {
489 if (listener != null) {
490 listener.onEstimateProgressChange(
491 PROMedSUPnPPointCorrespondencePinholeCameraRobustEstimator.this, progress);
492 }
493 }
494
495 @Override
496 public double[] getQualityScores() {
497 return qualityScores;
498 }
499 });
500
501 try {
502 locked = true;
503 inliersData = null;
504 innerEstimator.setConfidence(confidence);
505 innerEstimator.setMaxIterations(maxIterations);
506 innerEstimator.setProgressDelta(progressDelta);
507 final var result = innerEstimator.estimate();
508 inliersData = innerEstimator.getInliersData();
509 return attemptRefine(result, nonRobustEstimator.getMaxSuggestionWeight());
510 } catch (final com.irurueta.numerical.LockedException e) {
511 throw new LockedException(e);
512 } catch (final com.irurueta.numerical.NotReadyException e) {
513 throw new NotReadyException(e);
514 } finally {
515 locked = false;
516 }
517
518 }
519
520 /**
521 * Returns method being used for robust estimation.
522 *
523 * @return method being used for robust estimation.
524 */
525 @Override
526 public RobustEstimatorMethod getMethod() {
527 return RobustEstimatorMethod.PROMEDS;
528 }
529
530 /**
531 * Gets standard deviation used for Levenberg-Marquardt fitting during
532 * refinement.
533 * Returned value gives an indication of how much variance each residual
534 * has.
535 * Typically, this value is related to the threshold used on each robust
536 * estimation, since residuals of found inliers are within the range of
537 * such threshold.
538 *
539 * @return standard deviation used for refinement.
540 */
541 @Override
542 protected double getRefinementStandardDeviation() {
543 final var inliersData = (PROMedSRobustEstimator.PROMedSInliersData) getInliersData();
544 return inliersData.getEstimatedThreshold();
545 }
546
547 /**
548 * Sets quality scores corresponding to each pair of matched points.
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_POINT_CORRESPONDENCES) {
558 throw new IllegalArgumentException();
559 }
560
561 this.qualityScores = qualityScores;
562 }
563 }