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.PinholeCameraIntrinsicParameters;
21 import com.irurueta.geometry.Point2D;
22 import com.irurueta.geometry.Point3D;
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
32 /**
33 * Finds the best pinhole camera for provided collections of matched 2D/3D
34 * points using PROMedS + EPnP algorithms.
35 */
36 @SuppressWarnings("DuplicatedCode")
37 public class PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator extends
38 EPnPPointCorrespondencePinholeCameraRobustEstimator {
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 = 1.0;
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 pair of matched points.
83 * The larger the score value the better the quality of the matching.
84 */
85 private double[] qualityScores;
86
87 /**
88 * Constructor.
89 */
90 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator() {
91 super();
92 stopThreshold = DEFAULT_STOP_THRESHOLD;
93 }
94
95 /**
96 * Constructor with lists of points to be used to estimate a pinhole camera.
97 * Points in the list located at the same position are considered to be
98 * matched. Hence, both lists must have the same size, and their size must
99 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
100 *
101 * @param points3D list of 3D points used to estimate a pinhole camera.
102 * @param points2D list of corresponding projected 2D points used to
103 * estimate a pinhole camera.
104 * @throws IllegalArgumentException if provided lists of points don't have
105 * the same size or their size is smaller than required minimum size
106 * (6 correspondences).
107 */
108 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
109 final List<Point3D> points3D, final List<Point2D> points2D) {
110 super(points3D, points2D);
111 stopThreshold = DEFAULT_STOP_THRESHOLD;
112 }
113
114 /**
115 * Constructor.
116 *
117 * @param listener listener to be notified of events such as when estimation
118 * starts, ends or its progress significantly changes.
119 */
120 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
121 final PinholeCameraRobustEstimatorListener listener) {
122 super(listener);
123 stopThreshold = DEFAULT_STOP_THRESHOLD;
124 }
125
126 /**
127 * Constructor with listener and lists of points to be used ot estimate a
128 * pinhole camera.
129 * Points in the list located at the same position are considered to be
130 * matched. Hence, both lists must have the same size, and their size must
131 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
132 *
133 * @param listener listener to be notified of events such as when estimation
134 * starts, ends or its progress significantly changes.
135 * @param points3D list of 3D points used to estimate a pinhole camera.
136 * @param points2D list of corresponding projected 2D points used to
137 * estimate a pinhole camera.
138 * @throws IllegalArgumentException if provided lists of points don't have
139 * the same size or their size is smaller than required minimum size
140 * (6 correspondences).
141 */
142 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
143 final PinholeCameraRobustEstimatorListener listener, final List<Point3D> points3D,
144 final List<Point2D> points2D) {
145 super(listener, points3D, points2D);
146 stopThreshold = DEFAULT_STOP_THRESHOLD;
147 }
148
149 /**
150 * Constructor.
151 *
152 * @param qualityScores quality scores corresponding to each pair of matched
153 * points.
154 * @throws IllegalArgumentException if provided quality scores length is
155 * smaller than MINIMUM_SIZE (i.e. 3 samples).
156 */
157 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(final double[] qualityScores) {
158 super();
159 stopThreshold = DEFAULT_STOP_THRESHOLD;
160 internalSetQualityScores(qualityScores);
161 }
162
163 /**
164 * Constructor with lists of points to be used to estimate a pinhole camera.
165 * Points in the list located at the same position are considered to be
166 * matched. Hence, both lists must have the same size, and their size must
167 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
168 *
169 * @param points3D list of 3D points used to estimate a pinhole camera.
170 * @param points2D list of corresponding projected 2D points used to
171 * estimate a pinhole camera.
172 * @param qualityScores quality scores corresponding to each pair of matched
173 * points.
174 * @throws IllegalArgumentException if provided lists of points and array
175 * of quality scores don't have the same size or their size is smaller than
176 * 6 correspondences.
177 */
178 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
179 final List<Point3D> points3D, final List<Point2D> points2D, final double[] qualityScores) {
180 super(points3D, points2D);
181
182 if (qualityScores.length != points3D.size()) {
183 throw new IllegalArgumentException();
184 }
185
186 stopThreshold = DEFAULT_STOP_THRESHOLD;
187 internalSetQualityScores(qualityScores);
188 }
189
190 /**
191 * Constructor.
192 *
193 * @param listener listener to be notified of events such as when estimation
194 * starts, ends or its progress significantly changes.
195 * @param qualityScores quality scores corresponding to each pair of matched
196 * points.
197 * @throws IllegalArgumentException if provided quality scores length is
198 * smaller than MINIMUM_SIZE (i.e. 3 samples).
199 */
200 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
201 final PinholeCameraRobustEstimatorListener listener, final double[] qualityScores) {
202 super(listener);
203 stopThreshold = DEFAULT_STOP_THRESHOLD;
204 internalSetQualityScores(qualityScores);
205 }
206
207 /**
208 * Constructor with listener and lists of points to be used ot estimate a
209 * pinhole camera.
210 * Points in the list located at the same position are considered to be
211 * matched. Hence, both lists must have the same size, and their size must
212 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
213 *
214 * @param listener listener to be notified of events such as when estimation
215 * starts, ends or its progress significantly changes.
216 * @param points3D list of 3D points used to estimate a pinhole camera.
217 * @param points2D list of corresponding projected 2D points used to
218 * estimate a pinhole camera.
219 * @param qualityScores quality scores corresponding to each pair of matched
220 * points.
221 * @throws IllegalArgumentException if provided lists of points don't have
222 * the same size or their size is smaller than
223 * MIN_NUMBER_OF_POINT_CORRESPONDENCES.
224 */
225 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
226 final PinholeCameraRobustEstimatorListener listener, final List<Point3D> points3D,
227 final List<Point2D> points2D, final double[] qualityScores) {
228 super(listener, points3D, points2D);
229
230 if (qualityScores.length != points3D.size()) {
231 throw new IllegalArgumentException();
232 }
233
234 stopThreshold = DEFAULT_STOP_THRESHOLD;
235 internalSetQualityScores(qualityScores);
236 }
237
238 /**
239 * Constructor with intrinsic parameters.
240 *
241 * @param intrinsic intrinsic parameters of camera to be estimated.
242 */
243 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
244 final PinholeCameraIntrinsicParameters intrinsic) {
245 super(intrinsic);
246 stopThreshold = DEFAULT_STOP_THRESHOLD;
247 }
248
249 /**
250 * Constructor with lists of points to be used to estimate a pinhole camera
251 * and intrinsic parameters.
252 * Points in the list located at the same position are considered to be
253 * matched. Hence, both lists must have the same size, and their size must
254 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
255 *
256 * @param intrinsic intrinsic parameters of camera to be estimated.
257 * @param points3D list of 3D points used to estimate a pinhole camera.
258 * @param points2D list of corresponding projected 2D points used to
259 * estimate a pinhole camera.
260 * @throws IllegalArgumentException if provided lists of points don't have
261 * the same size or their size is smaller than required minimum size
262 * (6 correspondences).
263 */
264 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
265 final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
266 final List<Point2D> points2D) {
267 super(intrinsic, points3D, points2D);
268 stopThreshold = DEFAULT_STOP_THRESHOLD;
269 }
270
271 /**
272 * Constructor with intrinsic parameters and listener.
273 *
274 * @param listener listener to be notified of events such as when estimation
275 * starts, ends or its progress significantly changes.
276 * @param intrinsic intrinsic parameters of camera to be estimated.
277 */
278 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
279 final PinholeCameraRobustEstimatorListener listener, final PinholeCameraIntrinsicParameters intrinsic) {
280 super(listener, intrinsic);
281 stopThreshold = DEFAULT_STOP_THRESHOLD;
282 }
283
284 /**
285 * Constructor with listener and lists of points to be used ot estimate a
286 * pinhole camera and intrinsic parameters.
287 * Points in the list located at the same position are considered to be
288 * matched. Hence, both lists must have the same size, and their size must
289 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
290 *
291 * @param listener listener to be notified of events such as when estimation
292 * starts, ends or its progress significantly changes.
293 * @param intrinsic intrinsic parameters of camera to be estimated.
294 * @param points3D list of 3D points used to estimate a pinhole camera.
295 * @param points2D list of corresponding projected 2D points used to
296 * estimate a pinhole camera.
297 * @throws IllegalArgumentException if provided lists of points don't have
298 * the same size or their size is smaller than required minimum size
299 * (6 correspondences).
300 */
301 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
302 final PinholeCameraRobustEstimatorListener listener,
303 final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
304 final List<Point2D> points2D) {
305 super(listener, intrinsic, points3D, points2D);
306 stopThreshold = DEFAULT_STOP_THRESHOLD;
307 }
308
309 /**
310 * Constructor with quality scores and intrinsic parameters.
311 *
312 * @param intrinsic intrinsic parameters of camera to be estimated.
313 * @param qualityScores quality scores corresponding to each pair of matched
314 * points.
315 * @throws IllegalArgumentException if provided quality scores length is
316 * smaller than MINIMUM_SIZE (i.e. 3 samples).
317 */
318 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
319 final PinholeCameraIntrinsicParameters intrinsic, final double[] qualityScores) {
320 super(intrinsic);
321 stopThreshold = DEFAULT_STOP_THRESHOLD;
322 internalSetQualityScores(qualityScores);
323 }
324
325 /**
326 * Constructor with lists of points to be used to estimate a pinhole camera.
327 * Points in the list located at the same position are considered to be
328 * matched. Hence, both lists must have the same size, and their size must
329 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
330 *
331 * @param intrinsic intrinsic parameters of camera to be estimated.
332 * @param points3D list of 3D points used to estimate a pinhole camera.
333 * @param points2D list of corresponding projected 2D points used to
334 * estimate a pinhole camera.
335 * @param qualityScores quality scores corresponding to each pair of matched
336 * points.
337 * @throws IllegalArgumentException if provided lists of points and array
338 * of quality scores don't have the same size or their size is smaller than
339 * 6 correspondences.
340 */
341 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
342 final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
343 final List<Point2D> points2D, final double[] qualityScores) {
344 super(intrinsic, points3D, points2D);
345
346 if (qualityScores.length != points3D.size()) {
347 throw new IllegalArgumentException();
348 }
349
350 stopThreshold = DEFAULT_STOP_THRESHOLD;
351 internalSetQualityScores(qualityScores);
352 }
353
354 /**
355 * Constructor.
356 *
357 * @param listener listener to be notified of events such as when estimation
358 * starts, ends or its progress significantly changes.
359 * @param intrinsic intrinsic parameters of camera to be estimated.
360 * @param qualityScores quality scores corresponding to each pair of matched
361 * points.
362 * @throws IllegalArgumentException if provided quality scores length is
363 * smaller than MINIMUM_SIZE (i.e. 3 samples).
364 */
365 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
366 final PinholeCameraRobustEstimatorListener listener, final PinholeCameraIntrinsicParameters intrinsic,
367 final double[] qualityScores) {
368 super(listener, intrinsic);
369 stopThreshold = DEFAULT_STOP_THRESHOLD;
370 internalSetQualityScores(qualityScores);
371 }
372
373 /**
374 * Constructor with listener and lists of points to be used ot estimate a
375 * pinhole camera and intrinsic parameters.
376 * Points in the list located at the same position are considered to be
377 * matched. Hence, both lists must have the same size, and their size must
378 * be greater or equal than MIN_NUMBER_OF_POINT_CORRESPONDENCES.
379 *
380 * @param listener listener to be notified of events such as when estimation
381 * starts, ends or its progress significantly changes.
382 * @param intrinsic intrinsic parameters of camera to be estimated.
383 * @param points3D list of 3D points used to estimate a pinhole camera.
384 * @param points2D list of corresponding projected 2D points used to
385 * estimate a pinhole camera.
386 * @param qualityScores quality scores corresponding to each pair of matched
387 * points.
388 * @throws IllegalArgumentException if provided lists of points don't have
389 * the same size or their size is smaller than
390 * MIN_NUMBER_OF_POINT_CORRESPONDENCES.
391 */
392 public PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator(
393 final PinholeCameraRobustEstimatorListener listener, final PinholeCameraIntrinsicParameters intrinsic,
394 final List<Point3D> points3D, final List<Point2D> points2D, final double[] qualityScores) {
395 super(listener, intrinsic, points3D, points2D);
396
397 if (qualityScores.length != points3D.size()) {
398 throw new IllegalArgumentException();
399 }
400
401 stopThreshold = DEFAULT_STOP_THRESHOLD;
402 internalSetQualityScores(qualityScores);
403 }
404
405 /**
406 * Returns threshold to be used to keep the algorithm iterating in case that
407 * best estimated threshold using median of residuals is not small enough.
408 * Once a solution is found that generates a threshold below this value, the
409 * algorithm will stop.
410 * As in LMedS, the stop threshold can be used to prevent the PROMedS
411 * algorithm iterating too many times in cases where samples have a very
412 * similar accuracy.
413 * For instance, in cases where proportion of outliers is very small (close
414 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
415 * iterate for a long time trying to find the best solution when indeed
416 * there is no need to do that if a reasonable threshold has already been
417 * reached.
418 * Because of this behaviour the stop threshold can be set to a value much
419 * lower than the one typically used in RANSAC, and yet the algorithm could
420 * still produce even smaller thresholds in estimated results.
421 *
422 * @return stop threshold to stop the algorithm prematurely when a certain
423 * accuracy has been reached.
424 */
425 public double getStopThreshold() {
426 return stopThreshold;
427 }
428
429 /**
430 * Sets threshold to be used to keep the algorithm iterating in case that
431 * best estimated threshold using median of residuals is not small enough.
432 * Once a solution is found that generates a threshold below this value, the
433 * algorithm will stop.
434 * As in LMedS, the stop threshold can be used to prevent the PROMedS
435 * algorithm iterating too many times in cases where samples have a very
436 * similar accuracy.
437 * For instance, in cases where proportion of outliers is very small (close
438 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
439 * iterate for a long time trying to find the best solution when indeed
440 * there is no need to do that if a reasonable threshold has already been
441 * reached.
442 * Because of this behaviour the stop threshold can be set to a value much
443 * lower than the one typically used in RANSAC, and yet the algorithm could
444 * still produce even smaller thresholds in estimated results.
445 *
446 * @param stopThreshold stop threshold to stop the algorithm prematurely
447 * when a certain accuracy has been reached.
448 * @throws IllegalArgumentException if provided value is zero or negative.
449 * @throws LockedException if robust estimator is locked because an
450 * estimation is already in progress.
451 */
452 public void setStopThreshold(final double stopThreshold) throws LockedException {
453 if (isLocked()) {
454 throw new LockedException();
455 }
456 if (stopThreshold <= MIN_STOP_THRESHOLD) {
457 throw new IllegalArgumentException();
458 }
459
460 this.stopThreshold = stopThreshold;
461 }
462
463 /**
464 * Returns quality scores corresponding to each pair of matched points.
465 * The larger the score value the better the quality of the matching.
466 *
467 * @return quality scores corresponding to each pair of matched points.
468 */
469 @Override
470 public double[] getQualityScores() {
471 return qualityScores;
472 }
473
474 /**
475 * Sets quality scores corresponding to each pair of matched points.
476 * The larger the score value the better the quality of the matching.
477 *
478 * @param qualityScores quality scores corresponding to each pair of matched
479 * points.
480 * @throws LockedException if robust estimator is locked because an
481 * estimation is already in progress.
482 * @throws IllegalArgumentException if provided quality scores length is
483 * smaller than MINIMUM_SIZE (i.e. 6 samples).
484 */
485 @Override
486 public void setQualityScores(final double[] qualityScores) throws LockedException {
487 if (isLocked()) {
488 throw new LockedException();
489 }
490 internalSetQualityScores(qualityScores);
491 }
492
493 /**
494 * Indicates if estimator is ready to start the affine 2D transformation
495 * estimation.
496 * This is true when input data (i.e. lists of matched points and quality
497 * scores) are provided and a minimum of MINIMUM_SIZE points are available.
498 *
499 * @return true if estimator is ready, false otherwise.
500 */
501 @Override
502 public boolean isReady() {
503 return super.isReady() && qualityScores != null && qualityScores.length == points3D.size();
504 }
505
506 /**
507 * Estimates an affine 2D transformation using a robust estimator and
508 * the best set of matched 2D point correspondences found using the robust
509 * estimator.
510 *
511 * @return an affine 2D transformation.
512 * @throws LockedException if robust estimator is locked because an
513 * estimation is already in progress.
514 * @throws NotReadyException if provided input data is not enough to start
515 * the estimation.
516 * @throws RobustEstimatorException if estimation fails for any reason
517 * (i.e. numerical instability, no solution available, etc).
518 */
519 @Override
520 public PinholeCamera estimate() throws LockedException, NotReadyException, RobustEstimatorException {
521 if (isLocked()) {
522 throw new LockedException();
523 }
524 if (!isReady()) {
525 throw new NotReadyException();
526 }
527
528 // pinhole camera estimator using EPnP (Efficient Perspective-n-Point) algorithm
529 final var nonRobustEstimator = new EPnPPointCorrespondencePinholeCameraEstimator(intrinsic);
530
531 nonRobustEstimator.setPlanarConfigurationAllowed(planarConfigurationAllowed);
532 nonRobustEstimator.setNullspaceDimension2Allowed(nullspaceDimension2Allowed);
533 nonRobustEstimator.setNullspaceDimension3Allowed(nullspaceDimension3Allowed);
534 nonRobustEstimator.setPlanarThreshold(planarThreshold);
535
536 // suggestions
537 nonRobustEstimator.setSuggestSkewnessValueEnabled(isSuggestSkewnessValueEnabled());
538 nonRobustEstimator.setSuggestedSkewnessValue(getSuggestedSkewnessValue());
539 nonRobustEstimator.setSuggestHorizontalFocalLengthEnabled(isSuggestHorizontalFocalLengthEnabled());
540 nonRobustEstimator.setSuggestedHorizontalFocalLengthValue(getSuggestedHorizontalFocalLengthValue());
541 nonRobustEstimator.setSuggestVerticalFocalLengthEnabled(isSuggestVerticalFocalLengthEnabled());
542 nonRobustEstimator.setSuggestedVerticalFocalLengthValue(getSuggestedVerticalFocalLengthValue());
543 nonRobustEstimator.setSuggestAspectRatioEnabled(isSuggestAspectRatioEnabled());
544 nonRobustEstimator.setSuggestedAspectRatioValue(getSuggestedAspectRatioValue());
545 nonRobustEstimator.setSuggestPrincipalPointEnabled(isSuggestPrincipalPointEnabled());
546 nonRobustEstimator.setSuggestedPrincipalPointValue(getSuggestedPrincipalPointValue());
547 nonRobustEstimator.setSuggestRotationEnabled(isSuggestRotationEnabled());
548 nonRobustEstimator.setSuggestedRotationValue(getSuggestedRotationValue());
549 nonRobustEstimator.setSuggestCenterEnabled(isSuggestCenterEnabled());
550 nonRobustEstimator.setSuggestedCenterValue(getSuggestedCenterValue());
551
552 final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<PinholeCamera>() {
553
554 // point to be reused when computing residuals
555 private final Point2D testPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
556
557 // 3D points for a subset of samples
558 private final List<Point3D> subset3D = new ArrayList<>();
559
560 // 2D points for a subset of samples
561 private final List<Point2D> subset2D = new ArrayList<>();
562
563 @Override
564 public double getThreshold() {
565 return stopThreshold;
566 }
567
568 @Override
569 public int getTotalSamples() {
570 return points3D.size();
571 }
572
573 @Override
574 public int getSubsetSize() {
575 return PointCorrespondencePinholeCameraRobustEstimator.MIN_NUMBER_OF_POINT_CORRESPONDENCES;
576 }
577
578 @Override
579 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<PinholeCamera> solutions) {
580 subset3D.clear();
581 subset3D.add(points3D.get(samplesIndices[0]));
582 subset3D.add(points3D.get(samplesIndices[1]));
583 subset3D.add(points3D.get(samplesIndices[2]));
584 subset3D.add(points3D.get(samplesIndices[3]));
585 subset3D.add(points3D.get(samplesIndices[4]));
586 subset3D.add(points3D.get(samplesIndices[5]));
587
588 subset2D.clear();
589 subset2D.add(points2D.get(samplesIndices[0]));
590 subset2D.add(points2D.get(samplesIndices[1]));
591 subset2D.add(points2D.get(samplesIndices[2]));
592 subset2D.add(points2D.get(samplesIndices[3]));
593 subset2D.add(points2D.get(samplesIndices[4]));
594 subset2D.add(points2D.get(samplesIndices[5]));
595
596 try {
597 nonRobustEstimator.setLists(subset3D, subset2D);
598
599 final var cam = nonRobustEstimator.estimate();
600 solutions.add(cam);
601 } catch (final Exception e) {
602 // if points configuration is degenerate, no solution is added
603 }
604 }
605
606 @Override
607 public double computeResidual(final PinholeCamera currentEstimation, final int i) {
608 // pick i-th points
609 final var point3D = points3D.get(i);
610 final var point2D = points2D.get(i);
611
612 // project point3D into test point
613 currentEstimation.project(point3D, testPoint);
614
615 // compare test point and 2D point
616 return testPoint.distanceTo(point2D);
617 }
618
619 @Override
620 public boolean isReady() {
621 return PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator.this.isReady();
622 }
623
624 @Override
625 public void onEstimateStart(final RobustEstimator<PinholeCamera> estimator) {
626 if (listener != null) {
627 listener.onEstimateStart(PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator.this);
628 }
629 }
630
631 @Override
632 public void onEstimateEnd(final RobustEstimator<PinholeCamera> estimator) {
633 if (listener != null) {
634 listener.onEstimateEnd(PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator.this);
635 }
636 }
637
638 @Override
639 public void onEstimateNextIteration(final RobustEstimator<PinholeCamera> estimator, final int iteration) {
640 if (listener != null) {
641 listener.onEstimateNextIteration(
642 PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator.this, iteration);
643 }
644 }
645
646 @Override
647 public void onEstimateProgressChange(final RobustEstimator<PinholeCamera> estimator, final float progress) {
648 if (listener != null) {
649 listener.onEstimateProgressChange(
650 PROMedSEPnPPointCorrespondencePinholeCameraRobustEstimator.this, progress);
651 }
652 }
653
654 @Override
655 public double[] getQualityScores() {
656 return qualityScores;
657 }
658 });
659
660 try {
661 locked = true;
662 inliersData = null;
663 innerEstimator.setConfidence(confidence);
664 innerEstimator.setMaxIterations(maxIterations);
665 innerEstimator.setProgressDelta(progressDelta);
666 final var result = innerEstimator.estimate();
667 inliersData = innerEstimator.getInliersData();
668 return attemptRefine(result, nonRobustEstimator.getMaxSuggestionWeight());
669 } catch (final com.irurueta.numerical.LockedException e) {
670 throw new LockedException(e);
671 } catch (final com.irurueta.numerical.NotReadyException e) {
672 throw new NotReadyException(e);
673 } finally {
674 locked = false;
675 }
676 }
677
678 /**
679 * Returns method being used for robust estimation.
680 *
681 * @return method being used for robust estimation.
682 */
683 @Override
684 public RobustEstimatorMethod getMethod() {
685 return RobustEstimatorMethod.PROMEDS;
686 }
687
688 /**
689 * Gets standard deviation used for Levenberg-Marquardt fitting during
690 * refinement.
691 * Returned value gives an indication of how much variance each residual
692 * has.
693 * Typically, this value is related to the threshold used on each robust
694 * estimation, since residuals of found inliers are within the range of
695 * such threshold.
696 *
697 * @return standard deviation used for refinement.
698 */
699 @Override
700 protected double getRefinementStandardDeviation() {
701 final var inliersData = (PROMedSRobustEstimator.PROMedSInliersData) getInliersData();
702 return inliersData.getEstimatedThreshold();
703 }
704
705 /**
706 * Sets quality scores corresponding to each pair of matched points.
707 * This method is used internally and does not check whether instance is
708 * locked or not.
709 *
710 * @param qualityScores quality scores to be set.
711 * @throws IllegalArgumentException if provided quality scores length is
712 * smaller than MINIMUM_SIZE.
713 */
714 private void internalSetQualityScores(double[] qualityScores) {
715 if (qualityScores.length < MIN_NUMBER_OF_POINT_CORRESPONDENCES) {
716 throw new IllegalArgumentException();
717 }
718
719 this.qualityScores = qualityScores;
720 }
721 }