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