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