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