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