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