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