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.MetricTransformation3D;
20 import com.irurueta.geometry.Point3D;
21 import com.irurueta.numerical.robust.PROSACRobustEstimator;
22 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
23 import com.irurueta.numerical.robust.RobustEstimator;
24 import com.irurueta.numerical.robust.RobustEstimatorException;
25 import com.irurueta.numerical.robust.RobustEstimatorMethod;
26
27 import java.util.ArrayList;
28 import java.util.List;
29
30 /**
31 * Finds the best metric 3D transformation for provided collections of
32 * matched 3D points using PROSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class PROSACMetricTransformation3DRobustEstimator extends MetricTransformation3DRobustEstimator {
36
37 /**
38 * Constant defining default threshold to determine whether points are
39 * inliers or not.
40 * By default, 1.0 is considered a good value for cases where measures are
41 * done on pixels, since typically the minimum resolution is 1 pixel.
42 */
43 public static final double DEFAULT_THRESHOLD = 1.0;
44
45 /**
46 * Minimum value that can be set as threshold.
47 * Threshold must be strictly greater than 0.0.
48 */
49 public static final double MIN_THRESHOLD = 0.0;
50
51 /**
52 * Indicates that by default inliers will only be computed but not kept.
53 */
54 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
55
56 /**
57 * Indicates that by default residuals will only be computed but not kept.
58 */
59 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
60
61 /**
62 * Threshold to determine whether points are inliers or not when testing
63 * possible estimation solutions.
64 * The threshold refers to the amount of error (i.e. distance) a possible
65 * solution has on a matched pair of points.
66 */
67 private double threshold;
68
69 /**
70 * Quality scores corresponding to each pair of matched points.
71 * The larger the score value the better the quality of the matching.
72 */
73 private double[] qualityScores;
74
75 /**
76 * Indicates whether inliers must be computed and kept.
77 */
78 private boolean computeAndKeepInliers;
79
80 /**
81 * Indicates whether residuals must be computed and kept.
82 */
83 private boolean computeAndKeepResiduals;
84
85 /**
86 * Constructor.
87 */
88 public PROSACMetricTransformation3DRobustEstimator() {
89 super();
90 threshold = DEFAULT_THRESHOLD;
91 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
92 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
93 }
94
95 /**
96 * Constructor with lists of points to be used to estimate a metric 3D
97 * transformation.
98 * Points in the list located at the same position are considered to be
99 * matched. Hence, both lists must have the same size, and their size must
100 * be greater or equal than MINIMUM_SIZE.
101 *
102 * @param inputPoints list of input points to be used to estimate a
103 * metric 3D transformation.
104 * @param outputPoints list of output points to be used to estimate a
105 * metric 3D transformation.
106 * @throws IllegalArgumentException if provided lists of points don't have
107 * the same size or their size is smaller than MINIMUM_SIZE.
108 */
109 public PROSACMetricTransformation3DRobustEstimator(
110 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
111 super(inputPoints, outputPoints);
112 threshold = DEFAULT_THRESHOLD;
113 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
114 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
115 }
116
117 /**
118 * Constructor.
119 *
120 * @param listener listener to be notified of events such as when estimation
121 * starts, ends or its progress significantly changes.
122 */
123 public PROSACMetricTransformation3DRobustEstimator(final MetricTransformation3DRobustEstimatorListener listener) {
124 super(listener);
125 threshold = DEFAULT_THRESHOLD;
126 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
127 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
128 }
129
130 /**
131 * Constructor with listener and lists of points to be used to estimate a
132 * metric 3D transformation.
133 * Points in the list located at the same position are considered to be
134 * matched. Hence, both lists must have the same size, and their size must
135 * be greater or equal than MINIMUM_SIZE.
136 *
137 * @param listener listener to be notified of events such as when estimation
138 * stars, ends or its progress significantly changes.
139 * @param inputPoints list of input points to be used to estimate a
140 * metric 3D transformation.
141 * @param outputPoints list of output points to be used to estimate a
142 * metric 3D transformation.
143 * @throws IllegalArgumentException if provided lists of points don't have
144 * the same size or their size is smaller than MINIMUM_SIZE.
145 */
146 public PROSACMetricTransformation3DRobustEstimator(
147 final MetricTransformation3DRobustEstimatorListener listener,
148 final List<Point3D> inputPoints, final List<Point3D> outputPoints) {
149 super(listener, inputPoints, outputPoints);
150 threshold = DEFAULT_THRESHOLD;
151 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
152 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
153 }
154
155 /**
156 * Constructor.
157 *
158 * @param qualityScores quality scores corresponding to each pair of matched
159 * points.
160 * @throws IllegalArgumentException if provided quality scores length is
161 * smaller than MINIMUM_SIZE (i.e. 3 samples).
162 */
163 public PROSACMetricTransformation3DRobustEstimator(final double[] qualityScores) {
164 super();
165 threshold = DEFAULT_THRESHOLD;
166 internalSetQualityScores(qualityScores);
167 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
168 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
169 }
170
171 /**
172 * Constructor with lists of points to be used to estimate a metric 3D
173 * transformation.
174 * Points in the list located at the same position are considered to be
175 * matched. Hence, both lists must have the same size, and their size must
176 * be greater or equal than MINIMUM_SIZE.
177 *
178 * @param inputPoints list of input points to be used to estimate a
179 * metric 3D transformation.
180 * @param outputPoints list of output points to be used to estimate a
181 * metric 3D transformation.
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 * MINIMUM_SIZE.
187 */
188 public PROSACMetricTransformation3DRobustEstimator(
189 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final double[] qualityScores) {
190 super(inputPoints, outputPoints);
191
192 if (qualityScores.length != inputPoints.size()) {
193 throw new IllegalArgumentException();
194 }
195
196 threshold = DEFAULT_THRESHOLD;
197 internalSetQualityScores(qualityScores);
198 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
199 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
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 PROSACMetricTransformation3DRobustEstimator(
213 final MetricTransformation3DRobustEstimatorListener listener, final double[] qualityScores) {
214 super(listener);
215 threshold = DEFAULT_THRESHOLD;
216 internalSetQualityScores(qualityScores);
217 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
218 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
219 }
220
221 /**
222 * Constructor with listener and lists of points to be used to estimate a
223 * metric 3D transformation.
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 MINIMUM_SIZE.
227 *
228 * @param listener listener to be notified of events such as when estimation
229 * stars, ends or its progress significantly changes.
230 * @param inputPoints list of input points to be used to estimate a
231 * metric 3D transformation.
232 * @param outputPoints list of output points to be used to estimate a
233 * metric 3D transformation.
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 MINIMUM_SIZE.
238 */
239 public PROSACMetricTransformation3DRobustEstimator(
240 final MetricTransformation3DRobustEstimatorListener listener, final List<Point3D> inputPoints,
241 final List<Point3D> outputPoints, final double[] qualityScores) {
242 super(listener, inputPoints, outputPoints);
243
244 if (qualityScores.length != inputPoints.size()) {
245 throw new IllegalArgumentException();
246 }
247
248 threshold = DEFAULT_THRESHOLD;
249 internalSetQualityScores(qualityScores);
250 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
251 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
252 }
253
254 /**
255 * Constructor.
256 *
257 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
258 */
259 public PROSACMetricTransformation3DRobustEstimator(final boolean weakMinimumSizeAllowed) {
260 super(weakMinimumSizeAllowed);
261 threshold = DEFAULT_THRESHOLD;
262 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
263 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
264 }
265
266 /**
267 * Constructor with lists of points to be used to estimate a metric 3D
268 * transformation.
269 * Points in the list located at the same position are considered to be
270 * matched. Hence, both lists must have the same size, and their size must
271 * be greater or equal than MINIMUM_SIZE.
272 *
273 * @param inputPoints list of input points to be used to estimate a
274 * metric 3D transformation.
275 * @param outputPoints list of output points to be used to estimate a
276 * metric 3D transformation.
277 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
278 * @throws IllegalArgumentException if provided lists of points don't have
279 * the same size or their size is smaller than MINIMUM_SIZE.
280 */
281 public PROSACMetricTransformation3DRobustEstimator(
282 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
283 super(inputPoints, outputPoints, weakMinimumSizeAllowed);
284 threshold = DEFAULT_THRESHOLD;
285 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
286 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
287 }
288
289 /**
290 * Constructor.
291 *
292 * @param listener listener to be notified of events such as when estimation
293 * starts, ends or its progress significantly changes.
294 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
295 */
296 public PROSACMetricTransformation3DRobustEstimator(
297 final MetricTransformation3DRobustEstimatorListener listener, final boolean weakMinimumSizeAllowed) {
298 super(listener, weakMinimumSizeAllowed);
299 threshold = DEFAULT_THRESHOLD;
300 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
301 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
302 }
303
304 /**
305 * Constructor with listener and lists of points to be used to estimate a
306 * metric 3D transformation.
307 * Points in the list located at the same position are considered to be
308 * matched. Hence, both lists must have the same size, and their size must
309 * be greater or equal than MINIMUM_SIZE.
310 *
311 * @param listener listener to be notified of events such as when estimation
312 * stars, ends or its progress significantly changes.
313 * @param inputPoints list of input points to be used to estimate a
314 * metric 3D transformation.
315 * @param outputPoints list of output points to be used to estimate a
316 * metric 3D transformation.
317 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
318 * @throws IllegalArgumentException if provided lists of points don't have
319 * the same size or their size is smaller than MINIMUM_SIZE.
320 */
321 public PROSACMetricTransformation3DRobustEstimator(
322 final MetricTransformation3DRobustEstimatorListener listener,
323 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final boolean weakMinimumSizeAllowed) {
324 super(listener, inputPoints, outputPoints, weakMinimumSizeAllowed);
325 threshold = DEFAULT_THRESHOLD;
326 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
327 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
328 }
329
330 /**
331 * Constructor.
332 *
333 * @param qualityScores quality scores corresponding to each pair of matched
334 * points.
335 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
336 * @throws IllegalArgumentException if provided quality scores length is
337 * smaller than MINIMUM_SIZE (i.e. 3 samples).
338 */
339 public PROSACMetricTransformation3DRobustEstimator(
340 final double[] qualityScores, final boolean weakMinimumSizeAllowed) {
341 super(weakMinimumSizeAllowed);
342 threshold = DEFAULT_THRESHOLD;
343 internalSetQualityScores(qualityScores);
344 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
345 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
346 }
347
348 /**
349 * Constructor with lists of points to be used to estimate a metric 3D
350 * transformation.
351 * Points in the list located at the same position are considered to be
352 * matched. Hence, both lists must have the same size, and their size must
353 * be greater or equal than MINIMUM_SIZE.
354 *
355 * @param inputPoints list of input points to be used to estimate a
356 * metric 3D transformation.
357 * @param outputPoints list of output points to be used to estimate a
358 * metric 3D transformation.
359 * @param qualityScores quality scores corresponding to each pair of matched
360 * points.
361 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
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 * MINIMUM_SIZE.
365 */
366 public PROSACMetricTransformation3DRobustEstimator(
367 final List<Point3D> inputPoints, final List<Point3D> outputPoints, final double[] qualityScores,
368 final boolean weakMinimumSizeAllowed) {
369 super(inputPoints, outputPoints, weakMinimumSizeAllowed);
370
371 if (qualityScores.length != inputPoints.size()) {
372 throw new IllegalArgumentException();
373 }
374
375 threshold = DEFAULT_THRESHOLD;
376 internalSetQualityScores(qualityScores);
377 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
378 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
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 qualityScores quality scores corresponding to each pair of matched
387 * points.
388 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
389 * @throws IllegalArgumentException if provided quality scores length is
390 * smaller than MINIMUM_SIZE (i.e. 3 samples).
391 */
392 public PROSACMetricTransformation3DRobustEstimator(
393 final MetricTransformation3DRobustEstimatorListener listener, final double[] qualityScores,
394 final boolean weakMinimumSizeAllowed) {
395 super(listener, weakMinimumSizeAllowed);
396 threshold = DEFAULT_THRESHOLD;
397 internalSetQualityScores(qualityScores);
398 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
399 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
400 }
401
402 /**
403 * Constructor with listener and lists of points to be used to estimate a
404 * metric 3D transformation.
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 MINIMUM_SIZE.
408 *
409 * @param listener listener to be notified of events such as when estimation
410 * stars, ends or its progress significantly changes.
411 * @param inputPoints list of input points to be used to estimate a
412 * metric 3D transformation.
413 * @param outputPoints list of output points to be used to estimate a
414 * metric 3D transformation.
415 * @param qualityScores quality scores corresponding to each pair of matched
416 * points.
417 * @param weakMinimumSizeAllowed true allows 3 points, false requires 4.
418 * @throws IllegalArgumentException if provided lists of points don't have
419 * the same size or their size is smaller than MINIMUM_SIZE.
420 */
421 public PROSACMetricTransformation3DRobustEstimator(
422 final MetricTransformation3DRobustEstimatorListener listener, final List<Point3D> inputPoints,
423 final List<Point3D> outputPoints, final double[] qualityScores, final boolean weakMinimumSizeAllowed) {
424 super(listener, inputPoints, outputPoints, weakMinimumSizeAllowed);
425
426 if (qualityScores.length != inputPoints.size()) {
427 throw new IllegalArgumentException();
428 }
429
430 threshold = DEFAULT_THRESHOLD;
431 internalSetQualityScores(qualityScores);
432 computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
433 computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
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 not
456 * when testing possible estimation solutions.
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 metric 3D 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 == inputPoints.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 a metric 3D transformation using a robust estimator and
565 * the best set of matched 3D point correspondences found using the robust
566 * estimator.
567 *
568 * @return a metric 3D 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 MetricTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
578 if (isLocked()) {
579 throw new LockedException();
580 }
581 if (!isReady()) {
582 throw new NotReadyException();
583 }
584
585 final var innerEstimator = new PROSACRobustEstimator<>(
586 new PROSACRobustEstimatorListener<MetricTransformation3D>() {
587
588 // point to be reused when computing residuals
589 private final Point3D testPoint = Point3D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
590
591 private final MetricTransformation3DEstimator nonRobustEstimator =
592 new MetricTransformation3DEstimator(isWeakMinimumSizeAllowed());
593
594 private final List<Point3D> subsetInputPoints = new ArrayList<>();
595 private final List<Point3D> subsetOutputPoints = new ArrayList<>();
596
597 @Override
598 public double getThreshold() {
599 return threshold;
600 }
601
602 @Override
603 public int getTotalSamples() {
604 return inputPoints.size();
605 }
606
607 @Override
608 public int getSubsetSize() {
609 return nonRobustEstimator.getMinimumPoints();
610 }
611
612 @Override
613 public void estimatePreliminarSolutions(
614 final int[] samplesIndices, final List<MetricTransformation3D> solutions) {
615 subsetInputPoints.clear();
616 subsetOutputPoints.clear();
617 for (final var samplesIndex : samplesIndices) {
618 subsetInputPoints.add(inputPoints.get(samplesIndex));
619 subsetOutputPoints.add(outputPoints.get(samplesIndex));
620 }
621
622 try {
623 nonRobustEstimator.setPoints(subsetInputPoints, subsetOutputPoints);
624 solutions.add(nonRobustEstimator.estimate());
625 } catch (final Exception e) {
626 // if points are coincident, no solution is added
627 }
628 }
629
630 @Override
631 public double computeResidual(final MetricTransformation3D currentEstimation, final int i) {
632 final var inputPoint = inputPoints.get(i);
633 final var outputPoint = outputPoints.get(i);
634
635 // transform input point and store result in mTestPoint
636 currentEstimation.transform(inputPoint, testPoint);
637
638 return outputPoint.distanceTo(testPoint);
639 }
640
641 @Override
642 public boolean isReady() {
643 return PROSACMetricTransformation3DRobustEstimator.this.isReady();
644 }
645
646 @Override
647 public void onEstimateStart(final RobustEstimator<MetricTransformation3D> estimator) {
648 if (listener != null) {
649 listener.onEstimateStart(PROSACMetricTransformation3DRobustEstimator.this);
650 }
651 }
652
653 @Override
654 public void onEstimateEnd(final RobustEstimator<MetricTransformation3D> estimator) {
655 if (listener != null) {
656 listener.onEstimateEnd(PROSACMetricTransformation3DRobustEstimator.this);
657 }
658 }
659
660 @Override
661 public void onEstimateNextIteration(
662 final RobustEstimator<MetricTransformation3D> estimator, final int iteration) {
663 if (listener != null) {
664 listener.onEstimateNextIteration(
665 PROSACMetricTransformation3DRobustEstimator.this, iteration);
666 }
667 }
668
669 @Override
670 public void onEstimateProgressChange(
671 final RobustEstimator<MetricTransformation3D> estimator, final float progress) {
672 if (listener != null) {
673 listener.onEstimateProgressChange(
674 PROSACMetricTransformation3DRobustEstimator.this, progress);
675 }
676 }
677
678 @Override
679 public double[] getQualityScores() {
680 return qualityScores;
681 }
682 });
683
684 try {
685 locked = true;
686 inliersData = null;
687 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
688 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
689 innerEstimator.setConfidence(confidence);
690 innerEstimator.setMaxIterations(maxIterations);
691 innerEstimator.setProgressDelta(progressDelta);
692 final var transformation = innerEstimator.estimate();
693 inliersData = innerEstimator.getInliersData();
694 return attemptRefine(transformation);
695 } catch (final com.irurueta.numerical.LockedException e) {
696 throw new LockedException(e);
697 } catch (final com.irurueta.numerical.NotReadyException e) {
698 throw new NotReadyException(e);
699 } finally {
700 locked = false;
701 }
702 }
703
704 /**
705 * Returns method being used for robust estimation.
706 *
707 * @return method being used for robust estimation.
708 */
709 @Override
710 public RobustEstimatorMethod getMethod() {
711 return RobustEstimatorMethod.PROSAC;
712 }
713
714 /**
715 * Gets standard deviation used for Levenberg-Marquardt fitting during
716 * refinement.
717 * Returned value gives an indication of how much variance each residual
718 * has.
719 * Typically, this value is related to the threshold used on each robust
720 * estimation, since residuals of found inliers are within the range of
721 * such threshold.
722 *
723 * @return standard deviation used for refinement.
724 */
725 @Override
726 protected double getRefinementStandardDeviation() {
727 return threshold;
728 }
729
730 /**
731 * Sets quality scores corresponding to each pair of matched points.
732 * This method is used internally and does not check whether instance is
733 * locked or not.
734 *
735 * @param qualityScores quality scores to be set.
736 * @throws IllegalArgumentException if provided quality scores length is
737 * smaller than MINIMUM_SIZE.
738 */
739 private void internalSetQualityScores(final double[] qualityScores) {
740 if (qualityScores.length < getMinimumPoints()) {
741 throw new IllegalArgumentException();
742 }
743
744 this.qualityScores = qualityScores;
745 }
746 }