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