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