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.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 3D
32 * positions and distances among the provided ones using RANSAC algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class RANSACRobustLateration3DSolver extends RobustLateration3DSolver {
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 RANSACRobustLateration3DSolver() {
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 RANSACRobustLateration3DSolver(final RobustLaterationSolverListener<Point3D> 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 (4 points).
101 */
102 public RANSACRobustLateration3DSolver(final Point3D[] 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 (4 points).
115 */
116 public RANSACRobustLateration3DSolver(
117 final Point3D[] 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 (4 points).
132 */
133 public RANSACRobustLateration3DSolver(
134 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations,
135 final RobustLaterationSolverListener<Point3D> 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 (4 points).
149 */
150 public RANSACRobustLateration3DSolver(
151 final Point3D[] positions, final double[] distances,
152 final RobustLaterationSolverListener<Point3D> listener) {
153 super(positions, distances, listener);
154 }
155
156 /**
157 * Constructor.
158 *
159 * @param spheres spheres defining positions and distances.
160 * @throws IllegalArgumentException if spheres is null or if length of spheres array
161 * is less than required (4 points).
162 */
163 public RANSACRobustLateration3DSolver(final Sphere[] spheres) {
164 super(spheres);
165 }
166
167 /**
168 * Constructor.
169 *
170 * @param spheres spheres defining positions and distances.
171 * @param distanceStandardDeviations standard deviations of provided measured distances.
172 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
173 * than required (4 points) or don't have the same length.
174 */
175 public RANSACRobustLateration3DSolver(final Sphere[] spheres, final double[] distanceStandardDeviations) {
176 super(spheres, distanceStandardDeviations);
177 }
178
179 /**
180 * Constructor.
181 *
182 * @param spheres spheres defining positions and distances.
183 * @param listener listener to be notified of events such as when estimation starts,
184 * ends or its progress significantly changes.
185 * @throws IllegalArgumentException if spheres is null or if length of spheres array
186 * is less than required (4 points).
187 */
188 public RANSACRobustLateration3DSolver(
189 final Sphere[] spheres, final RobustLaterationSolverListener<Point3D> listener) {
190 super(spheres, listener);
191 }
192
193 /**
194 * Constructor.
195 *
196 * @param spheres spheres defining positions and distances.
197 * @param distanceStandardDeviations standard deviations of provided measured distances.
198 * @param listener listener to be notified of events such as when estimation starts,
199 * ends or its progress significantly changes.
200 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
201 * than required (4 points) or don't have the same length.
202 */
203 public RANSACRobustLateration3DSolver(
204 final Sphere[] spheres, final double[] distanceStandardDeviations,
205 final RobustLaterationSolverListener<Point3D> listener) {
206 super(spheres, distanceStandardDeviations, listener);
207 }
208
209 /**
210 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
211 * The threshold refers to the amount of error on distance between estimated position and distances
212 * provided for each sample.
213 *
214 * @return threshold to determine whether samples are inliers or not.
215 */
216 public double getThreshold() {
217 return threshold;
218 }
219
220 /**
221 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
222 * The threshold refers to the amount of error on distance between estimated position and distances
223 * provided for each sample.
224 *
225 * @param threshold threshold to determine whether samples are inliers or not.
226 * @throws IllegalArgumentException if provided value is equal or less than zero.
227 * @throws LockedException if this solver is locked.
228 */
229 public void setThreshold(final double threshold) throws LockedException {
230 if (isLocked()) {
231 throw new LockedException();
232 }
233 if (threshold <= MIN_THRESHOLD) {
234 throw new IllegalArgumentException();
235 }
236 this.threshold = threshold;
237 }
238
239 /**
240 * Indicates whether inliers must be computed and kept.
241 *
242 * @return true if inliers must be computed and kept, false if inliers
243 * only need to be computed but not kept.
244 */
245 public boolean isComputeAndKeepInliersEnabled() {
246 return computeAndKeepInliers;
247 }
248
249 /**
250 * Specifies whether inliers must be computed and kept.
251 *
252 * @param computeAndKeepInliers true if inliers must be computed and kept,
253 * false if inliers only need to be computed but not kept.
254 * @throws LockedException if this solver is locked.
255 */
256 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
257 if (isLocked()) {
258 throw new LockedException();
259 }
260 this.computeAndKeepInliers = computeAndKeepInliers;
261 }
262
263 /**
264 * Indicates whether residuals must be computed and kept.
265 *
266 * @return true if residuals must be computed and kept, false if residuals
267 * only need to be computed but not kept.
268 */
269 public boolean isComputeAndKeepResiduals() {
270 return computeAndKeepResiduals;
271 }
272
273 /**
274 * Specifies whether residuals must be computed and kept.
275 *
276 * @param computeAndKeepResiduals true if residuals must be computed and kept,
277 * false if residuals only need to be computed but not kept.
278 * @throws LockedException if this solver is locked.
279 */
280 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
281 if (isLocked()) {
282 throw new LockedException();
283 }
284 this.computeAndKeepResiduals = computeAndKeepResiduals;
285 }
286
287 /**
288 * Solves the lateration problem.
289 *
290 * @return estimated position.
291 * @throws LockedException if instance is busy solving the lateration problem.
292 * @throws NotReadyException is solver is not ready.
293 * @throws RobustEstimatorException if estimation fails for any reason
294 * (i.e. numerical instability, no solution available, etc).
295 */
296 @Override
297 public Point3D solve() throws LockedException, NotReadyException, RobustEstimatorException {
298 if (isLocked()) {
299 throw new LockedException();
300 }
301 if (!isReady()) {
302 throw new NotReadyException();
303 }
304
305 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Point3D>() {
306
307 @Override
308 public double getThreshold() {
309 return threshold;
310 }
311
312 @Override
313 public int getTotalSamples() {
314 return distances.length;
315 }
316
317 @Override
318 public int getSubsetSize() {
319 return preliminarySubsetSize;
320 }
321
322 @Override
323 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
324 solvePreliminarySolutions(samplesIndices, solutions);
325 }
326
327 @Override
328 public double computeResidual(final Point3D currentEstimation, final int i) {
329 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
330 }
331
332 @Override
333 public boolean isReady() {
334 return RANSACRobustLateration3DSolver.this.isReady();
335 }
336
337 @Override
338 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
339 // no action needed
340 }
341
342 @Override
343 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
344 // no action needed
345 }
346
347 @Override
348 public void onEstimateNextIteration(final RobustEstimator<Point3D> estimator, final int iteration) {
349 if (listener != null) {
350 listener.onSolveNextIteration(RANSACRobustLateration3DSolver.this, iteration);
351 }
352 }
353
354 @Override
355 public void onEstimateProgressChange(final RobustEstimator<Point3D> estimator, final float progress) {
356 if (listener != null) {
357 listener.onSolveProgressChange(RANSACRobustLateration3DSolver.this, progress);
358 }
359 }
360 });
361
362 try {
363 locked = true;
364
365 if (listener != null) {
366 listener.onSolveStart(RANSACRobustLateration3DSolver.this);
367 }
368
369 inliersData = null;
370 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
371 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
372 innerEstimator.setConfidence(confidence);
373 innerEstimator.setMaxIterations(maxIterations);
374 innerEstimator.setProgressDelta(progressDelta);
375 var result = innerEstimator.estimate();
376 inliersData = innerEstimator.getInliersData();
377 result = attemptRefine(result);
378
379 if (listener != null) {
380 listener.onSolveEnd(RANSACRobustLateration3DSolver.this);
381 }
382
383 return result;
384
385 } catch (final com.irurueta.numerical.LockedException e) {
386 throw new LockedException(e);
387 } catch (final com.irurueta.numerical.NotReadyException e) {
388 throw new NotReadyException(e);
389 } finally {
390 locked = false;
391 }
392 }
393
394 /**
395 * Returns method being used for robust estimation.
396 *
397 * @return method being used for robust estimation.
398 */
399 @Override
400 public RobustEstimatorMethod getMethod() {
401 return RobustEstimatorMethod.RANSAC;
402 }
403 }