1 /*
2 * Copyright (C) 2018 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.navigation.lateration;
17
18 import com.irurueta.geometry.Circle;
19 import com.irurueta.geometry.Point2D;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.numerical.robust.LMedSRobustEstimator;
23 import com.irurueta.numerical.robust.LMedSRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.List;
29
30 /**
31 * Robustly solves the lateration problem by finding the best pairs of 2D
32 * positions and distances among the provided ones using LMedS algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class LMedSRobustLateration2DSolver extends RobustLateration2DSolver {
37
38 /**
39 * Default value to be used for stop threshold. Stop threshold can be used to
40 * avoid keeping the algorithm unnecessarily iterating in case that best
41 * estimated threshold using median of residuals is not small enough. Once a
42 * solution is found that generates a threshold below this value, the
43 * algorithm will stop.
44 * The stop threshold can be used to prevent the LMedS algorithm iterating
45 * too many times in cases where samples have a very similar accuracy.
46 * For instance, in cases where proportion of outliers is very small (close
47 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
48 * iterate for a long time trying to find the best solution when indeed
49 * there is no need to do that if a reasonable threshold has already been
50 * reached.
51 * Because of this behaviour the stop threshold can be set to a value much
52 * lower than the one typically used in RANSAC, and yet the algorithm could
53 * still produce even smaller thresholds in estimated results.
54 */
55 public static final double DEFAULT_STOP_THRESHOLD = 1e-5;
56
57 /**
58 * Minimum allowed stop threshold value.
59 */
60 public static final double MIN_STOP_THRESHOLD = 0.0;
61
62 /**
63 * Threshold to be used to keep the algorithm iterating in case that best
64 * estimated threshold using median of residuals is not small enough. Once
65 * a solution is found that generates a threshold below this value, the
66 * algorithm will stop.
67 * The stop threshold can be used to prevent the LMedS algorithm iterating
68 * too many times in cases where samples have a very similar accuracy.
69 * For instance, in cases where proportion of outliers is very small (close
70 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
71 * iterate for a long time trying to find the best solution when indeed
72 * there is no need to do that if a reasonable threshold has already been
73 * reached.
74 * Because of this behaviour the stop threshold can be set to a value much
75 * lower than the one typically used in RANSAC, and yet the algorithm could
76 * still produce even smaller thresholds in estimated results.
77 */
78 private double stopThreshold = DEFAULT_STOP_THRESHOLD;
79
80 /**
81 * Constructor.
82 */
83 public LMedSRobustLateration2DSolver() {
84 super();
85 }
86
87 /**
88 * Constructor.
89 *
90 * @param listener listener to be notified of events such as when estimation
91 * starts, ends or its progress significantly changes.
92 */
93 public LMedSRobustLateration2DSolver(final RobustLaterationSolverListener<Point2D> listener) {
94 super(listener);
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param positions known positions of static nodes.
101 * @param distances euclidean distances from static nodes to mobile node to be estimated.
102 * @throws IllegalArgumentException if either positions or distances are null,
103 * don't have the same length or their length is smaller than required (3 points).
104 */
105 public LMedSRobustLateration2DSolver(final Point2D[] positions, final double[] distances) {
106 super(positions, distances);
107 }
108
109 /**
110 * Constructor.
111 *
112 * @param positions known positions of static nodes.
113 * @param distances euclidean distances from static nodes to mobile node to be
114 * estimated.
115 * @param distanceStandardDeviations standard deviations of provided measured distances.
116 * @throws IllegalArgumentException if either positions or distances are null,
117 * don't have the same length or their length is smaller than required (3 points).
118 */
119 public LMedSRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
120 final double[] distanceStandardDeviations) {
121 super(positions, distances, distanceStandardDeviations);
122 }
123
124 /**
125 * Constructor.
126 *
127 * @param positions known positions of static nodes.
128 * @param distances euclidean distances from static nodes to mobile node.
129 * @param distanceStandardDeviations standard deviations of provided measured distances.
130 * @param listener listener to be notified of events such as when estimation starts,
131 * ends or its progress significantly changes.
132 * @throws IllegalArgumentException if either positions, distances or
133 * standard deviations are null, don't have the same length or their length is
134 * smaller than required (3 points).
135 */
136 public LMedSRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
137 final double[] distanceStandardDeviations,
138 final RobustLaterationSolverListener<Point2D> listener) {
139 super(positions, distances, distanceStandardDeviations, listener);
140 }
141
142 /**
143 * Constructor.
144 *
145 * @param positions known positions of static nodes.
146 * @param distances euclidean distances from static nodes to mobile node.
147 * @param listener listener to be notified of events such as when estimation starts,
148 * ends or its progress significantly changes.
149 * @throws IllegalArgumentException if either positions or distances are null,
150 * don't have the same length or their length is smaller than required (3 points).
151 */
152 public LMedSRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
153 final RobustLaterationSolverListener<Point2D> listener) {
154 super(positions, distances, listener);
155 }
156
157 /**
158 * Constructor.
159 *
160 * @param circles circles defining positions and distances.
161 * @throws IllegalArgumentException if circles is null or if length of circles array
162 * is less than required (3 points).
163 */
164 public LMedSRobustLateration2DSolver(final Circle[] circles) {
165 super(circles);
166 }
167
168 /**
169 * Constructor.
170 *
171 * @param circles circles defining positions and distances.
172 * @param distanceStandardDeviations standard deviations of provided measured distances.
173 * @throws IllegalArgumentException if circles is null, length of circles array is less
174 * than required (3 points) or don't have the same length.
175 */
176 public LMedSRobustLateration2DSolver(final Circle[] circles,
177 final double[] distanceStandardDeviations) {
178 super(circles, distanceStandardDeviations);
179 }
180
181 /**
182 * Constructor.
183 *
184 * @param circles circles defining positions and distances.
185 * @param listener listener to be notified of events such as when estimation starts,
186 * ends or its progress significantly changes.
187 * @throws IllegalArgumentException if circles is null or if length of circles array
188 * is less than required (3 points).
189 */
190 public LMedSRobustLateration2DSolver(
191 final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener) {
192 super(circles, listener);
193 }
194
195 /**
196 * Constructor.
197 *
198 * @param circles circles defining positions and distances.
199 * @param distanceStandardDeviations standard deviations of provided measured distances.
200 * @param listener listener to be notified of events such as when estimation stats,
201 * ends or its progress significantly changes.
202 * @throws IllegalArgumentException if circles is null, length of circles array is less
203 * than required (3 points) or don't have the same length.
204 */
205 public LMedSRobustLateration2DSolver(final Circle[] circles,
206 final double[] distanceStandardDeviations,
207 final RobustLaterationSolverListener<Point2D> listener) {
208 super(circles, distanceStandardDeviations, listener);
209 }
210
211 /**
212 * Returns threshold to be used to keep the algorithm iterating in case that
213 * best estimated threshold using median of residuals is not small enough.
214 * Once a solution is found that generates a threshold below this value, the
215 * algorithm will stop.
216 * The stop threshold can be used to prevent the LMedS algorithm to iterate
217 * too many times in cases where samples have a very similar accuracy.
218 * For instance, in cases where proportion of outliers is very small (close
219 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
220 * iterate for a long time trying to find the best solution when indeed
221 * there is no need to do that if a reasonable threshold has already been
222 * reached.
223 * Because of this behaviour the stop threshold can be set to a value much
224 * lower than the one typically used in RANSAC, and yet the algorithm could
225 * still produce even smaller thresholds in estimated results.
226 *
227 * @return stop threshold to stop the algorithm prematurely when a certain
228 * accuracy has been reached.
229 */
230 public double getStopThreshold() {
231 return stopThreshold;
232 }
233
234 /**
235 * Sets threshold to be used to keep the algorithm iterating in case that
236 * best estimated threshold using median of residuals is not small enough.
237 * Once a solution is found that generates a threshold below this value,
238 * the algorithm will stop.
239 * The stop threshold can be used to prevent the LMedS algorithm to iterate
240 * too many times in cases where samples have a very similar accuracy.
241 * For instance, in cases where proportion of outliers is very small (close
242 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
243 * iterate for a long time trying to find the best solution when indeed
244 * there is no need to do that if a reasonable threshold has already been
245 * reached.
246 * Because of this behaviour the stop threshold can be set to a value much
247 * lower than the one typically used in RANSAC, and yet the algorithm could
248 * still produce even smaller thresholds in estimated results.
249 *
250 * @param stopThreshold stop threshold to stop the algorithm prematurely
251 * when a certain accuracy has been reached.
252 * @throws IllegalArgumentException if provided value is zero or negative.
253 * @throws LockedException if this solver is locked.
254 */
255 public void setStopThreshold(final double stopThreshold) throws LockedException {
256 if (isLocked()) {
257 throw new LockedException();
258 }
259 if (stopThreshold <= MIN_STOP_THRESHOLD) {
260 throw new IllegalArgumentException();
261 }
262
263 this.stopThreshold = stopThreshold;
264 }
265
266 /**
267 * Solves the lateration problem.
268 *
269 * @return estimated position.
270 * @throws LockedException if instance is busy solving the lateration problem.
271 * @throws NotReadyException is solver is not ready.
272 * @throws RobustEstimatorException if estimation fails for any reason
273 * (i.e. numerical instability, no solution available, etc).
274 */
275 @Override
276 public Point2D solve() 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 LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Point2D>() {
285 @Override
286 public int getTotalSamples() {
287 return distances.length;
288 }
289
290 @Override
291 public int getSubsetSize() {
292 return preliminarySubsetSize;
293 }
294
295 @Override
296 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point2D> solutions) {
297 solvePreliminarySolutions(samplesIndices, solutions);
298 }
299
300 @Override
301 public double computeResidual(final Point2D currentEstimation, final int i) {
302 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
303 }
304
305 @Override
306 public boolean isReady() {
307 return LMedSRobustLateration2DSolver.this.isReady();
308 }
309
310 @Override
311 public void onEstimateStart(final RobustEstimator<Point2D> estimator) {
312 // no action needed
313 }
314
315 @Override
316 public void onEstimateEnd(final RobustEstimator<Point2D> estimator) {
317 // no action needed
318 }
319
320 @Override
321 public void onEstimateNextIteration(final RobustEstimator<Point2D> estimator, final int iteration) {
322 if (listener != null) {
323 listener.onSolveNextIteration(LMedSRobustLateration2DSolver.this, iteration);
324 }
325 }
326
327 @Override
328 public void onEstimateProgressChange(final RobustEstimator<Point2D> estimator, final float progress) {
329 if (listener != null) {
330 listener.onSolveProgressChange(LMedSRobustLateration2DSolver.this, progress);
331 }
332 }
333 });
334
335 try {
336 locked = true;
337
338 if (listener != null) {
339 listener.onSolveStart(this);
340 }
341
342 inliersData = null;
343 innerEstimator.setConfidence(confidence);
344 innerEstimator.setMaxIterations(maxIterations);
345 innerEstimator.setProgressDelta(progressDelta);
346 var result = innerEstimator.estimate();
347 inliersData = innerEstimator.getInliersData();
348 result = attemptRefine(result);
349
350 if (listener != null) {
351 listener.onSolveEnd(this);
352 }
353
354 return result;
355
356 } catch (final com.irurueta.numerical.LockedException e) {
357 throw new LockedException(e);
358 } catch (final com.irurueta.numerical.NotReadyException e) {
359 throw new NotReadyException(e);
360 } finally {
361 locked = false;
362 }
363 }
364
365 /**
366 * Returns method being used for robust estimation.
367 *
368 * @return method being used for robust estimation.
369 */
370 @Override
371 public RobustEstimatorMethod getMethod() {
372 return RobustEstimatorMethod.LMEDS;
373 }
374 }