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.CoincidentPlanesException;
20 import com.irurueta.geometry.Plane;
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 planes using PROSAC algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator
36 extends PlaneCorrespondenceProjectiveTransformation3DRobustEstimator {
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 planes 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 * lines.
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 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator() {
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 a projective 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 a
113 * projective 3D transformation.
114 * @param outputPlanes list of output planes to be used to estimate a
115 * projective 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 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
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 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
134 final ProjectiveTransformation3DRobustEstimatorListener 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 a
143 * projective 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 a
151 * projective 3D transformation.
152 * @param outputPlanes list of output planes to be used to estimate a
153 * projective 3D transformation.
154 * @throws IllegalArgumentException if provided lists of planes don't have
155 * the same size or their size is smaller than MINIMUM_SIZE.
156 */
157 public PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
158 final ProjectiveTransformation3DRobustEstimatorListener 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 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(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 a projective 3D
184 * transformation.
185 * Planes 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 a
190 * projective 3D transformation.
191 * @param outputPlanes list of output planes to be used to estimate a
192 * projective 3D transformation.
193 * @param qualityScores quality scores corresponding to each pair of matched
194 * planes.
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 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
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 * planes.
220 * @throws IllegalArgumentException if provided quality scores length is
221 * smaller than MINIMUM_SIZE (i.e. 3 samples).
222 */
223 public PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
224 final ProjectiveTransformation3DRobustEstimatorListener 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 a
234 * projective 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 a
242 * projective 3D transformation.
243 * @param outputPlanes list of output planes to be used to estimate a
244 * projective 3D transformation.
245 * @param qualityScores quality scores corresponding to each pair of matched
246 * planes.
247 * @throws IllegalArgumentException if provided lists of planes don't have
248 * the same size or their size is smaller than MINIMUM_SIZE.
249 */
250 public PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
251 final ProjectiveTransformation3DRobustEstimatorListener 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 planes is -1, then although their director vectors
276 * are opposed, planes are considered equal, since sign changes are not taken
277 * 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 lines 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 projective 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 lines 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 a projective 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 a projective 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 ProjectiveTransformation3D 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<ProjectiveTransformation3D>() {
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 ProjectiveTransformation3DRobustEstimator.MINIMUM_SIZE;
448 }
449
450 @Override
451 public void estimatePreliminarSolutions(
452 final int[] samplesIndices, final List<ProjectiveTransformation3D> solutions) {
453 final var inputPlane1 = inputPlanes.get(samplesIndices[0]);
454 final var inputPlane2 = inputPlanes.get(samplesIndices[1]);
455 final var inputPlane3 = inputPlanes.get(samplesIndices[2]);
456 final var inputPlane4 = inputPlanes.get(samplesIndices[3]);
457 final var inputPlane5 = inputPlanes.get(samplesIndices[4]);
458
459 final var outputPlane1 = outputPlanes.get(samplesIndices[0]);
460 final var outputPlane2 = outputPlanes.get(samplesIndices[1]);
461 final var outputPlane3 = outputPlanes.get(samplesIndices[2]);
462 final var outputPlane4 = outputPlanes.get(samplesIndices[3]);
463 final var outputPlane5 = outputPlanes.get(samplesIndices[4]);
464
465 try {
466 final var transformation = new ProjectiveTransformation3D(inputPlane1, inputPlane2,
467 inputPlane3, inputPlane4, inputPlane5, outputPlane1, outputPlane2, outputPlane3,
468 outputPlane4, outputPlane5);
469 solutions.add(transformation);
470 } catch (final CoincidentPlanesException e) {
471 // if lines are coincident, no solution is added
472 }
473 }
474
475 @Override
476 public double computeResidual(final ProjectiveTransformation3D currentEstimation, final int i) {
477 final var inputPlane = inputPlanes.get(i);
478 final var outputPlane = outputPlanes.get(i);
479
480 // transform input plane and store result in mTestPlane
481 try {
482 currentEstimation.transform(inputPlane, testPlane);
483
484 return getResidual(outputPlane, testPlane);
485 } catch (final AlgebraException e) {
486 // this happens when internal matrix of affine transformation
487 // cannot be reverse (i.e. transformation is not well-defined,
488 // numerical instabilities, etc.)
489 return Double.MAX_VALUE;
490 }
491 }
492
493 @Override
494 public boolean isReady() {
495 return PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this.isReady();
496 }
497
498 @Override
499 public void onEstimateStart(final RobustEstimator<ProjectiveTransformation3D> estimator) {
500 if (listener != null) {
501 listener.onEstimateStart(
502 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
503 }
504 }
505
506 @Override
507 public void onEstimateEnd(final RobustEstimator<ProjectiveTransformation3D> estimator) {
508 if (listener != null) {
509 listener.onEstimateEnd(
510 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
511 }
512 }
513
514 @Override
515 public void onEstimateNextIteration(
516 final RobustEstimator<ProjectiveTransformation3D> estimator, final int iteration) {
517 if (listener != null) {
518 listener.onEstimateNextIteration(
519 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
520 iteration);
521 }
522 }
523
524 @Override
525 public void onEstimateProgressChange(
526 final RobustEstimator<ProjectiveTransformation3D> estimator, final float progress) {
527 if (listener != null) {
528 listener.onEstimateProgressChange(
529 PROSACPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
530 progress);
531 }
532 }
533
534 @Override
535 public double[] getQualityScores() {
536 return qualityScores;
537 }
538 });
539
540 try {
541 locked = true;
542 inliersData = null;
543 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
544 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
545 innerEstimator.setConfidence(confidence);
546 innerEstimator.setMaxIterations(maxIterations);
547 innerEstimator.setProgressDelta(progressDelta);
548 final var transformation = innerEstimator.estimate();
549 inliersData = innerEstimator.getInliersData();
550 return attemptRefine(transformation);
551 } catch (final com.irurueta.numerical.LockedException e) {
552 throw new LockedException(e);
553 } catch (final com.irurueta.numerical.NotReadyException e) {
554 throw new NotReadyException(e);
555 } finally {
556 locked = false;
557 }
558 }
559
560 /**
561 * Returns method being used for robust estimation.
562 *
563 * @return method being used for robust estimation.
564 */
565 @Override
566 public RobustEstimatorMethod getMethod() {
567 return RobustEstimatorMethod.PROSAC;
568 }
569
570 /**
571 * Gets standard deviation used for Levenberg-Marquardt fitting during
572 * refinement.
573 * Returned value gives an indication of how much variance each residual
574 * has.
575 * Typically, this value is related to the threshold used on each robust
576 * estimation, since residuals of found inliers are within the range of
577 * such threshold.
578 *
579 * @return standard deviation used for refinement.
580 */
581 @Override
582 protected double getRefinementStandardDeviation() {
583 return threshold;
584 }
585
586 /**
587 * Sets quality scores corresponding to each pair of matched lines.
588 * This method is used internally and does not check whether instance is
589 * locked or not.
590 *
591 * @param qualityScores quality scores to be set.
592 * @throws IllegalArgumentException if provided quality scores length is
593 * smaller than MINIMUM_SIZE.
594 */
595 private void internalSetQualityScores(final double[] qualityScores) {
596 if (qualityScores.length < MINIMUM_SIZE) {
597 throw new IllegalArgumentException();
598 }
599
600 this.qualityScores = qualityScores;
601 }
602
603 }