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