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
17 package com.irurueta.geometry.estimators;
18
19 import com.irurueta.geometry.CoincidentPointsException;
20 import com.irurueta.geometry.Point3D;
21 import com.irurueta.geometry.Quadric;
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 quadric for provided collection of 3D points using PROMedS
32 * algorithm.
33 */
34 @SuppressWarnings("DuplicatedCode")
35 public class PROMedSQuadricRobustEstimator extends QuadricRobustEstimator {
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-6;
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 pair of matched points.
79 * The larger the score value the better the quality of the matching.
80 */
81 private double[] qualityScores;
82
83 /**
84 * Constructor.
85 */
86 public PROMedSQuadricRobustEstimator() {
87 super();
88 stopThreshold = DEFAULT_STOP_THRESHOLD;
89 }
90
91 /**
92 * Constructor with points.
93 *
94 * @param points 3D points to estimate a quadric.
95 * @throws IllegalArgumentException if provided list of points don't have
96 * a size greater or equal than MINIMUM_SIZE.
97 */
98 public PROMedSQuadricRobustEstimator(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 PROMedSQuadricRobustEstimator(final QuadricRobustEstimatorListener 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 quadric.
121 * @throws IllegalArgumentException if provided list of points don't have
122 * a size greater or equal than MINIMUM_SIZE.
123 */
124 public PROMedSQuadricRobustEstimator(final QuadricRobustEstimatorListener 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. 5 points).
135 */
136 public PROMedSQuadricRobustEstimator(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 quadric.
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 PROMedSQuadricRobustEstimator(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. 9 points).
170 */
171 public PROMedSQuadricRobustEstimator(final QuadricRobustEstimatorListener 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 quadric.
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 PROMedSQuadricRobustEstimator(
190 final QuadricRobustEstimatorListener 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 * As in LMedS, the stop threshold can be used to prevent the PROMedS
207 * algorithm iterating too many times in cases where samples have a very
208 * similar accuracy.
209 * For instance, in cases where proportion of outliers is very small (close
210 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
211 * iterate for a long time trying to find the best solution when indeed
212 * there is no need to do that if a reasonable threshold has already been
213 * reached.
214 * Because of this behaviour the stop threshold can be set to a value much
215 * lower than the one typically used in RANSAC, and yet the algorithm could
216 * still produce even smaller thresholds in estimated results.
217 *
218 * @return stop threshold to stop the algorithm prematurely when a certain
219 * accuracy has been reached.
220 */
221 public double getStopThreshold() {
222 return stopThreshold;
223 }
224
225 /**
226 * Sets threshold to be used to keep the algorithm iterating in case that
227 * best estimated threshold using median of residuals is not small enough.
228 * Once a solution is found that generates a threshold below this value, the
229 * algorithm will stop.
230 * As in LMedS, the stop threshold can be used to prevent the PROMedS
231 * algorithm iterating too many times in cases where samples have a very
232 * similar accuracy.
233 * For instance, in cases where proportion of outliers is very small (close
234 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
235 * iterate for a long time trying to find the best solution when indeed
236 * there is no need to do that if a reasonable threshold has already been
237 * reached.
238 * Because of this behaviour the stop threshold can be set to a value much
239 * lower than the one typically used in RANSAC, and yet the algorithm could
240 * still produce even smaller thresholds in estimated results.
241 *
242 * @param stopThreshold stop threshold to stop the algorithm prematurely
243 * when a certain accuracy has been reached.
244 * @throws IllegalArgumentException if provided value is zero or negative.
245 * @throws LockedException if robust estimator is locked because an
246 * estimation is already in progress.
247 */
248 public void setStopThreshold(final double stopThreshold) throws LockedException {
249 if (isLocked()) {
250 throw new LockedException();
251 }
252 if (stopThreshold <= MIN_STOP_THRESHOLD) {
253 throw new IllegalArgumentException();
254 }
255
256 this.stopThreshold = stopThreshold;
257 }
258
259 /**
260 * Returns quality scores corresponding to each provided point.
261 * The larger the score value the better the quality of the sampled point.
262 *
263 * @return quality scores corresponding to each point.
264 */
265 @Override
266 public double[] getQualityScores() {
267 return qualityScores;
268 }
269
270 /**
271 * Sets quality scores corresponding to each provided point.
272 * The larger the score value the better the quality of the sampled point.
273 *
274 * @param qualityScores quality scores corresponding to each point.
275 * @throws LockedException if robust estimator is locked because an
276 * estimation is already in progress.
277 * @throws IllegalArgumentException if provided quality scores length is
278 * smaller than MINIMUM_SIZE (i.e. 5 samples).
279 */
280 @Override
281 public void setQualityScores(final double[] qualityScores) throws LockedException {
282 if (isLocked()) {
283 throw new LockedException();
284 }
285 internalSetQualityScores(qualityScores);
286 }
287
288 /**
289 * Indicates if estimator is ready to start the quadric estimation.
290 * This is true when input data (i.e. 3D points and quality scores) are
291 * provided and a minimum of MINIMUM_SIZE points are available.
292 *
293 * @return true if estimator is ready, false otherwise.
294 */
295 @Override
296 public boolean isReady() {
297 return super.isReady() && qualityScores != null && qualityScores.length == points.size();
298 }
299
300 /**
301 * Estimates a quadric using a robust estimator and the best set of 3D points
302 * that fit into the locus of the estimated quadric found using the robust
303 * estimator.
304 *
305 * @return a quadric.
306 * @throws LockedException if robust estimator is locked because an
307 * estimation is already in progress.
308 * @throws NotReadyException if provided input data is not enough to start
309 * the estimation.
310 * @throws RobustEstimatorException if estimation fails for any reason
311 * (i.e. numerical instability, no solution available, etc).
312 */
313 @Override
314 public Quadric estimate() throws LockedException, NotReadyException, RobustEstimatorException {
315 if (isLocked()) {
316 throw new LockedException();
317 }
318 if (!isReady()) {
319 throw new NotReadyException();
320 }
321
322 final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Quadric>() {
323
324 @Override
325 public double getThreshold() {
326 return stopThreshold;
327 }
328
329 @Override
330 public int getTotalSamples() {
331 return points.size();
332 }
333
334 @Override
335 public int getSubsetSize() {
336 return QuadricRobustEstimator.MINIMUM_SIZE;
337 }
338
339 @Override
340 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Quadric> solutions) {
341 final var point1 = points.get(samplesIndices[0]);
342 final var point2 = points.get(samplesIndices[1]);
343 final var point3 = points.get(samplesIndices[2]);
344 final var point4 = points.get(samplesIndices[3]);
345 final var point5 = points.get(samplesIndices[4]);
346 final var point6 = points.get(samplesIndices[5]);
347 final var point7 = points.get(samplesIndices[6]);
348 final var point8 = points.get(samplesIndices[7]);
349 final var point9 = points.get(samplesIndices[8]);
350
351 try {
352 final var quadric = new Quadric(point1, point2, point3, point4, point5, point6, point7, point8,
353 point9);
354 solutions.add(quadric);
355 } catch (final CoincidentPointsException e) {
356 // if points are coincident, no solution is added
357 }
358 }
359
360 @Override
361 public double computeResidual(final Quadric currentEstimation, final int i) {
362 return residual(currentEstimation, points.get(i));
363 }
364
365 @Override
366 public boolean isReady() {
367 return PROMedSQuadricRobustEstimator.this.isReady();
368 }
369
370 @Override
371 public void onEstimateStart(final RobustEstimator<Quadric> estimator) {
372 if (listener != null) {
373 listener.onEstimateStart(PROMedSQuadricRobustEstimator.this);
374 }
375 }
376
377 @Override
378 public void onEstimateEnd(final RobustEstimator<Quadric> estimator) {
379 if (listener != null) {
380 listener.onEstimateEnd(PROMedSQuadricRobustEstimator.this);
381 }
382 }
383
384 @Override
385 public void onEstimateNextIteration(final RobustEstimator<Quadric> estimator, final int iteration) {
386 if (listener != null) {
387 listener.onEstimateNextIteration(PROMedSQuadricRobustEstimator.this, iteration);
388 }
389 }
390
391 @Override
392 public void onEstimateProgressChange(final RobustEstimator<Quadric> estimator, final float progress) {
393 if (listener != null) {
394 listener.onEstimateProgressChange(PROMedSQuadricRobustEstimator.this, progress);
395 }
396 }
397
398 @Override
399 public double[] getQualityScores() {
400 return qualityScores;
401 }
402 });
403
404 try {
405 locked = true;
406 innerEstimator.setConfidence(confidence);
407 innerEstimator.setMaxIterations(maxIterations);
408 innerEstimator.setProgressDelta(progressDelta);
409 return innerEstimator.estimate();
410 } catch (final com.irurueta.numerical.LockedException e) {
411 throw new LockedException(e);
412 } catch (final com.irurueta.numerical.NotReadyException e) {
413 throw new NotReadyException(e);
414 } finally {
415 locked = false;
416 }
417 }
418
419 /**
420 * Returns method being used for robust estimation.
421 *
422 * @return method being used for robust estimation.
423 */
424 @Override
425 public RobustEstimatorMethod getMethod() {
426 return RobustEstimatorMethod.PROMEDS;
427 }
428
429 /**
430 * Sets quality scores corresponding to each provided point.
431 * This method is used internally and does not check whether instance is
432 * locked or not.
433 *
434 * @param qualityScores quality scores to be set.
435 * @throws IllegalArgumentException if provided quality scores length is
436 * smaller than MINIMUM_SIZE.
437 */
438 private void internalSetQualityScores(final double[] qualityScores) {
439 if (qualityScores.length < MINIMUM_SIZE) {
440 throw new IllegalArgumentException();
441 }
442
443 this.qualityScores = qualityScores;
444 }
445 }