1 /*
2 * Copyright (C) 2016 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.numerical.polynomials.estimators;
17
18 import com.irurueta.numerical.LockedException;
19 import com.irurueta.numerical.NotReadyException;
20 import com.irurueta.numerical.polynomials.Polynomial;
21 import com.irurueta.numerical.robust.PROSACRobustEstimator;
22 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
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.ArrayList;
28 import java.util.List;
29
30 /**
31 * Finds the best polynomial using PROSAC algorithm.
32 */
33 public class PROSACPolynomialRobustEstimator extends PolynomialRobustEstimator {
34
35 /**
36 * Constant defining default threshold to determine whether polynomials are
37 * inliers or not.
38 * Threshold will be used to compare either algebraic or geometric distance
39 * of estimated polynomial respect each provided evaluation.
40 */
41 public static final double DEFAULT_THRESHOLD = 1e-6;
42
43 /**
44 * Minimum value that can be set as threshold.
45 * Threshold must be strictly greater than 0.0.
46 */
47 public static final double MIN_THRESHOLD = 0.0;
48
49 /**
50 * Threshold to determine whether polynomial evaluations are inlers or not
51 * when testing possible estimation solutions
52 */
53 private double threshold;
54
55 /**
56 * Quality scores corresponding to each provided polynomial evaluation.
57 * The larger the score value the better the quality of the sample.
58 */
59 private double[] qualityScores;
60
61 /**
62 * Constructor.
63 */
64 public PROSACPolynomialRobustEstimator() {
65 super();
66 threshold = DEFAULT_THRESHOLD;
67 }
68
69 /**
70 * Constructor.
71 *
72 * @param degree degree of polynomial to be estimated.
73 * @throws IllegalArgumentException if provided degree is less than 1.
74 */
75 public PROSACPolynomialRobustEstimator(final int degree) {
76 super(degree);
77 threshold = DEFAULT_THRESHOLD;
78 }
79
80 /**
81 * Constructor.
82 *
83 * @param evaluations collection of polynomial evaluations.
84 * @throws IllegalArgumentException if provided number of evaluations is
85 * less than the required minimum.
86 */
87 public PROSACPolynomialRobustEstimator(final List<PolynomialEvaluation> evaluations) {
88 super(evaluations);
89 threshold = DEFAULT_THRESHOLD;
90 }
91
92 /**
93 * Constructor.
94 *
95 * @param listener listener to be notified of events such as when estimation
96 * starts, ends or its progress significantly changes.
97 */
98 public PROSACPolynomialRobustEstimator(final PolynomialRobustEstimatorListener listener) {
99 super(listener);
100 threshold = DEFAULT_THRESHOLD;
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param degree degree of polynomial to be estimated.
107 * @param evaluations collection of polynomial evaluations.
108 * @throws IllegalArgumentException if provided degree is less than 1 or if
109 * provided number of evaluations is less than the required minimum for
110 * provided degree.
111 */
112 public PROSACPolynomialRobustEstimator(final int degree, final List<PolynomialEvaluation> evaluations) {
113 super(degree, evaluations);
114 threshold = DEFAULT_THRESHOLD;
115 }
116
117 /**
118 * Constructor.
119 *
120 * @param degree degree of polynomial to be estimated.
121 * @param listener listener to be notified of events such as when estimation
122 * starts, ends or its progress significantly changes.
123 * @throws IllegalArgumentException if provided degree is less than 1.
124 */
125 public PROSACPolynomialRobustEstimator(final int degree, final PolynomialRobustEstimatorListener listener) {
126 super(degree, listener);
127 threshold = DEFAULT_THRESHOLD;
128 }
129
130 /**
131 * Constructor.
132 *
133 * @param evaluations collection of polynomial evaluations.
134 * @param listener listener to be notified of events such as when estimation
135 * starts, ends or its progress significantly changes.
136 * @throws IllegalArgumentException if provided number of evaluations is
137 * less than the required minimum.
138 */
139 public PROSACPolynomialRobustEstimator(
140 final List<PolynomialEvaluation> evaluations, final PolynomialRobustEstimatorListener listener) {
141 super(evaluations, listener);
142 threshold = DEFAULT_THRESHOLD;
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param degree degree of polynomial to be estimated.
149 * @param evaluations collection of polynomial evaluations.
150 * @param listener listener to be notified of events such as when estimation
151 * starts, ends or its progress significantly changes.
152 * @throws IllegalArgumentException if provided degree is less than 1 or if
153 * provided number of evaluations is less than the required minimum for
154 * provided degree.
155 */
156 public PROSACPolynomialRobustEstimator(
157 final int degree, final List<PolynomialEvaluation> evaluations,
158 final PolynomialRobustEstimatorListener listener) {
159 super(degree, evaluations, listener);
160 threshold = DEFAULT_THRESHOLD;
161 }
162
163 /**
164 * Returns threshold to determine whether polynomials are inliers or not
165 * when testing possible estimation solutions.
166 *
167 * @return threshold to determine whether polynomials are inliers or not
168 * when testing possible estimation solutions.
169 */
170 public double getThreshold() {
171 return threshold;
172 }
173
174 /**
175 * Sets threshold to determine whether polynomials are inliers or not when
176 * testing possible estimation solutions.
177 *
178 * @param threshold threshold to determine whether polynomials are inliers
179 * or not when testing possible estimation solutions.
180 * @throws IllegalArgumentException if provided value is equal or less than
181 * zero.
182 * @throws LockedException if robust estimator is locked.
183 */
184 public void setThreshold(final double threshold) throws LockedException {
185 if (isLocked()) {
186 throw new LockedException();
187 }
188 if (threshold <= MIN_THRESHOLD) {
189 throw new IllegalArgumentException();
190 }
191 this.threshold = threshold;
192 }
193
194 /**
195 * Returns quality scores corresponding to each provided point.
196 * The larger the score value the betther the quality of the sampled point
197 *
198 * @return quality scores corresponding to each point
199 */
200 @Override
201 public double[] getQualityScores() {
202 return qualityScores;
203 }
204
205 /**
206 * Sets quality scores corresponding to each provided point.
207 * The larger the score value the better the quality of the sampled point.
208 *
209 * @param qualityScores quality scores corresponding to each point.
210 * @throws LockedException if robust estimator is locked because an
211 * estimation is already in progress.
212 * @throws IllegalArgumentException if provided quality scores length is
213 * smaller than required minimum size.
214 */
215 @Override
216 public void setQualityScores(final double[] qualityScores) throws LockedException {
217 if (isLocked()) {
218 throw new LockedException();
219 }
220 internalSetQualityScores(qualityScores);
221 }
222
223 /**
224 * Indicates if estimator is ready to start the polynomial estimation.
225 * This is true when input data (i.e. polynomial evaluations and quality
226 * scores) are provided and enough data is available.
227 *
228 * @return true if estimator is ready, false otherwise
229 */
230 @Override
231 public boolean isReady() {
232 return super.isReady() && qualityScores != null && qualityScores.length == evaluations.size();
233 }
234
235
236 /**
237 * Estimates polynomial.
238 *
239 * @return estimated polynomial.
240 * @throws LockedException if robust estimator is locked because an
241 * estimation is already in progress.
242 * @throws NotReadyException if provided input data is not enough to start
243 * the estimation.
244 * @throws RobustEstimatorException if estimation fails for any other reason
245 * (i.e. numerical instability, no solution available, etc).
246 */
247 @Override
248 public Polynomial estimate() throws LockedException, NotReadyException, RobustEstimatorException {
249 if (isLocked()) {
250 throw new LockedException();
251 }
252 if (!isReady()) {
253 throw new NotReadyException();
254 }
255
256 final PROSACRobustEstimator<Polynomial> innerEstimator = new PROSACRobustEstimator<>(
257 new PROSACRobustEstimatorListener<>() {
258
259 // subset of evaluations picked on each iteration
260 private final List<PolynomialEvaluation> subsetEvaluations = new ArrayList<>();
261
262 @Override
263 public double getThreshold() {
264 return threshold;
265 }
266
267 @Override
268 public int getTotalSamples() {
269 return evaluations.size();
270 }
271
272 @Override
273 public int getSubsetSize() {
274 return polynomialEstimator.getMinNumberOfEvaluations();
275 }
276
277 @SuppressWarnings("DuplicatedCode")
278 @Override
279 public void estimatePreliminarSolutions(
280 final int[] samplesIndices, final List<Polynomial> solutions) {
281 subsetEvaluations.clear();
282 for (var samplesIndex : samplesIndices) {
283 subsetEvaluations.add(evaluations.get(samplesIndex));
284 }
285
286 try {
287 polynomialEstimator.setLMSESolutionAllowed(false);
288 polynomialEstimator.setEvaluations(subsetEvaluations);
289
290 final var polynomial = polynomialEstimator.estimate();
291 solutions.add(polynomial);
292 } catch (final Exception e) {
293 // if anything fails, no solution is added
294 }
295 }
296
297 @Override
298 public double computeResidual(final Polynomial currentEstimation, final int i) {
299 final var eval = evaluations.get(i);
300 return getDistance(eval, currentEstimation);
301 }
302
303 @Override
304 public boolean isReady() {
305 return PROSACPolynomialRobustEstimator.this.isReady();
306 }
307
308 @Override
309 public void onEstimateStart(final RobustEstimator<Polynomial> estimator) {
310 if (listener != null) {
311 listener.onEstimateStart(PROSACPolynomialRobustEstimator.this);
312 }
313 }
314
315 @Override
316 public void onEstimateEnd(final RobustEstimator<Polynomial> estimator) {
317 if (listener != null) {
318 listener.onEstimateEnd(PROSACPolynomialRobustEstimator.this);
319 }
320 }
321
322 @Override
323 public void onEstimateNextIteration(
324 final RobustEstimator<Polynomial> estimator, final int iteration) {
325 if (listener != null) {
326 listener.onEstimateNextIteration(PROSACPolynomialRobustEstimator.this, iteration);
327 }
328 }
329
330 @Override
331 public void onEstimateProgressChange(
332 final RobustEstimator<Polynomial> estimator, final float progress) {
333 if (listener != null) {
334 listener.onEstimateProgressChange(PROSACPolynomialRobustEstimator.this, progress);
335 }
336 }
337
338 @Override
339 public double[] getQualityScores() {
340 return qualityScores;
341 }
342 });
343
344 try {
345 locked = true;
346 innerEstimator.setConfidence(confidence);
347 innerEstimator.setMaxIterations(maxIterations);
348 innerEstimator.setProgressDelta(progressDelta);
349 return innerEstimator.estimate();
350 } finally {
351 locked = false;
352 }
353 }
354
355 /**
356 * Returns method being used for robust estimation.
357 *
358 * @return method being used for robust estimation.
359 */
360 @Override
361 public RobustEstimatorMethod getMethod() {
362 return RobustEstimatorMethod.PROSAC;
363 }
364
365 /**
366 * Sets quality scores corresponding to each provided polynomial evaluation.
367 * This method is used internally and does not check whether instance is
368 * locked or not
369 *
370 * @param qualityScores quality scores to be set
371 * @throws IllegalArgumentException if provided quality scores length is
372 * smaller than required minimum size.
373 */
374 private void internalSetQualityScores(final double[] qualityScores) {
375 if (qualityScores.length < polynomialEstimator.getMinNumberOfEvaluations()) {
376 throw new IllegalArgumentException();
377 }
378
379 this.qualityScores = qualityScores;
380 }
381 }