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.RANSACRobustEstimator;
23 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
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 RANSAC algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class RANSACRobustLateration2DSolver extends RobustLateration2DSolver {
37
38 /**
39 * Constant defining default threshold to determine whether samples are inliers or not.
40 */
41 public static final double DEFAULT_THRESHOLD = 1e-2;
42
43 /**
44 * Minimum value that can be set as threshold.
45 * Threshold must be strictly greater than 0.0.
46 */
47 public static final double MIN_THRESHOLD = 0.0;
48
49 /**
50 * Indicates that by default inliers will only be computed but not kept.
51 */
52 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
53
54 /**
55 * Indicates that by default residuals will only be computed but not kept.
56 */
57 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
58
59 /**
60 * Threshold to determine whether samples are inliers or not when testing possible solutions.
61 * The threshold refers to the amount of error on distance between estimated position and
62 * distances provided for each sample.
63 */
64 private double threshold = DEFAULT_THRESHOLD;
65
66 /**
67 * Indicates whether inliers must be computed and kept.
68 */
69 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
70
71 /**
72 * Indicates whether residuals must be computed and kept.
73 */
74 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
75
76 /**
77 * Constructor.
78 */
79 public RANSACRobustLateration2DSolver() {
80 super();
81 }
82
83 /**
84 * Constructor.
85 *
86 * @param listener listener to be notified of events such as when estimation
87 * starts, ends or its progress significantly changes.
88 */
89 public RANSACRobustLateration2DSolver(final RobustLaterationSolverListener<Point2D> listener) {
90 super(listener);
91 }
92
93 /**
94 * Constructor.
95 *
96 * @param positions known positions of static nodes.
97 * @param distances euclidean distances from static nodes to mobile node to be
98 * estimated.
99 * @throws IllegalArgumentException if either positions or distances are null,
100 * don't have the same length or their length is smaller than required (3 points).
101 */
102 public RANSACRobustLateration2DSolver(final Point2D[] positions, final double[] distances) {
103 super(positions, distances);
104 }
105
106 /**
107 * Constructor.
108 *
109 * @param positions known positions of static nodes.
110 * @param distances euclidean distances from static nodes to mobile node to be
111 * estimated.
112 * @param distanceStandardDeviations standard deviations of provided measured distances.
113 * @throws IllegalArgumentException if either positions or distances are null,
114 * don't have the same length or their length is smaller than required (3 points).
115 */
116 public RANSACRobustLateration2DSolver(
117 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations) {
118 super(positions, distances, distanceStandardDeviations);
119 }
120
121 /**
122 * Constructor.
123 *
124 * @param positions known positions of static nodes.
125 * @param distances euclidean distances from static nodes to mobile node.
126 * @param distanceStandardDeviations standard deviations of provided measured distances.
127 * @param listener listener to be notified of events such as when estimation starts,
128 * ends or its progress significantly changes.
129 * @throws IllegalArgumentException if either positions, distances or
130 * standard deviations are null, don't have the same length or their length is
131 * smaller than required (3 points).
132 */
133 public RANSACRobustLateration2DSolver(
134 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
135 final RobustLaterationSolverListener<Point2D> listener) {
136 super(positions, distances, distanceStandardDeviations, listener);
137 }
138
139
140 /**
141 * Constructor.
142 *
143 * @param positions known positions of static nodes.
144 * @param distances euclidean distances from static nodes to mobile node.
145 * @param listener listener to be notified of events such as when estimation starts,
146 * ends or its progress significantly changes.
147 * @throws IllegalArgumentException if either positions or distances are null,
148 * don't have the same length or their length is smaller than required (3 points).
149 */
150 public RANSACRobustLateration2DSolver(
151 final Point2D[] positions, final double[] distances,
152 final RobustLaterationSolverListener<Point2D> listener) {
153 super(positions, distances, listener);
154 }
155
156 /**
157 * Constructor.
158 *
159 * @param circles circles defining positions and distances.
160 * @throws IllegalArgumentException if circles is null or if length of circles array
161 * is less than required (3 points).
162 */
163 public RANSACRobustLateration2DSolver(final Circle[] circles) {
164 super(circles);
165 }
166
167 /**
168 * Constructor.
169 *
170 * @param circles circles defining positions and distances.
171 * @param distanceStandardDeviations standard deviations of provided measured distances.
172 * @throws IllegalArgumentException if circles is null, length of circles array is less
173 * than required (3 points) or don't have the same length.
174 */
175 public RANSACRobustLateration2DSolver(
176 final Circle[] circles, final double[] distanceStandardDeviations) {
177 super(circles, distanceStandardDeviations);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param circles circles defining positions and distances.
184 * @param listener listener to be notified of events such as when estimation starts,
185 * ends or its progress significantly changes.
186 * @throws IllegalArgumentException if circles is null or if length of circles array
187 * is less than required (3 points).
188 */
189 public RANSACRobustLateration2DSolver(
190 final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener) {
191 super(circles, listener);
192 }
193
194 /**
195 * Constructor.
196 *
197 * @param circles circles defining positions and distances.
198 * @param distanceStandardDeviations standard deviations of provided measured distances.
199 * @param listener listener to be notified of events such as when estimation starts,
200 * ends or its progress significantly changes.
201 * @throws IllegalArgumentException if circles is null, length of circles array is less
202 * than required (3 points) or don't have the same length.
203 */
204 public RANSACRobustLateration2DSolver(
205 final Circle[] circles, final double[] distanceStandardDeviations,
206 final RobustLaterationSolverListener<Point2D> listener) {
207 super(circles, distanceStandardDeviations, listener);
208 }
209
210 /**
211 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
212 * The threshold refers to the amount of error on distance between estimated position and distances
213 * provided for each sample.
214 *
215 * @return threshold to determine whether samples are inliers or not.
216 */
217 public double getThreshold() {
218 return threshold;
219 }
220
221 /**
222 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
223 * The threshold refers to the amount of error on distance between estimated position and distances
224 * provided for each sample.
225 *
226 * @param threshold threshold to determine whether samples are inliers or not.
227 * @throws IllegalArgumentException if provided value is equal or less than zero.
228 * @throws LockedException if this solver is locked.
229 */
230 public void setThreshold(final double threshold) throws LockedException {
231 if (isLocked()) {
232 throw new LockedException();
233 }
234 if (threshold <= MIN_THRESHOLD) {
235 throw new IllegalArgumentException();
236 }
237 this.threshold = threshold;
238 }
239
240 /**
241 * Indicates whether inliers must be computed and kept.
242 *
243 * @return true if inliers must be computed and kept, false if inliers
244 * only need to be computed but not kept.
245 */
246 public boolean isComputeAndKeepInliersEnabled() {
247 return computeAndKeepInliers;
248 }
249
250 /**
251 * Specifies whether inliers must be computed and kept.
252 *
253 * @param computeAndKeepInliers true if inliers must be computed and kept,
254 * false if inliers only need to be computed but not kept.
255 * @throws LockedException if this solver is locked.
256 */
257 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
258 if (isLocked()) {
259 throw new LockedException();
260 }
261 this.computeAndKeepInliers = computeAndKeepInliers;
262 }
263
264 /**
265 * Indicates whether residuals must be computed and kept.
266 *
267 * @return true if residuals must be computed and kept, false if residuals
268 * only need to be computed but not kept.
269 */
270 public boolean isComputeAndKeepResiduals() {
271 return computeAndKeepResiduals;
272 }
273
274 /**
275 * Specifies whether residuals must be computed and kept.
276 *
277 * @param computeAndKeepResiduals true if residuals must be computed and kept,
278 * false if residuals only need to be computed but not kept.
279 * @throws LockedException if this solver is locked.
280 */
281 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
282 if (isLocked()) {
283 throw new LockedException();
284 }
285 this.computeAndKeepResiduals = computeAndKeepResiduals;
286 }
287
288 /**
289 * Solves the lateration problem.
290 *
291 * @return estimated position.
292 * @throws LockedException if instance is busy solving the lateration problem.
293 * @throws NotReadyException is solver is not ready.
294 * @throws RobustEstimatorException if estimation fails for any reason
295 * (i.e. numerical instability, no solution available, etc).
296 */
297 @Override
298 public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {
299 if (isLocked()) {
300 throw new LockedException();
301 }
302 if (!isReady()) {
303 throw new NotReadyException();
304 }
305
306 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Point2D>() {
307
308 @Override
309 public double getThreshold() {
310 return threshold;
311 }
312
313 @Override
314 public int getTotalSamples() {
315 return distances.length;
316 }
317
318 @Override
319 public int getSubsetSize() {
320 return preliminarySubsetSize;
321 }
322
323 @Override
324 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point2D> solutions) {
325 solvePreliminarySolutions(samplesIndices, solutions);
326 }
327
328 @Override
329 public double computeResidual(
330 final Point2D currentEstimation, final int i) {
331 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
332 }
333
334 @Override
335 public boolean isReady() {
336 return RANSACRobustLateration2DSolver.this.isReady();
337 }
338
339 @Override
340 public void onEstimateStart(final RobustEstimator<Point2D> estimator) {
341 // no action needed
342 }
343
344 @Override
345 public void onEstimateEnd(final RobustEstimator<Point2D> estimator) {
346 // no action needed
347 }
348
349 @Override
350 public void onEstimateNextIteration(final RobustEstimator<Point2D> estimator, final int iteration) {
351 if (listener != null) {
352 listener.onSolveNextIteration(RANSACRobustLateration2DSolver.this, iteration);
353 }
354 }
355
356 @Override
357 public void onEstimateProgressChange(final RobustEstimator<Point2D> estimator, final float progress) {
358 if (listener != null) {
359 listener.onSolveProgressChange(RANSACRobustLateration2DSolver.this, progress);
360 }
361 }
362 });
363
364 try {
365 locked = true;
366
367 if (listener != null) {
368 listener.onSolveStart(this);
369 }
370
371 inliersData = null;
372 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
373 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
374 innerEstimator.setConfidence(confidence);
375 innerEstimator.setMaxIterations(maxIterations);
376 innerEstimator.setProgressDelta(progressDelta);
377 var result = innerEstimator.estimate();
378 inliersData = innerEstimator.getInliersData();
379 result = attemptRefine(result);
380
381 if (listener != null) {
382 listener.onSolveEnd(this);
383 }
384
385 return result;
386
387 } catch (final com.irurueta.numerical.LockedException e) {
388 throw new LockedException(e);
389 } catch (final com.irurueta.numerical.NotReadyException e) {
390 throw new NotReadyException(e);
391 } finally {
392 locked = false;
393 }
394 }
395
396 /**
397 * Returns method being used for robust estimation.
398 *
399 * @return method being used for robust estimation.
400 */
401 @Override
402 public RobustEstimatorMethod getMethod() {
403 return RobustEstimatorMethod.RANSAC;
404 }
405 }