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.NoIntersectionException;
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 3D point for provided collection of 3D points using PROMedS
31 * algorithm.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class PROMedSPoint3DRobustEstimator extends Point3DRobustEstimator {
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 PROMedSPoint3DRobustEstimator() {
87 super();
88 stopThreshold = DEFAULT_STOP_THRESHOLD;
89 }
90
91 /**
92 * Constructor with planes.
93 *
94 * @param planes 3D planes to estimate a 3D point.
95 * @throws IllegalArgumentException if provided list of planes doesn't have
96 * a size greater or equal than MINIMUM_SIZE.
97 */
98 public PROMedSPoint3DRobustEstimator(final List<Plane> planes) {
99 super(planes);
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 PROMedSPoint3DRobustEstimator(final Point3DRobustEstimatorListener 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 planes 3D lines to estimate a 3D point.
121 * @throws IllegalArgumentException if provided list of planes doesn't have
122 * a size greater or equal than MINIMUM_SIZE.
123 */
124 public PROMedSPoint3DRobustEstimator(final Point3DRobustEstimatorListener 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. 3 planes).
135 */
136 public PROMedSPoint3DRobustEstimator(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 3D point.
146 * @param qualityScores quality scores corresponding to each provided plane.
147 * @throws IllegalArgumentException if provided list of planes doesn'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 PROMedSPoint3DRobustEstimator(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. 3 planes).
170 */
171 public PROMedSPoint3DRobustEstimator(final Point3DRobustEstimatorListener 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 planes 3D planes to estimate a 3D point.
184 * @param qualityScores quality scores corresponding to each provided plane.
185 * @throws IllegalArgumentException if provided list of planes doesn'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 PROMedSPoint3DRobustEstimator(
190 final Point3DRobustEstimatorListener listener, final List<Plane> planes, final double[] qualityScores) {
191 super(listener, planes);
192
193 if (qualityScores.length != planes.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 3D point 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 == planes.size();
296 }
297
298 /**
299 * Estimates a 3D point using a robust estimator and the best set of 3D
300 * planes that intersect into the estimated 3D point.
301 *
302 * @return a 3D point.
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 Point3D 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<Point3D>() {
320
321 @Override
322 public double getThreshold() {
323 return stopThreshold;
324 }
325
326 @Override
327 public int getTotalSamples() {
328 return planes.size();
329 }
330
331 @Override
332 public int getSubsetSize() {
333 return Point3DRobustEstimator.MINIMUM_SIZE;
334 }
335
336 @Override
337 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
338 final var plane1 = planes.get(samplesIndices[0]);
339 final var plane2 = planes.get(samplesIndices[1]);
340 final var plane3 = planes.get(samplesIndices[2]);
341
342 try {
343 final var point = plane1.getIntersection(plane2, plane3);
344 solutions.add(point);
345 } catch (final NoIntersectionException e) {
346 // if points are coincident, no solution is added
347 }
348 }
349
350 @Override
351 public double computeResidual(final Point3D currentEstimation, final int i) {
352 return residual(currentEstimation, planes.get(i));
353 }
354
355 @Override
356 public boolean isReady() {
357 return PROMedSPoint3DRobustEstimator.this.isReady();
358 }
359
360 @Override
361 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
362 if (listener != null) {
363 listener.onEstimateStart(PROMedSPoint3DRobustEstimator.this);
364 }
365 }
366
367 @Override
368 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
369 if (listener != null) {
370 listener.onEstimateEnd(PROMedSPoint3DRobustEstimator.this);
371 }
372 }
373
374 @Override
375 public void onEstimateNextIteration(
376 final RobustEstimator<Point3D> estimator, final int iteration) {
377 if (listener != null) {
378 listener.onEstimateNextIteration(PROMedSPoint3DRobustEstimator.this, iteration);
379 }
380 }
381
382 @Override
383 public void onEstimateProgressChange(
384 final RobustEstimator<Point3D> estimator, final float progress) {
385 if (listener != null) {
386 listener.onEstimateProgressChange(PROMedSPoint3DRobustEstimator.this, progress);
387 }
388 }
389
390 @Override
391 public double[] getQualityScores() {
392 return qualityScores;
393 }
394 });
395
396 try {
397 locked = true;
398 inliersData = null;
399 innerEstimator.setConfidence(confidence);
400 innerEstimator.setMaxIterations(maxIterations);
401 innerEstimator.setProgressDelta(progressDelta);
402 final var result = innerEstimator.estimate();
403 inliersData = innerEstimator.getInliersData();
404 return attemptRefine(result);
405 } catch (final com.irurueta.numerical.LockedException e) {
406 throw new LockedException(e);
407 } catch (final com.irurueta.numerical.NotReadyException e) {
408 throw new NotReadyException(e);
409 } finally {
410 locked = false;
411 }
412 }
413
414 /**
415 * Returns method being used for robust estimation.
416 *
417 * @return method being used for robust estimation.
418 */
419 @Override
420 public RobustEstimatorMethod getMethod() {
421 return RobustEstimatorMethod.PROMEDS;
422 }
423
424 /**
425 * Gets standard deviation used for Levenberg-Marquardt fitting during
426 * refinement.
427 * Returned value gives an indication of how much variance each residual
428 * has.
429 * Typically, this value is related to the threshold used on each robust
430 * estimation, since residuals of found inliers are within the range of
431 * such threshold.
432 *
433 * @return standard deviation used for refinement.
434 */
435 @Override
436 protected double getRefinementStandardDeviation() {
437 final var inliersData = (PROMedSRobustEstimator.PROMedSInliersData) getInliersData();
438 return inliersData.getEstimatedThreshold();
439 }
440
441 /**
442 * Sets quality scores corresponding to each provided point.
443 * This method is used internally and does not check whether instance is
444 * locked or not.
445 *
446 * @param qualityScores quality scores to be set.
447 * @throws IllegalArgumentException if provided quality scores length is
448 * smaller than MINIMUM_SIZE.
449 */
450 private void internalSetQualityScores(final double[] qualityScores) {
451 if (qualityScores.length < MINIMUM_SIZE) {
452 throw new IllegalArgumentException();
453 }
454
455 this.qualityScores = qualityScores;
456 }
457 }