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.PROMedSRobustEstimator;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
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 PROMedS algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator
36 extends PlaneCorrespondenceProjectiveTransformation3DRobustEstimator {
37
38 /**
39 * Default value to be used for stop threshold. Stop threshold can be used
40 * to keep the algorithm iterating in case that best estimated threshold
41 * using median of residuals is not small enough. Once a solution is found
42 * that generates a threshold below this value, the algorithm will stop.
43 * The stop threshold can be used to prevent the LMedS algorithm iterating
44 * too many times in cases where samples have a very similar accuracy.
45 * For instance, in cases where proportion of outliers is very small (close
46 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
47 * iterate for a long time trying to find the best solution when indeed
48 * there is no need to do that if a reasonable threshold has already been
49 * reached.
50 * Because of this behaviour the stop threshold can be set to a value much
51 * lower than the one typically used in RANSAC, and yet the algorithm could
52 * still produce even smaller thresholds in estimated results.
53 */
54 public static final double DEFAULT_STOP_THRESHOLD = 1e-6;
55
56 /**
57 * Minimum allowed stop threshold value.
58 */
59 public static final double MIN_STOP_THRESHOLD = 0.0;
60
61 /**
62 * Threshold to be used to keep the algorithm iterating in case that best
63 * estimated threshold using median of residuals is not small enough. Once
64 * a solution is found that generates a threshold below this value, the
65 * algorithm will stop.
66 * The stop threshold can be used to prevent the LMedS algorithm iterating
67 * too many times in cases where samples have a very similar accuracy.
68 * For instance, in cases where proportion of outliers is very small (close
69 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
70 * iterate for a long time trying to find the best solution when indeed
71 * there is no need to do that if a reasonable threshold has already been
72 * reached.
73 * Because of this behaviour the stop threshold can be set to a value much
74 * lower than the one typically used in RANSAC, and yet the algorithm could
75 * still produce even smaller thresholds in estimated results.
76 */
77 private double stopThreshold;
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 * Constructor.
87 */
88 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator() {
89 super();
90 stopThreshold = DEFAULT_STOP_THRESHOLD;
91 }
92
93 /**
94 * Constructor with lists of planes to be used to estimate a projective 3D
95 * transformation.
96 * Planes in the list located at the same position are considered to be
97 * matched. Hence, both lists must have the same size, and their size must
98 * be greater or equal than MINIMUM_SIZE.
99 *
100 * @param inputPlanes list of input planes to be used to estimate a
101 * projective 3D transformation.
102 * @param outputPlanes list of output planes to be used to estimate a
103 * projective 3D transformation.
104 * @throws IllegalArgumentException if provided lists of planes don't have
105 * the same size or their size is smaller than MINIMUM_SIZE.
106 */
107 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
108 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
109 super(inputPlanes, outputPlanes);
110 stopThreshold = DEFAULT_STOP_THRESHOLD;
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param listener listener to be notified of events such as when estimation
117 * starts, ends or its progress significantly changes.
118 */
119 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
120 final ProjectiveTransformation3DRobustEstimatorListener listener) {
121 super(listener);
122 stopThreshold = DEFAULT_STOP_THRESHOLD;
123 }
124
125 /**
126 * Constructor with listener and lists of planes to be used to estimate a
127 * projective 3D transformation.
128 * Planes in the list located at the same position are considered to be
129 * matched. Hence, both lists must have the same size, and their size must
130 * be greater or equal than MINIMUM_SIZE.
131 *
132 * @param listener listener to be notified of events such as when estimation
133 * starts, ends or its progress significantly changes.
134 * @param inputPlanes list of input planes to be used to estimate a
135 * projective 3D transformation.
136 * @param outputPlanes list of output planes to be used to estimate a
137 * projective 3D transformation.
138 * @throws IllegalArgumentException if provided lists of planes don't have
139 * the same size or their size is smaller than MINIMUM_SIZE.
140 */
141 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
142 final ProjectiveTransformation3DRobustEstimatorListener listener,
143 final List<Plane> inputPlanes, final List<Plane> outputPlanes) {
144 super(listener, inputPlanes, outputPlanes);
145 stopThreshold = DEFAULT_STOP_THRESHOLD;
146 }
147
148 /**
149 * Constructor.
150 *
151 * @param qualityScores quality scores corresponding to each pair of matched
152 * points.
153 * @throws IllegalArgumentException if provided quality scores length is
154 * smaller than MINIMUM_SIZE (i.e. 3 samples).
155 */
156 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(final double[] qualityScores) {
157 super();
158 stopThreshold = DEFAULT_STOP_THRESHOLD;
159 internalSetQualityScores(qualityScores);
160 }
161
162 /**
163 * Constructor with lists of planes to be used to estimate a projective 3D
164 * transformation.
165 * Planes in the list located at the same position are considered to be
166 * matched. Hence, both lists must have the same size, and their size must
167 * be greater or equal than MINIMUM_SIZE.
168 *
169 * @param inputPlanes list of input planes to be used to estimate a
170 * projective 3D transformation.
171 * @param outputPlanes list of output planes to be used to estimate a
172 * projective 3D transformation.
173 * @param qualityScores quality scores corresponding to each pair of matched
174 * planes.
175 * @throws IllegalArgumentException if provided lists of planes and array
176 * of quality scores don't have the same size or their size is smaller than
177 * MINIMUM_SIZE.
178 */
179 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
180 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final double[] qualityScores) {
181 super(inputPlanes, outputPlanes);
182
183 if (qualityScores.length != inputPlanes.size()) {
184 throw new IllegalArgumentException();
185 }
186
187 stopThreshold = DEFAULT_STOP_THRESHOLD;
188 internalSetQualityScores(qualityScores);
189 }
190
191 /**
192 * Constructor.
193 *
194 * @param listener listener to be notified of events such as when estimation
195 * starts, ends or its progress significantly changes.
196 * @param qualityScores quality scores corresponding to each pair of matched
197 * planes.
198 * @throws IllegalArgumentException if provided quality scores length is
199 * smaller than MINIMUM_SIZE (i.e. 3 samples).
200 */
201 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
202 final ProjectiveTransformation3DRobustEstimatorListener listener, final double[] qualityScores) {
203 super(listener);
204 stopThreshold = DEFAULT_STOP_THRESHOLD;
205 internalSetQualityScores(qualityScores);
206 }
207
208 /**
209 * Constructor with listener and lists of planes to be used to estimate a
210 * projective 3D transformation.
211 * Planes in the list located at the same position are considered to be
212 * matched. Hence, both lists must have the same size, and their size must
213 * be greater or equal than MINIMUM_SIZE.
214 *
215 * @param listener listener to be notified of events such as when estimation
216 * starts, ends or its progress significantly changes.
217 * @param inputPlanes list of input planes to be used to estimate a
218 * projective 3D transformation.
219 * @param outputPlanes list of output planes to be used to estimate a
220 * projective 3D transformation.
221 * @param qualityScores quality scores corresponding to each pair of matched
222 * planes.
223 * @throws IllegalArgumentException if provided lists of planes don't have
224 * the same size or their size is smaller than MINIMUM_SIZE.
225 */
226 public PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator(
227 final ProjectiveTransformation3DRobustEstimatorListener listener,
228 final List<Plane> inputPlanes, final List<Plane> outputPlanes, final double[] qualityScores) {
229 super(listener, inputPlanes, outputPlanes);
230
231 if (qualityScores.length != inputPlanes.size()) {
232 throw new IllegalArgumentException();
233 }
234
235 stopThreshold = DEFAULT_STOP_THRESHOLD;
236 internalSetQualityScores(qualityScores);
237 }
238
239 /**
240 * Returns threshold to be used to keep the algorithm iterating in case that
241 * best estimated threshold using median of residuals is not small enough.
242 * Once a solution is found that generates a threshold below this value, the
243 * algorithm will stop.
244 * The stop threshold can be used to prevent the LMedS algorithm iterating
245 * too many times in cases where samples have a very similar accuracy.
246 * For instance, in cases where proportion of outliers is very small (close
247 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
248 * iterate for a long time trying to find the best solution when indeed
249 * there is no need to do that if a reasonable threshold has already been
250 * reached.
251 * Because of this behaviour the stop threshold can be set to a value much
252 * lower than the one typically used in RANSAC, and yet the algorithm could
253 * still produce even smaller thresholds in estimated results.
254 *
255 * @return stop threshold to stop the algorithm prematurely when a certain
256 * accuracy has been reached.
257 */
258 public double getStopThreshold() {
259 return stopThreshold;
260 }
261
262 /**
263 * Sets threshold to be used to keep the algorithm iterating in case that
264 * best estimated threshold using median of residuals is not small enough.
265 * Once a solution is found that generates a threshold below this value, the
266 * algorithm will stop.
267 * The stop threshold can be used to prevent the LMedS algorithm iterating
268 * too many times in cases where samples have a very similar accuracy.
269 * For instance, in cases where proportion of outliers is very small (close
270 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
271 * iterate for a long time trying to find the best solution when indeed
272 * there is no need to do that if a reasonable threshold has already been
273 * reached.
274 * Because of this behaviour the stop threshold can be set to a value much
275 * lower than the one typically used in RANSAC, and yet the algorithm could
276 * still produce even smaller thresholds in estimated results.
277 *
278 * @param stopThreshold stop threshold to stop the algorithm prematurely
279 * when a certain accuracy has been reached.
280 * @throws IllegalArgumentException if provided value is zero or negative.
281 * @throws LockedException if robust estimator is locked because an
282 * estimation is already in progress.
283 */
284 public void setStopThreshold(final double stopThreshold) throws LockedException {
285 if (isLocked()) {
286 throw new LockedException();
287 }
288 if (stopThreshold <= MIN_STOP_THRESHOLD) {
289 throw new IllegalArgumentException();
290 }
291
292 this.stopThreshold = stopThreshold;
293 }
294
295 /**
296 * Returns quality scores corresponding to each pair of matched planes.
297 * The larger the score value the better the quality of the matching.
298 *
299 * @return quality scores corresponding to each pair of matched planes.
300 */
301 @Override
302 public double[] getQualityScores() {
303 return qualityScores;
304 }
305
306 /**
307 * Sets quality scores corresponding to each pair of matched planes.
308 * The larger the score value the better the quality of the matching.
309 *
310 * @param qualityScores quality scores corresponding to each pair of matched
311 * planes.
312 * @throws LockedException if robust estimator is locked because an
313 * estimation is already in progress.
314 * @throws IllegalArgumentException if provided quality scores length is
315 * smaller than MINIMUM_SIZE (i.e. 3 samples).
316 */
317 @Override
318 public void setQualityScores(final double[] qualityScores) throws LockedException {
319 if (isLocked()) {
320 throw new LockedException();
321 }
322 internalSetQualityScores(qualityScores);
323 }
324
325 /**
326 * Indicates if estimator is ready to start the projective 3D transformation
327 * estimation.
328 * This is true when input data (i.e. lists of matched planes and quality
329 * scores) are provided and a minimum of MINIMUM_SIZE lines are available.
330 *
331 * @return true if estimator is ready, false otherwise.
332 */
333 @Override
334 public boolean isReady() {
335 return super.isReady() && qualityScores != null && qualityScores.length == inputPlanes.size();
336 }
337
338 /**
339 * Estimates a projective 3D transformation using a robust estimator and
340 * the best set of matched 3D planes correspondences found using the robust
341 * estimator.
342 *
343 * @return a projective 3D transformation.
344 * @throws LockedException if robust estimator is locked because an
345 * estimation is already in progress.
346 * @throws NotReadyException if provided input data is not enough to start
347 * the estimation.
348 * @throws RobustEstimatorException if estimation fails for any reason
349 * (i.e. numerical instability, no solution available, etc).
350 */
351 @Override
352 public ProjectiveTransformation3D estimate() throws LockedException, NotReadyException, RobustEstimatorException {
353 if (isLocked()) {
354 throw new LockedException();
355 }
356 if (!isReady()) {
357 throw new NotReadyException();
358 }
359
360 final var innerEstimator = new PROMedSRobustEstimator<>(
361 new PROMedSRobustEstimatorListener<ProjectiveTransformation3D>() {
362
363 // plane to be reused when computing residuals
364 private final Plane testPlane = new Plane();
365
366 @Override
367 public double getThreshold() {
368 return stopThreshold;
369 }
370
371 @Override
372 public int getTotalSamples() {
373 return inputPlanes.size();
374 }
375
376 @Override
377 public int getSubsetSize() {
378 return ProjectiveTransformation3DRobustEstimator.MINIMUM_SIZE;
379 }
380
381 @Override
382 public void estimatePreliminarSolutions(
383 final int[] samplesIndices, final List<ProjectiveTransformation3D> solutions) {
384 final var inputPlane1 = inputPlanes.get(samplesIndices[0]);
385 final var inputPlane2 = inputPlanes.get(samplesIndices[1]);
386 final var inputPlane3 = inputPlanes.get(samplesIndices[2]);
387 final var inputPlane4 = inputPlanes.get(samplesIndices[3]);
388 final var inputPlane5 = inputPlanes.get(samplesIndices[4]);
389
390 final var outputPlane1 = outputPlanes.get(samplesIndices[0]);
391 final var outputPlane2 = outputPlanes.get(samplesIndices[1]);
392 final var outputPlane3 = outputPlanes.get(samplesIndices[2]);
393 final var outputPlane4 = outputPlanes.get(samplesIndices[3]);
394 final var outputPlane5 = outputPlanes.get(samplesIndices[4]);
395
396 try {
397 final var transformation = new ProjectiveTransformation3D(inputPlane1, inputPlane2,
398 inputPlane3, inputPlane4, inputPlane5, outputPlane1, outputPlane2, outputPlane3,
399 outputPlane4, outputPlane5);
400 solutions.add(transformation);
401 } catch (final CoincidentPlanesException e) {
402 // if lines are coincident, no solution is added
403 }
404 }
405
406 @Override
407 public double computeResidual(final ProjectiveTransformation3D currentEstimation, final int i) {
408 final var inputPlane = inputPlanes.get(i);
409 final var outputPlane = outputPlanes.get(i);
410
411 // transform input plane and store result in mTestPlane
412 try {
413 currentEstimation.transform(inputPlane, testPlane);
414
415 return getResidual(outputPlane, testPlane);
416 } catch (final AlgebraException e) {
417 // this happens when internal matrix of affine transformation
418 // cannot be reverse (i.e. transformation is not well-defined,
419 // numerical instabilities, etc.)
420 return Double.MAX_VALUE;
421 }
422 }
423
424 @Override
425 public boolean isReady() {
426 return PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this.isReady();
427 }
428
429 @Override
430 public void onEstimateStart(final RobustEstimator<ProjectiveTransformation3D> estimator) {
431 if (listener != null) {
432 listener.onEstimateStart(
433 PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
434 }
435 }
436
437 @Override
438 public void onEstimateEnd(final RobustEstimator<ProjectiveTransformation3D> estimator) {
439 if (listener != null) {
440 listener.onEstimateEnd(
441 PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this);
442 }
443 }
444
445 @Override
446 public void onEstimateNextIteration(
447 final RobustEstimator<ProjectiveTransformation3D> estimator, final int iteration) {
448 if (listener != null) {
449 listener.onEstimateNextIteration(
450 PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
451 iteration);
452 }
453 }
454
455 @Override
456 public void onEstimateProgressChange(
457 final RobustEstimator<ProjectiveTransformation3D> estimator, final float progress) {
458 if (listener != null) {
459 listener.onEstimateProgressChange(
460 PROMedSPlaneCorrespondenceProjectiveTransformation3DRobustEstimator.this,
461 progress);
462 }
463 }
464
465 @Override
466 public double[] getQualityScores() {
467 return qualityScores;
468 }
469 });
470
471 try {
472 locked = true;
473 inliersData = null;
474 innerEstimator.setConfidence(confidence);
475 innerEstimator.setMaxIterations(maxIterations);
476 innerEstimator.setProgressDelta(progressDelta);
477 final var transformation = innerEstimator.estimate();
478 inliersData = innerEstimator.getInliersData();
479 return attemptRefine(transformation);
480 } catch (final com.irurueta.numerical.LockedException e) {
481 throw new LockedException(e);
482 } catch (final com.irurueta.numerical.NotReadyException e) {
483 throw new NotReadyException(e);
484 } finally {
485 locked = false;
486 }
487 }
488
489 /**
490 * Returns method being used for robust estimation.
491 *
492 * @return method being used for robust estimation.
493 */
494 @Override
495 public RobustEstimatorMethod getMethod() {
496 return RobustEstimatorMethod.PROMEDS;
497 }
498
499 /**
500 * Gets standard deviation used for Levenberg-Marquardt fitting during
501 * refinement.
502 * Returned value gives an indication of how much variance each residual
503 * has.
504 * Typically, this value is related to the threshold used on each robust
505 * estimation, since residuals of found inliers are within the range of such
506 * threshold.
507 *
508 * @return standard deviation used for refinement.
509 */
510 @Override
511 protected double getRefinementStandardDeviation() {
512 final var inliersData = (PROMedSRobustEstimator.PROMedSInliersData) getInliersData();
513
514 // avoid setting a threshold too strict
515 final var threshold = inliersData.getEstimatedThreshold();
516 return Math.max(threshold, stopThreshold);
517 }
518
519 /**
520 * Sets quality scores corresponding to each pair of matched lines.
521 * This method is used internally and does not check whether instance is
522 * locked or not.
523 *
524 * @param qualityScores quality scores to be set.
525 * @throws IllegalArgumentException if provided quality scores length is
526 * smaller than MINIMUM_SIZE.
527 */
528 private void internalSetQualityScores(final double[] qualityScores) {
529 if (qualityScores.length < MINIMUM_SIZE) {
530 throw new IllegalArgumentException();
531 }
532
533 this.qualityScores = qualityScores;
534 }
535
536 }