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