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.ColinearPointsException;
19 import com.irurueta.geometry.Plane;
20 import com.irurueta.geometry.Point3D;
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 2D plane for provided collection of 3D points using PROSAC
31 * algorithm.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class PROSACPlaneRobustEstimator extends PlaneRobustEstimator {
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 sampled line.
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 PROSACPlaneRobustEstimator() {
67 super();
68 threshold = DEFAULT_THRESHOLD;
69 }
70
71 /**
72 * Constructor with points.
73 *
74 * @param points 3D points to estimate a 3D plane.
75 * @throws IllegalArgumentException if provided list of points doesn't have
76 * a size greater or equal than MINIMUM_SIZE.
77 */
78 public PROSACPlaneRobustEstimator(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 PROSACPlaneRobustEstimator(final PlaneRobustEstimatorListener 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 3D plane.
101 * @throws IllegalArgumentException if provided list of points doesn't have
102 * a size greater or equal than MINIMUM_SIZE.
103 */
104 public PROSACPlaneRobustEstimator(final PlaneRobustEstimatorListener 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. 3 points).
115 */
116 public PROSACPlaneRobustEstimator(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 3D plane.
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 PROSACPlaneRobustEstimator(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 PROSACPlaneRobustEstimator(final PlaneRobustEstimatorListener 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 3D plane.
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 PROSACPlaneRobustEstimator(
170 final PlaneRobustEstimatorListener 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 2D line 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 3D plane using a robust estimator and the best set of 3D
259 * points that pass through the estimated 3D plane (i.e. belong to its
260 * locus).
261 *
262 * @return a 3D plane.
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 Plane 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<Plane>() {
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 PlaneRobustEstimator.MINIMUM_SIZE;
294 }
295
296 @Override
297 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Plane> 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
302 try {
303 final var plane = new Plane(point1, point2, point3);
304 solutions.add(plane);
305 } catch (final ColinearPointsException e) {
306 //if points are coincident, no solution is added
307 }
308 }
309
310 @Override
311 public double computeResidual(final Plane currentEstimation, int i) {
312 return residual(currentEstimation, points.get(i));
313 }
314
315 @Override
316 public boolean isReady() {
317 return PROSACPlaneRobustEstimator.this.isReady();
318 }
319
320 @Override
321 public void onEstimateStart(final RobustEstimator<Plane> estimator) {
322 if (listener != null) {
323 listener.onEstimateStart(PROSACPlaneRobustEstimator.this);
324 }
325 }
326
327 @Override
328 public void onEstimateEnd(final RobustEstimator<Plane> estimator) {
329 if (listener != null) {
330 listener.onEstimateEnd(PROSACPlaneRobustEstimator.this);
331 }
332 }
333
334 @Override
335 public void onEstimateNextIteration(final RobustEstimator<Plane> estimator, final int iteration) {
336 if (listener != null) {
337 listener.onEstimateNextIteration(PROSACPlaneRobustEstimator.this, iteration);
338 }
339 }
340
341 @Override
342 public void onEstimateProgressChange(final RobustEstimator<Plane> estimator, final float progress) {
343 if (listener != null) {
344 listener.onEstimateProgressChange(PROSACPlaneRobustEstimator.this, progress);
345 }
346 }
347
348 @Override
349 public double[] getQualityScores() {
350 return qualityScores;
351 }
352 });
353
354 try {
355 locked = true;
356 innerEstimator.setConfidence(confidence);
357 innerEstimator.setMaxIterations(maxIterations);
358 innerEstimator.setProgressDelta(progressDelta);
359 return innerEstimator.estimate();
360 } catch (final com.irurueta.numerical.LockedException e) {
361 throw new LockedException(e);
362 } catch (final com.irurueta.numerical.NotReadyException e) {
363 throw new NotReadyException(e);
364 } finally {
365 locked = false;
366 }
367 }
368
369 /**
370 * Returns method being used for robust estimation.
371 *
372 * @return method being used for robust estimation.
373 */
374 @Override
375 public RobustEstimatorMethod getMethod() {
376 return RobustEstimatorMethod.PROSAC;
377 }
378
379 /**
380 * Sets quality scores corresponding to each provided point.
381 * This method is used internally and does not check whether instance is
382 * locked or not.
383 *
384 * @param qualityScores quality scores to be set.
385 * @throws IllegalArgumentException if provided quality scores length is
386 * smaller than MINIMUM_SIZE.
387 */
388 private void internalSetQualityScores(final double[] qualityScores) {
389 if (qualityScores.length < MINIMUM_SIZE) {
390 throw new IllegalArgumentException();
391 }
392
393 this.qualityScores = qualityScores;
394 }
395 }