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.NoIntersectionException;
19 import com.irurueta.geometry.Plane;
20 import com.irurueta.geometry.Point3D;
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 3D point for provided collection of 3D planes using LMedS
31 * algorithm
32 */
33 public class LMedSPoint3DRobustEstimator extends Point3DRobustEstimator {
34
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-3;
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 LMedSPoint3DRobustEstimator() {
80 super();
81 stopThreshold = DEFAULT_STOP_THRESHOLD;
82 }
83
84 /**
85 * Constructor with planes.
86 *
87 * @param planes 3D planes to estimate a 3D point.
88 * @throws IllegalArgumentException if provided list of planes doesn't have
89 * a size greater or equal than MINIMUM_SIZE.
90 */
91 public LMedSPoint3DRobustEstimator(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 LMedSPoint3DRobustEstimator(final Point3DRobustEstimatorListener 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 3D point.
114 * @throws IllegalArgumentException if provided list of planes doesn't have
115 * a size greater or equal than MINIMUM_SIZE.
116 */
117 public LMedSPoint3DRobustEstimator(final Point3DRobustEstimatorListener listener, final List<Plane> planes) {
118 super(listener, planes);
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 /**
180 * Estimates a 3D point using a robust estimator and the best set of 3D
181 * planes that intersect into the estimated 3D point.
182 *
183 * @return a 3D point.
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 Point3D 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<Point3D>() {
201
202 @Override
203 public int getTotalSamples() {
204 return planes.size();
205 }
206
207 @Override
208 public int getSubsetSize() {
209 return Point3DRobustEstimator.MINIMUM_SIZE;
210 }
211
212 @SuppressWarnings("DuplicatedCode")
213 @Override
214 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
215 final var plane1 = planes.get(samplesIndices[0]);
216 final var plane2 = planes.get(samplesIndices[1]);
217 final var plane3 = planes.get(samplesIndices[2]);
218
219 try {
220 final var point = plane1.getIntersection(plane2, plane3);
221 solutions.add(point);
222 } catch (final NoIntersectionException e) {
223 // if points are coincident, no solution is added
224 }
225 }
226
227 @Override
228 public double computeResidual(final Point3D currentEstimation, final int i) {
229 return residual(currentEstimation, planes.get(i));
230 }
231
232 @Override
233 public boolean isReady() {
234 return LMedSPoint3DRobustEstimator.this.isReady();
235 }
236
237 @Override
238 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
239 if (listener != null) {
240 listener.onEstimateStart(LMedSPoint3DRobustEstimator.this);
241 }
242 }
243
244 @Override
245 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
246 if (listener != null) {
247 listener.onEstimateEnd(LMedSPoint3DRobustEstimator.this);
248 }
249 }
250
251 @Override
252 public void onEstimateNextIteration(final RobustEstimator<Point3D> estimator, final int iteration) {
253 if (listener != null) {
254 listener.onEstimateNextIteration(LMedSPoint3DRobustEstimator.this, iteration);
255 }
256 }
257
258 @Override
259 public void onEstimateProgressChange(final RobustEstimator<Point3D> estimator, final float progress) {
260 if (listener != null) {
261 listener.onEstimateProgressChange(LMedSPoint3DRobustEstimator.this, progress);
262 }
263 }
264 });
265
266 try {
267 locked = true;
268 inliersData = null;
269 innerEstimator.setConfidence(confidence);
270 innerEstimator.setMaxIterations(maxIterations);
271 innerEstimator.setProgressDelta(progressDelta);
272 innerEstimator.setStopThreshold(stopThreshold);
273 final var result = innerEstimator.estimate();
274 inliersData = innerEstimator.getInliersData();
275 return attemptRefine(result);
276 } catch (final com.irurueta.numerical.LockedException e) {
277 throw new LockedException(e);
278 } catch (final com.irurueta.numerical.NotReadyException e) {
279 throw new NotReadyException(e);
280 } finally {
281 locked = false;
282 }
283 }
284
285 /**
286 * Returns method being used for robust estimation.
287 *
288 * @return method being used for robust estimation.
289 */
290 @Override
291 public RobustEstimatorMethod getMethod() {
292 return RobustEstimatorMethod.LMEDS;
293 }
294
295 /**
296 * Gets standard deviation used for Levenberg-Marquardt fitting during
297 * refinement.
298 * Returned value gives an indication of how much variance each residual
299 * has.
300 * Typically, this value is related to the threshold used on each robust
301 * estimation, since residuals of found inliers are within the range of
302 * such threshold.
303 *
304 * @return standard deviation used for refinement.
305 */
306 @Override
307 protected double getRefinementStandardDeviation() {
308 final var inliersData = (LMedSRobustEstimator.LMedSInliersData) getInliersData();
309 return inliersData.getEstimatedThreshold();
310 }
311 }