1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.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 algorithm.
34 */
35 @SuppressWarnings("DuplicatedCode")
36 public class PROSACDLTPointCorrespondencePinholeCameraRobustEstimator extends
37 DLTPointCorrespondencePinholeCameraRobustEstimator {
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator() {
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
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 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator(
240 final PinholeCameraRobustEstimatorListener listener, final List<Point3D> points3D,
241 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 DLT (Direct Linear Transform) algorithm
404 final var nonRobustEstimator = new DLTPointCorrespondencePinholeCameraEstimator();
405
406 nonRobustEstimator.setLMSESolutionAllowed(false);
407 nonRobustEstimator.setPointCorrespondencesNormalized(normalizeSubsetPointCorrespondences);
408
409 // suggestions
410 nonRobustEstimator.setSuggestSkewnessValueEnabled(isSuggestSkewnessValueEnabled());
411 nonRobustEstimator.setSuggestedSkewnessValue(getSuggestedSkewnessValue());
412 nonRobustEstimator.setSuggestHorizontalFocalLengthEnabled(isSuggestHorizontalFocalLengthEnabled());
413 nonRobustEstimator.setSuggestedHorizontalFocalLengthValue(getSuggestedHorizontalFocalLengthValue());
414 nonRobustEstimator.setSuggestVerticalFocalLengthEnabled(isSuggestVerticalFocalLengthEnabled());
415 nonRobustEstimator.setSuggestedVerticalFocalLengthValue(getSuggestedVerticalFocalLengthValue());
416 nonRobustEstimator.setSuggestAspectRatioEnabled(isSuggestAspectRatioEnabled());
417 nonRobustEstimator.setSuggestedAspectRatioValue(getSuggestedAspectRatioValue());
418 nonRobustEstimator.setSuggestPrincipalPointEnabled(isSuggestPrincipalPointEnabled());
419 nonRobustEstimator.setSuggestedPrincipalPointValue(getSuggestedPrincipalPointValue());
420 nonRobustEstimator.setSuggestRotationEnabled(isSuggestRotationEnabled());
421 nonRobustEstimator.setSuggestedRotationValue(getSuggestedRotationValue());
422 nonRobustEstimator.setSuggestCenterEnabled(isSuggestCenterEnabled());
423 nonRobustEstimator.setSuggestedCenterValue(getSuggestedCenterValue());
424
425 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PinholeCamera>() {
426
427 // point to be reused when computing residuals
428 private final Point2D testPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
429
430 // 3D points for a subset of samples
431 private final List<Point3D> subset3D = new ArrayList<>();
432
433 // 2D points for a subset of samples
434 private final List<Point2D> subset2D = new ArrayList<>();
435
436 @Override
437 public double getThreshold() {
438 return threshold;
439 }
440
441 @Override
442 public int getTotalSamples() {
443 return points3D.size();
444 }
445
446 @Override
447 public int getSubsetSize() {
448 return PointCorrespondencePinholeCameraRobustEstimator.MIN_NUMBER_OF_POINT_CORRESPONDENCES;
449 }
450
451 @Override
452 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<PinholeCamera> solutions) {
453 subset3D.clear();
454 subset3D.add(points3D.get(samplesIndices[0]));
455 subset3D.add(points3D.get(samplesIndices[1]));
456 subset3D.add(points3D.get(samplesIndices[2]));
457 subset3D.add(points3D.get(samplesIndices[3]));
458 subset3D.add(points3D.get(samplesIndices[4]));
459 subset3D.add(points3D.get(samplesIndices[5]));
460
461 subset2D.clear();
462 subset2D.add(points2D.get(samplesIndices[0]));
463 subset2D.add(points2D.get(samplesIndices[1]));
464 subset2D.add(points2D.get(samplesIndices[2]));
465 subset2D.add(points2D.get(samplesIndices[3]));
466 subset2D.add(points2D.get(samplesIndices[4]));
467 subset2D.add(points2D.get(samplesIndices[5]));
468
469 try {
470 nonRobustEstimator.setLists(subset3D, subset2D);
471
472 final var cam = nonRobustEstimator.estimate();
473 solutions.add(cam);
474 } catch (final Exception e) {
475 // if points configuration is degenerate, no solution is
476 // added
477 }
478 }
479
480 @Override
481 public double computeResidual(final PinholeCamera currentEstimation, final int i) {
482 // pick i-th points
483 final var point3D = points3D.get(i);
484 final var point2D = points2D.get(i);
485
486 // project point3D into test point
487 currentEstimation.project(point3D, testPoint);
488
489 // compare test point and 2D point
490 return testPoint.distanceTo(point2D);
491 }
492
493 @Override
494 public boolean isReady() {
495 return PROSACDLTPointCorrespondencePinholeCameraRobustEstimator.this.isReady();
496 }
497
498 @Override
499 public void onEstimateStart(final RobustEstimator<PinholeCamera> estimator) {
500 if (listener != null) {
501 listener.onEstimateStart(PROSACDLTPointCorrespondencePinholeCameraRobustEstimator.this);
502 }
503 }
504
505 @Override
506 public void onEstimateEnd(final RobustEstimator<PinholeCamera> estimator) {
507 if (listener != null) {
508 listener.onEstimateEnd(PROSACDLTPointCorrespondencePinholeCameraRobustEstimator.this);
509 }
510 }
511
512 @Override
513 public void onEstimateNextIteration(final RobustEstimator<PinholeCamera> estimator, final int iteration) {
514 if (listener != null) {
515 listener.onEstimateNextIteration(
516 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator.this, iteration);
517 }
518 }
519
520 @Override
521 public void onEstimateProgressChange(final RobustEstimator<PinholeCamera> estimator, final float progress) {
522 if (listener != null) {
523 listener.onEstimateProgressChange(
524 PROSACDLTPointCorrespondencePinholeCameraRobustEstimator.this, progress);
525 }
526 }
527
528 @Override
529 public double[] getQualityScores() {
530 return qualityScores;
531 }
532 });
533
534 try {
535 locked = true;
536 inliersData = null;
537 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
538 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
539 innerEstimator.setConfidence(confidence);
540 innerEstimator.setMaxIterations(maxIterations);
541 innerEstimator.setProgressDelta(progressDelta);
542 final var result = innerEstimator.estimate();
543 inliersData = innerEstimator.getInliersData();
544 return attemptRefine(result, nonRobustEstimator.getMaxSuggestionWeight());
545 } catch (final com.irurueta.numerical.LockedException e) {
546 throw new LockedException(e);
547 } catch (final com.irurueta.numerical.NotReadyException e) {
548 throw new NotReadyException(e);
549 } finally {
550 locked = false;
551 }
552 }
553
554 /**
555 * Returns method being used for robust estimation.
556 *
557 * @return method being used for robust estimation.
558 */
559 @Override
560 public RobustEstimatorMethod getMethod() {
561 return RobustEstimatorMethod.PROSAC;
562 }
563
564 /**
565 * Gets standard deviation used for Levenberg-Marquardt fitting during
566 * refinement.
567 * Returned value gives an indication of how much variance each residual
568 * has.
569 * Typically, this value is related to the threshold used on each robust
570 * estimation, since residuals of found inliers are within the range of
571 * such threshold.
572 *
573 * @return standard deviation used for refinement.
574 */
575 @Override
576 protected double getRefinementStandardDeviation() {
577 return threshold;
578 }
579
580 /**
581 * Sets quality scores corresponding to each pair of matched points.
582 * This method is used internally and does not check whether instance is
583 * locked or not.
584 *
585 * @param qualityScores quality scores to be set.
586 * @throws IllegalArgumentException if provided quality scores length is
587 * smaller than MINIMUM_SIZE.
588 */
589 private void internalSetQualityScores(final double[] qualityScores) {
590 if (qualityScores.length < MIN_NUMBER_OF_POINT_CORRESPONDENCES) {
591 throw new IllegalArgumentException();
592 }
593
594 this.qualityScores = qualityScores;
595 }
596 }