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