1 /*
2 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.geometry.estimators;
17
18 import com.irurueta.geometry.CoincidentPlanesException;
19 import com.irurueta.geometry.DualQuadric;
20 import com.irurueta.geometry.Plane;
21 import com.irurueta.numerical.robust.PROMedSRobustEstimator;
22 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
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.List;
28
29 /**
30 * Finds the best quadric for provided collection of 3D planes using PROMedS
31 * algorithm.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class PROMedSDualQuadricRobustEstimator extends DualQuadricRobustEstimator {
35 /**
36 * Default value to be used for stop threshold. Stop threshold can be used
37 * to keep the algorithm iterating in case that best estimated threshold
38 * using median of residuals is not small enough. Once a solution is found
39 * that generates a threshold below this value, the algorithm will stop.
40 * The stop threshold can be used to prevent the LMedS algorithm iterating
41 * too many times in cases where samples have a very similar accuracy.
42 * For instance, in cases where proportion of outliers is very small (close
43 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
44 * iterate for a long time trying to find the best solution when indeed
45 * there is no need to do that if a reasonable threshold has already been
46 * reached.
47 * Because of this behaviour the stop threshold can be set to a value much
48 * lower than the one typically used in RANSAC, and yet the algorithm could
49 * still produce even smaller thresholds in estimated results.
50 */
51 public static final double DEFAULT_STOP_THRESHOLD = 1e-9;
52
53 /**
54 * Minimum allowed stop threshold value.
55 */
56 public static final double MIN_STOP_THRESHOLD = 0.0;
57
58 /**
59 * Threshold to be used to keep the algorithm iterating in case that best
60 * estimated threshold using median of residuals is not small enough. Once
61 * a solution is found that generates a threshold below this value, the
62 * algorithm will stop.
63 * The stop threshold can be used to prevent the LMedS algorithm iterating
64 * too many times in cases where samples have a very similar accuracy.
65 * For instance, in cases where proportion of outliers is very small (close
66 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
67 * iterate for a long time trying to find the best solution when indeed
68 * there is no need to do that if a reasonable threshold has already been
69 * reached.
70 * Because of this behaviour the stop threshold can be set to a value much
71 * lower than the one typically used in RANSAC, and yet the algorithm could
72 * still produce even smaller thresholds in estimated results.
73 */
74 private double stopThreshold;
75
76 /**
77 * Quality scores corresponding to each 2D line.
78 * The larger the score value the better the quality of the sample.
79 */
80 private double[] qualityScores;
81
82 /**
83 * Constructor.
84 */
85 public PROMedSDualQuadricRobustEstimator() {
86 super();
87 stopThreshold = DEFAULT_STOP_THRESHOLD;
88 }
89
90 /**
91 * Constructor with planes.
92 *
93 * @param planes 3D planes to estimate a dual quadric.
94 * @throws IllegalArgumentException if provided list of planes don't have
95 * a size greater or equal than MINIMUM_SIZE.
96 */
97 public PROMedSDualQuadricRobustEstimator(final List<Plane> planes) {
98 super(planes);
99 stopThreshold = DEFAULT_STOP_THRESHOLD;
100 }
101
102 /**
103 * Constructor.
104 *
105 * @param listener listener to be notified of events such as when estimation
106 * starts, ends or its progress significantly changes.
107 */
108 public PROMedSDualQuadricRobustEstimator(final DualQuadricRobustEstimatorListener listener) {
109 super(listener);
110 stopThreshold = DEFAULT_STOP_THRESHOLD;
111 }
112
113
114 /**
115 * Constructor.
116 *
117 * @param listener listener to be notified of events such as when estimation
118 * starts, ends or its progress significantly changes.
119 * @param planes 3D planes to estimate a dual quadric.
120 * @throws IllegalArgumentException if provided list of planes don't have
121 * a size greater or equal than MINIMUM_SIZE.
122 */
123 public PROMedSDualQuadricRobustEstimator(
124 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes) {
125 super(listener, planes);
126 stopThreshold = DEFAULT_STOP_THRESHOLD;
127 }
128
129 /**
130 * Constructor.
131 *
132 * @param qualityScores quality scores corresponding to each provided plane.
133 * @throws IllegalArgumentException if provided quality scores length is
134 * smaller than MINIMUM_SIZE (i.e. 9 planes).
135 */
136 public PROMedSDualQuadricRobustEstimator(final double[] qualityScores) {
137 super();
138 stopThreshold = DEFAULT_STOP_THRESHOLD;
139 internalSetQualityScores(qualityScores);
140 }
141
142 /**
143 * Constructor with planes.
144 *
145 * @param planes 3D planes to estimate a dual quadric.
146 * @param qualityScores quality scores corresponding to each provided plane.
147 * @throws IllegalArgumentException if provided list of planes don't have
148 * the same size as the list of provided quality scores, or if their size
149 * is not greater or equal than MINIMUM_SIZE.
150 */
151 public PROMedSDualQuadricRobustEstimator(final List<Plane> planes, final double[] qualityScores) {
152 super(planes);
153
154 if (qualityScores.length != planes.size()) {
155 throw new IllegalArgumentException();
156 }
157
158 stopThreshold = DEFAULT_STOP_THRESHOLD;
159 internalSetQualityScores(qualityScores);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param listener listener to be notified of events such as when estimation
166 * starts, ends or its progress significantly changes.
167 * @param qualityScores quality scores corresponding to each provided plane.
168 * @throws IllegalArgumentException if provided quality scores length is
169 * smaller than MINIMUM_SIZE (i.e. 9 planes).
170 */
171 public PROMedSDualQuadricRobustEstimator(
172 final DualQuadricRobustEstimatorListener listener, final double[] qualityScores) {
173 super(listener);
174 stopThreshold = DEFAULT_STOP_THRESHOLD;
175 internalSetQualityScores(qualityScores);
176 }
177
178
179 /**
180 * Constructor.
181 *
182 * @param listener listener to be notified of events such as when estimation
183 * starts, ends or its progress significantly changes.
184 * @param planes 3D planes to estimate a dual quadric.
185 * @param qualityScores quality scores corresponding to each provided plane.
186 * @throws IllegalArgumentException if provided list of points don't have
187 * the same size as the list of provided quality scores, or it their size
188 * is not greater or equal than MINIMUM_SIZE.
189 */
190 public PROMedSDualQuadricRobustEstimator(
191 final DualQuadricRobustEstimatorListener listener, final List<Plane> planes, final double[] qualityScores) {
192 super(listener, planes);
193
194 if (qualityScores.length != planes.size()) {
195 throw new IllegalArgumentException();
196 }
197
198 stopThreshold = DEFAULT_STOP_THRESHOLD;
199 internalSetQualityScores(qualityScores);
200 }
201
202 /**
203 * Returns threshold to be used to keep the algorithm iterating in case that
204 * best estimated threshold using median of residuals is not small enough.
205 * Once a solution is found that generates a threshold below this value, the
206 * algorithm will stop.
207 * As in LMedS, the stop threshold can be used to prevent the PROMedS
208 * algorithm iterating too many times in cases where samples have a very
209 * similar accuracy.
210 * For instance, in cases where proportion of outliers is very small (close
211 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
212 * iterate for a long time trying to find the best solution when indeed
213 * there is no need to do that if a reasonable threshold has already been
214 * reached.
215 * Because of this behaviour the stop threshold can be set to a value much
216 * lower than the one typically used in RANSAC, and yet the algorithm could
217 * still produce even smaller thresholds in estimated results.
218 *
219 * @return stop threshold to stop the algorithm prematurely when a certain
220 * accuracy has been reached.
221 */
222 public double getStopThreshold() {
223 return stopThreshold;
224 }
225
226 /**
227 * Sets threshold to be used to keep the algorithm iterating in case that
228 * best estimated threshold using median of residuals is not small enough.
229 * Once a solution is found that generates a threshold below this value, the
230 * algorithm will stop.
231 * As in LMedS, the stop threshold can be used to prevent the PROMedS
232 * algorithm iterating too many times in cases where samples have a very
233 * similar accuracy.
234 * For instance, in cases where proportion of outliers is very small (close
235 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
236 * iterate for a long time trying to find the best solution when indeed
237 * there is no need to do that if a reasonable threshold has already been
238 * reached.
239 * Because of this behaviour the stop threshold can be set to a value much
240 * lower than the one typically used in RANSAC, and yet the algorithm could
241 * still produce even smaller thresholds in estimated results.
242 *
243 * @param stopThreshold stop threshold to stop the algorithm prematurely
244 * when a certain accuracy has been reached.
245 * @throws IllegalArgumentException if provided value is zero or negative.
246 * @throws LockedException if robust estimator is locked because an
247 * estimation is already in progress.
248 */
249 public void setStopThreshold(final double stopThreshold) throws LockedException {
250 if (isLocked()) {
251 throw new LockedException();
252 }
253 if (stopThreshold <= MIN_STOP_THRESHOLD) {
254 throw new IllegalArgumentException();
255 }
256
257 this.stopThreshold = stopThreshold;
258 }
259
260 /**
261 * Returns quality scores corresponding to each provided plane.
262 * The larger the score value the better the quality of the sampled plane.
263 *
264 * @return quality scores corresponding to each point.
265 */
266 @Override
267 public double[] getQualityScores() {
268 return qualityScores;
269 }
270
271 /**
272 * Sets quality scores corresponding to each provided plane.
273 * The larger the score value the better the quality of the sampled plane.
274 *
275 * @param qualityScores quality scores corresponding to each plane.
276 * @throws LockedException if robust estimator is locked because an
277 * estimation is already in progress.
278 * @throws IllegalArgumentException if provided quality scores length is
279 * smaller than MINIMUM_SIZE (i.e. 9 samples).
280 */
281 @Override
282 public void setQualityScores(final double[] qualityScores) throws LockedException {
283 if (isLocked()) {
284 throw new LockedException();
285 }
286 internalSetQualityScores(qualityScores);
287 }
288
289 /**
290 * Indicates if estimator is ready to start the quadric estimation.
291 * This is true when input data (i.e. 3D planes and quality scores) are
292 * provided and a minimum of MINIMUM_SIZE planes are available.
293 *
294 * @return true if estimator is ready, false otherwise.
295 */
296 @Override
297 public boolean isReady() {
298 return super.isReady() && qualityScores != null && qualityScores.length == planes.size();
299 }
300
301 /**
302 * Estimates a dual quadric using a robust estimator and the best set of 3D
303 * planes that fit into the locus of the estimated dual quadric found using
304 * the robust estimator.
305 *
306 * @return a dual quadric.
307 * @throws LockedException if robust estimator is locked because an
308 * estimation is already in progress.
309 * @throws NotReadyException if provided input data is not enough to start
310 * the estimation.
311 * @throws RobustEstimatorException if estimation fails for any reason
312 * (i.e. numerical instability, no solution available, etc).
313 */
314 @Override
315 public DualQuadric estimate() throws LockedException, NotReadyException, RobustEstimatorException {
316 if (isLocked()) {
317 throw new LockedException();
318 }
319 if (!isReady()) {
320 throw new NotReadyException();
321 }
322
323 final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<DualQuadric>() {
324
325 @Override
326 public double getThreshold() {
327 return stopThreshold;
328 }
329
330 @Override
331 public int getTotalSamples() {
332 return planes.size();
333 }
334
335 @Override
336 public int getSubsetSize() {
337 return DualQuadricRobustEstimator.MINIMUM_SIZE;
338 }
339
340 @Override
341 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<DualQuadric> solutions) {
342 final var plane1 = planes.get(samplesIndices[0]);
343 final var plane2 = planes.get(samplesIndices[1]);
344 final var plane3 = planes.get(samplesIndices[2]);
345 final var plane4 = planes.get(samplesIndices[3]);
346 final var plane5 = planes.get(samplesIndices[4]);
347 final var plane6 = planes.get(samplesIndices[5]);
348 final var plane7 = planes.get(samplesIndices[6]);
349 final var plane8 = planes.get(samplesIndices[7]);
350 final var plane9 = planes.get(samplesIndices[8]);
351
352 try {
353 final var dualQuadric = new DualQuadric(plane1, plane2, plane3, plane4, plane5, plane6, plane7,
354 plane8, plane9);
355 solutions.add(dualQuadric);
356 } catch (final CoincidentPlanesException e) {
357 // if points are coincident, no solution is added
358 }
359 }
360
361 @Override
362 public double computeResidual(final DualQuadric currentEstimation, final int i) {
363 return residual(currentEstimation, planes.get(i));
364 }
365
366 @Override
367 public boolean isReady() {
368 return PROMedSDualQuadricRobustEstimator.this.isReady();
369 }
370
371 @Override
372 public void onEstimateStart(final RobustEstimator<DualQuadric> estimator) {
373 if (listener != null) {
374 listener.onEstimateStart(PROMedSDualQuadricRobustEstimator.this);
375 }
376 }
377
378 @Override
379 public void onEstimateEnd(final RobustEstimator<DualQuadric> estimator) {
380 if (listener != null) {
381 listener.onEstimateEnd(PROMedSDualQuadricRobustEstimator.this);
382 }
383 }
384
385 @Override
386 public void onEstimateNextIteration(final RobustEstimator<DualQuadric> estimator, final int iteration) {
387 if (listener != null) {
388 listener.onEstimateNextIteration(PROMedSDualQuadricRobustEstimator.this, iteration);
389 }
390 }
391
392 @Override
393 public void onEstimateProgressChange(final RobustEstimator<DualQuadric> estimator, final float progress) {
394 if (listener != null) {
395 listener.onEstimateProgressChange(PROMedSDualQuadricRobustEstimator.this, progress);
396 }
397 }
398
399 @Override
400 public double[] getQualityScores() {
401 return qualityScores;
402 }
403 });
404
405 try {
406 locked = true;
407 innerEstimator.setConfidence(confidence);
408 innerEstimator.setMaxIterations(maxIterations);
409 innerEstimator.setProgressDelta(progressDelta);
410 return innerEstimator.estimate();
411 } catch (final com.irurueta.numerical.LockedException e) {
412 throw new LockedException(e);
413 } catch (final com.irurueta.numerical.NotReadyException e) {
414 throw new NotReadyException(e);
415 } finally {
416 locked = false;
417 }
418 }
419
420 /**
421 * Returns method being used for robust estimation.
422 *
423 * @return method being used for robust estimation.
424 */
425 @Override
426 public RobustEstimatorMethod getMethod() {
427 return RobustEstimatorMethod.PROMEDS;
428 }
429
430 /**
431 * Sets quality scores corresponding to each provided plane.
432 * This method is used internally and does not check whether instance is
433 * locked or not.
434 *
435 * @param qualityScores quality scores to be set.
436 * @throws IllegalArgumentException if provided quality scores length is
437 * smaller than MINIMUM_SIZE.
438 */
439 private void internalSetQualityScores(final double[] qualityScores) {
440 if (qualityScores.length < MINIMUM_SIZE) {
441 throw new IllegalArgumentException();
442 }
443
444 this.qualityScores = qualityScores;
445 }
446 }