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.Point3D;
19 import com.irurueta.geometry.Sphere;
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 3D
32 * positions and distances among the provided ones using LMedS algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class LMedSRobustLateration3DSolver extends RobustLateration3DSolver {
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 LMedSRobustLateration3DSolver() {
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 LMedSRobustLateration3DSolver(final RobustLaterationSolverListener<Point3D> 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 (4 points).
104 */
105 public LMedSRobustLateration3DSolver(final Point3D[] 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 (4 points).
118 */
119 public LMedSRobustLateration3DSolver(final Point3D[] 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 (4 points).
135 */
136 public LMedSRobustLateration3DSolver(
137 final Point3D[] positions, final double[] distances,
138 final double[] distanceStandardDeviations,
139 final RobustLaterationSolverListener<Point3D> listener) {
140 super(positions, distances, distanceStandardDeviations, listener);
141 }
142
143 /**
144 * Constructor.
145 *
146 * @param positions known positions of static nodes.
147 * @param distances euclidean distances from static nodes to mobile node.
148 * @param listener listener to be notified of events such as when estimation starts,
149 * ends or its progress significantly changes.
150 * @throws IllegalArgumentException if either positions or distances are null,
151 * don't have the same length or their length is smaller than required (4 points).
152 */
153 public LMedSRobustLateration3DSolver(
154 final Point3D[] positions, final double[] distances,
155 final RobustLaterationSolverListener<Point3D> listener) {
156 super(positions, distances, listener);
157 }
158
159 /**
160 * Constructor.
161 *
162 * @param spheres spheres defining positions and distances.
163 * @throws IllegalArgumentException if spheres is null or if length of spheres array
164 * is less than required (4 points).
165 */
166 public LMedSRobustLateration3DSolver(final Sphere[] spheres) {
167 super(spheres);
168 }
169
170 /**
171 * Constructor.
172 *
173 * @param spheres spheres defining positions and distances.
174 * @param distanceStandardDeviations standard deviations of provided measured distances.
175 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
176 * than required (4 points) or don't have the same length.
177 */
178 public LMedSRobustLateration3DSolver(final Sphere[] spheres,
179 final double[] distanceStandardDeviations) {
180 super(spheres, distanceStandardDeviations);
181 }
182
183 /**
184 * Constructor.
185 *
186 * @param spheres spheres defining positions and distances.
187 * @param listener listener to be notified of events such as when estimation starts,
188 * ends or its progress significantly changes.
189 * @throws IllegalArgumentException if spheres is null or if length of spheres array
190 * is less than required (4 points).
191 */
192 public LMedSRobustLateration3DSolver(
193 final Sphere[] spheres, final RobustLaterationSolverListener<Point3D> listener) {
194 super(spheres, listener);
195 }
196
197 /**
198 * Constructor.
199 *
200 * @param spheres spheres defining positions and distances.
201 * @param distanceStandardDeviations standard deviations of provided measured distances.
202 * @param listener listener to be notified of events such as when estimation stats,
203 * ends or its progress significantly changes.
204 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
205 * than required (4 points) or don't have the same length.
206 */
207 public LMedSRobustLateration3DSolver(
208 final Sphere[] spheres, final double[] distanceStandardDeviations,
209 final RobustLaterationSolverListener<Point3D> listener) {
210 super(spheres, distanceStandardDeviations, listener);
211 }
212
213 /**
214 * Returns threshold to be used to keep the algorithm iterating in case that
215 * best estimated threshold using median of residuals is not small enough.
216 * Once a solution is found that generates a threshold below this value, the
217 * algorithm will stop.
218 * The stop threshold can be used to prevent the LMedS algorithm to iterate
219 * too many times in cases where samples have a very similar accuracy.
220 * For instance, in cases where proportion of outliers is very small (close
221 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
222 * iterate for a long time trying to find the best solution when indeed
223 * there is no need to do that if a reasonable threshold has already been
224 * reached.
225 * Because of this behaviour the stop threshold can be set to a value much
226 * lower than the one typically used in RANSAC, and yet the algorithm could
227 * still produce even smaller thresholds in estimated results.
228 *
229 * @return stop threshold to stop the algorithm prematurely when a certain
230 * accuracy has been reached.
231 */
232 public double getStopThreshold() {
233 return stopThreshold;
234 }
235
236 /**
237 * Sets threshold to be used to keep the algorithm iterating in case that
238 * best estimated threshold using median of residuals is not small enough.
239 * Once a solution is found that generates a threshold below this value,
240 * the algorithm will stop.
241 * The stop threshold can be used to prevent the LMedS algorithm to iterate
242 * too many times in cases where samples have a very similar accuracy.
243 * For instance, in cases where proportion of outliers is very small (close
244 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
245 * iterate for a long time trying to find the best solution when indeed
246 * there is no need to do that if a reasonable threshold has already been
247 * reached.
248 * Because of this behaviour the stop threshold can be set to a value much
249 * lower than the one typically used in RANSAC, and yet the algorithm could
250 * still produce even smaller thresholds in estimated results.
251 *
252 * @param stopThreshold stop threshold to stop the algorithm prematurely
253 * when a certain accuracy has been reached.
254 * @throws IllegalArgumentException if provided value is zero or negative.
255 * @throws LockedException if this solver is locked.
256 */
257 public void setStopThreshold(final double stopThreshold) throws LockedException {
258 if (isLocked()) {
259 throw new LockedException();
260 }
261 if (stopThreshold <= MIN_STOP_THRESHOLD) {
262 throw new IllegalArgumentException();
263 }
264
265 this.stopThreshold = stopThreshold;
266 }
267
268 /**
269 * Solves the lateration problem.
270 *
271 * @return estimated position.
272 * @throws LockedException if instance is busy solving the lateration problem.
273 * @throws NotReadyException is solver is not ready.
274 * @throws RobustEstimatorException if estimation fails for any reason
275 * (i.e. numerical instability, no solution available, etc).
276 */
277 @Override
278 public Point3D solve() throws LockedException, NotReadyException, RobustEstimatorException {
279 if (isLocked()) {
280 throw new LockedException();
281 }
282 if (!isReady()) {
283 throw new NotReadyException();
284 }
285
286 final var innerEstimator = new LMedSRobustEstimator<>(new LMedSRobustEstimatorListener<Point3D>() {
287 @Override
288 public int getTotalSamples() {
289 return distances.length;
290 }
291
292 @Override
293 public int getSubsetSize() {
294 return preliminarySubsetSize;
295 }
296
297 @Override
298 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
299 solvePreliminarySolutions(samplesIndices, solutions);
300 }
301
302 @Override
303 public double computeResidual(final Point3D currentEstimation, final int i) {
304 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
305 }
306
307 @Override
308 public boolean isReady() {
309 return LMedSRobustLateration3DSolver.this.isReady();
310 }
311
312 @Override
313 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
314 // no action needed
315 }
316
317 @Override
318 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
319 // no action needed
320 }
321
322 @Override
323 public void onEstimateNextIteration(final RobustEstimator<Point3D> estimator, final int iteration) {
324 if (listener != null) {
325 listener.onSolveNextIteration(LMedSRobustLateration3DSolver.this, iteration);
326 }
327 }
328
329 @Override
330 public void onEstimateProgressChange(final RobustEstimator<Point3D> estimator, final float progress) {
331 if (listener != null) {
332 listener.onSolveProgressChange(LMedSRobustLateration3DSolver.this, progress);
333 }
334 }
335 });
336
337 try {
338 locked = true;
339
340 if (listener != null) {
341 listener.onSolveStart(this);
342 }
343
344 inliersData = null;
345 innerEstimator.setConfidence(confidence);
346 innerEstimator.setMaxIterations(maxIterations);
347 innerEstimator.setProgressDelta(progressDelta);
348 var result = innerEstimator.estimate();
349 inliersData = innerEstimator.getInliersData();
350 result = attemptRefine(result);
351
352 if (listener != null) {
353 listener.onSolveEnd(this);
354 }
355
356 return result;
357
358 } catch (final com.irurueta.numerical.LockedException e) {
359 throw new LockedException(e);
360 } catch (final com.irurueta.numerical.NotReadyException e) {
361 throw new NotReadyException(e);
362 } finally {
363 locked = false;
364 }
365 }
366
367 /**
368 * Returns method being used for robust estimation.
369 *
370 * @return method being used for robust estimation.
371 */
372 @Override
373 public RobustEstimatorMethod getMethod() {
374 return RobustEstimatorMethod.LMEDS;
375 }
376 }