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.LMedSRobustEstimator;
22 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
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 quadric for provided collection of 3D points using LMedS
31 * algorithm.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public class LMedSQuadricRobustEstimator extends QuadricRobustEstimator {
35
36 /**
37 * Default value to be used for stop threshold. Stop threshold can be used
38 * to keep the algorithm iterating in case that best estimated threshold
39 * using median of residuals is not small enough. Once a solution is found
40 * that generates a threshold below this value, the algorithm will stop.
41 * The stop threshold can be used to prevent the LMedS algorithm iterating
42 * too many times in cases where samples have a very similar accuracy.
43 * For instance, in cases where proportion of outliers is very small (close
44 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
45 * iterate for a long time trying to find the best solution when indeed
46 * there is no need to do that if a reasonable threshold has already been
47 * reached.
48 * Because of this behaviour the stop threshold can be set to a value much
49 * lower than the one typically used in RANSAC, and yet the algorithm could
50 * still produce even smaller thresholds in estimated results.
51 */
52 public static final double DEFAULT_STOP_THRESHOLD = 1e-9;
53
54 /**
55 * Minimum allowed stop threshold value.
56 */
57 public static final double MIN_STOP_THRESHOLD = 0.0;
58
59 /**
60 * Threshold to be used to keep the algorithm iterating in case that best
61 * estimated threshold using median of residuals is not small enough. Once
62 * a solution is found that generates a threshold below this value, the
63 * algorithm will stop.
64 * The stop threshold can be used to prevent the LMedS algorithm iterating
65 * too many times in cases where samples have a very similar accuracy.
66 * For instance, in cases where proportion of outliers is very small (close
67 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
68 * iterate for a long time trying to find the best solution when indeed
69 * there is no need to do that if a reasonable threshold has already been
70 * reached.
71 * Because of this behaviour the stop threshold can be set to a value much
72 * lower than the one typically used in RANSAC, and yet the algorithm could
73 * still produce even smaller thresholds in estimated results.
74 */
75 private double stopThreshold;
76
77 /**
78 * Constructor.
79 */
80 public LMedSQuadricRobustEstimator() {
81 super();
82 stopThreshold = DEFAULT_STOP_THRESHOLD;
83 }
84
85 /**
86 * Constructor with points.
87 *
88 * @param points 3D points to estimate a conic.
89 * @throws IllegalArgumentException if provided list of points don't have
90 * a size greater or equal than MINIMUM_SIZE.
91 */
92 public LMedSQuadricRobustEstimator(final List<Point3D> points) {
93 super(points);
94 stopThreshold = DEFAULT_STOP_THRESHOLD;
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 */
103 public LMedSQuadricRobustEstimator(final QuadricRobustEstimatorListener listener) {
104 super(listener);
105 stopThreshold = DEFAULT_STOP_THRESHOLD;
106 }
107
108 /**
109 * Constructor.
110 *
111 * @param listener listener to be notified of events such as when estimation
112 * starts, ends or its progress significantly changes.
113 * @param points 3D points to estimate a conic.
114 * @throws IllegalArgumentException if provided list of points don't have a
115 * size greater or equal than MINIMUM_SIZE.
116 */
117 public LMedSQuadricRobustEstimator(final QuadricRobustEstimatorListener listener, final List<Point3D> points) {
118 super(listener, points);
119 stopThreshold = DEFAULT_STOP_THRESHOLD;
120 }
121
122 /**
123 * Returns threshold to be used to keep the algorithm iterating in case that
124 * best estimated threshold using median of residuals is not small enough.
125 * Once a solution is found that generates a threshold below this value, the
126 * algorithm will stop.
127 * The stop threshold can be used to prevent the LMedS algorithm iterating
128 * too many times in cases where samples have a very similar accuracy.
129 * For instance, in cases where proportion of outliers is very small (close
130 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
131 * iterate for a long time trying to find the best solution when indeed
132 * there is no need to do that if a reasonable threshold has already been
133 * reached.
134 * Because of this behaviour the stop threshold can be set to a value much
135 * lower than the one typically used in RANSAC, and yet the algorithm could
136 * still produce even smaller thresholds in estimated results.
137 *
138 * @return stop threshold to stop the algorithm prematurely when a certain
139 * accuracy has been reached.
140 */
141 public double getStopThreshold() {
142 return stopThreshold;
143 }
144
145 /**
146 * Sets threshold to be used to keep the algorithm iterating in case that
147 * best estimated threshold using median of residuals is not small enough.
148 * Once a solution is found that generates a threshold below this value, the
149 * algorithm will stop.
150 * The stop threshold can be used to prevent the LMedS algorithm iterating
151 * too many times in cases where samples have a very similar accuracy.
152 * For instance, in cases where proportion of outliers is very small (close
153 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
154 * iterate for a long time trying to find the best solution when indeed
155 * there is no need to do that if a reasonable threshold has already been
156 * reached.
157 * Because of this behaviour the stop threshold can be set to a value much
158 * lower than the one typically used in RANSAC, and yet the algorithm could
159 * still produce even smaller thresholds in estimated results.
160 *
161 * @param stopThreshold stop threshold to stop the algorithm prematurely
162 * when a certain accuracy has been reached.
163 * @throws IllegalArgumentException if provided value is zero or negative.
164 * @throws LockedException if robust estimator is locked because an
165 * estimation is already in progress.
166 */
167 public void setStopThreshold(final double stopThreshold) throws LockedException {
168 if (isLocked()) {
169 throw new LockedException();
170 }
171 if (stopThreshold <= MIN_STOP_THRESHOLD) {
172 throw new IllegalArgumentException();
173 }
174
175 this.stopThreshold = stopThreshold;
176 }
177
178 /**
179 * Estimates a quadric using a robust estimator and the best set of 3D
180 * points that fit into the locus of the estimated quadric found using the
181 * robust estimator.
182 *
183 * @return a quadric.
184 * @throws LockedException if robust estimator is locked because an
185 * estimation is already in progress.
186 * @throws NotReadyException if provided input data is not enough to start
187 * the estimation.
188 * @throws RobustEstimatorException if estimation fails for any reason
189 * (i.e. numerical instability, no solution available, etc).
190 */
191 @Override
192 public Quadric estimate() throws LockedException, NotReadyException, RobustEstimatorException {
193 if (isLocked()) {
194 throw new LockedException();
195 }
196 if (!isReady()) {
197 throw new NotReadyException();
198 }
199
200 final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Quadric>() {
201
202 @Override
203 public int getTotalSamples() {
204 return points.size();
205 }
206
207 @Override
208 public int getSubsetSize() {
209 return QuadricRobustEstimator.MINIMUM_SIZE;
210 }
211
212 @Override
213 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Quadric> solutions) {
214 final var point1 = points.get(samplesIndices[0]);
215 final var point2 = points.get(samplesIndices[1]);
216 final var point3 = points.get(samplesIndices[2]);
217 final var point4 = points.get(samplesIndices[3]);
218 final var point5 = points.get(samplesIndices[4]);
219 final var point6 = points.get(samplesIndices[5]);
220 final var point7 = points.get(samplesIndices[6]);
221 final var point8 = points.get(samplesIndices[7]);
222 final var point9 = points.get(samplesIndices[8]);
223
224 try {
225 final var quadric = new Quadric(point1, point2, point3, point4, point5, point6, point7, point8,
226 point9);
227 solutions.add(quadric);
228 } catch (final CoincidentPointsException e) {
229 // if points are coincident, no solution is added
230 }
231 }
232
233 @Override
234 public double computeResidual(final Quadric currentEstimation, final int i) {
235 return residual(currentEstimation, points.get(i));
236 }
237
238 @Override
239 public boolean isReady() {
240 return LMedSQuadricRobustEstimator.this.isReady();
241 }
242
243 @Override
244 public void onEstimateStart(final RobustEstimator<Quadric> estimator) {
245 if (listener != null) {
246 listener.onEstimateStart(LMedSQuadricRobustEstimator.this);
247 }
248 }
249
250 @Override
251 public void onEstimateEnd(final RobustEstimator<Quadric> estimator) {
252 if (listener != null) {
253 listener.onEstimateEnd(LMedSQuadricRobustEstimator.this);
254 }
255 }
256
257 @Override
258 public void onEstimateNextIteration(final RobustEstimator<Quadric> estimator, final int iteration) {
259 if (listener != null) {
260 listener.onEstimateNextIteration(LMedSQuadricRobustEstimator.this, iteration);
261 }
262 }
263
264 @Override
265 public void onEstimateProgressChange(final RobustEstimator<Quadric> estimator, final float progress) {
266 if (listener != null) {
267 listener.onEstimateProgressChange(LMedSQuadricRobustEstimator.this, progress);
268 }
269 }
270 });
271
272 try {
273 locked = true;
274 innerEstimator.setConfidence(confidence);
275 innerEstimator.setMaxIterations(maxIterations);
276 innerEstimator.setProgressDelta(progressDelta);
277 innerEstimator.setStopThreshold(stopThreshold);
278 return innerEstimator.estimate();
279 } catch (final com.irurueta.numerical.LockedException e) {
280 throw new LockedException(e);
281 } catch (final com.irurueta.numerical.NotReadyException e) {
282 throw new NotReadyException(e);
283 } finally {
284 locked = false;
285 }
286 }
287
288 /**
289 * Returns method being used for robust estimation.
290 *
291 * @return method being used for robust estimation.
292 */
293 @Override
294 public RobustEstimatorMethod getMethod() {
295 return RobustEstimatorMethod.LMEDS;
296 }
297 }