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