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.PROMedSRobustEstimator;
23 import com.irurueta.numerical.robust.PROMedSRobustEstimatorListener;
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 PROMedS algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class PROMedSRobustLateration3DSolver 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 * Quality scores corresponding to each provided sample.
82 * The larger the score value the better the quality of the sample.
83 */
84 private double[] qualityScores;
85
86 /**
87 * Constructor.
88 */
89 public PROMedSRobustLateration3DSolver() {
90 super();
91 }
92
93 /**
94 * Constructor.
95 *
96 * @param listener listener to be notified of events such as when estimation
97 * starts, ends or its progress significantly changes.
98 */
99 public PROMedSRobustLateration3DSolver(final RobustLaterationSolverListener<Point3D> listener) {
100 super(listener);
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param positions known positions of static nodes.
107 * @param distances euclidean distances from static nodes to mobile node to be
108 * estimated.
109 * @throws IllegalArgumentException if either positions or distances are null,
110 * don't have the same length of their length is smaller than required (4 points).
111 */
112 public PROMedSRobustLateration3DSolver(final Point3D[] positions, final double[] distances) {
113 super(positions, distances);
114 }
115
116 /**
117 * Constructor.
118 *
119 * @param positions known positions of static nodes.
120 * @param distances euclidean distances from static nodes to mobile node to be
121 * estimated.
122 * @param distanceStandardDeviations standard deviations of provided measured distances.
123 * @throws IllegalArgumentException if either positions or distances are null,
124 * don't have the same length or their length is smaller than required (4 points).
125 */
126 public PROMedSRobustLateration3DSolver(
127 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations) {
128 super(positions, distances, distanceStandardDeviations);
129 }
130
131 /**
132 * Constructor.
133 *
134 * @param positions known positions of static nodes.
135 * @param distances euclidean distances from static nodes to mobile node.
136 * @param distanceStandardDeviations standard deviations of provided measured distances.
137 * @param listener listener to be notified of events such as when estimation stats,
138 * ends or its progress significantly changes.
139 * @throws IllegalArgumentException if either positions, distances or
140 * standard deviations are null, don't have the same length or their length is
141 * smaller than required (4 points).
142 */
143 public PROMedSRobustLateration3DSolver(
144 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations,
145 final RobustLaterationSolverListener<Point3D> listener) {
146 super(positions, distances, distanceStandardDeviations, listener);
147 }
148
149 /**
150 * Constructor.
151 *
152 * @param positions known positions of static nodes.
153 * @param distances euclidean distances from static nodes to mobile node.
154 * @param listener listener to be notified of events such as when estimation stats,
155 * ends or its progress significantly changes.
156 * @throws IllegalArgumentException if either positions or distances are null,
157 * don't have the same length or their length is smaller than required (4 points).
158 */
159 public PROMedSRobustLateration3DSolver(
160 final Point3D[] positions, final double[] distances,
161 final RobustLaterationSolverListener<Point3D> listener) {
162 super(positions, distances, listener);
163 }
164
165 /**
166 * Constructor.
167 *
168 * @param spheres spheres defining positions and distances.
169 * @throws IllegalArgumentException if spheres is null or if length of spheres array
170 * is less than required (4 points).
171 */
172 public PROMedSRobustLateration3DSolver(final Sphere[] spheres) {
173 super(spheres);
174 }
175
176 /**
177 * Constructor.
178 *
179 * @param spheres spheres defining positions and distances.
180 * @param distanceStandardDeviations standard deviations of provided measured distances.
181 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
182 * than required (4 points) or don't have the same length.
183 */
184 public PROMedSRobustLateration3DSolver(
185 final Sphere[] spheres, final double[] distanceStandardDeviations) {
186 super(spheres, distanceStandardDeviations);
187 }
188
189 /**
190 * Constructor.
191 *
192 * @param spheres spheres defining positions and distances.
193 * @param listener listener to be notified of events such as when estimation starts,
194 * ends or its progress significantly changes.
195 * @throws IllegalArgumentException if spheres is null or if length of spheres array
196 * is less than required (4 points).
197 */
198 public PROMedSRobustLateration3DSolver(
199 final Sphere[] spheres, final RobustLaterationSolverListener<Point3D> listener) {
200 super(spheres, listener);
201 }
202
203 /**
204 * Constructor.
205 *
206 * @param spheres spheres defining positions and distances.
207 * @param distanceStandardDeviations standard deviations of provided measured distances.
208 * @param listener listener to be notified of events such as when estimation starts,
209 * ends or its progress significantly changes.
210 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
211 * than required (4 points) or don't have the same length.
212 */
213 public PROMedSRobustLateration3DSolver(
214 final Sphere[] spheres, final double[] distanceStandardDeviations,
215 final RobustLaterationSolverListener<Point3D> listener) {
216 super(spheres, distanceStandardDeviations, listener);
217 }
218
219 /**
220 * Constructor.
221 *
222 * @param qualityScores quality scores corresponding to each provided
223 * sample. The larger the score value the better
224 * the quality of the sample.
225 * @throws IllegalArgumentException if quality scores is null, length
226 * of quality scores is less than required minimum (4 samples).
227 */
228 public PROMedSRobustLateration3DSolver(final double[] qualityScores) {
229 super();
230 internalSetQualityScores(qualityScores);
231 }
232
233 /**
234 * Constructor.
235 *
236 * @param qualityScores quality scores corresponding to each provided
237 * sample. The larger the score value the better
238 * the quality of the sample.
239 * @param listener listener to be notified of events such as when estimation
240 * starts, ends or its progress significantly changes.
241 * @throws IllegalArgumentException if quality scores is null, length
242 * of quality scores is less than required minimum (4 samples).
243 */
244 public PROMedSRobustLateration3DSolver(
245 final double[] qualityScores, final RobustLaterationSolverListener<Point3D> listener) {
246 super(listener);
247 internalSetQualityScores(qualityScores);
248 }
249
250 /**
251 * Constructor.
252 *
253 * @param qualityScores quality scores corresponding to each provided
254 * sample. The larger the score value the better
255 * the quality of the sample.
256 * @param positions known positions of static nodes.
257 * @param distances euclidean distances from static nodes to mobile node to be
258 * estimated.
259 * @throws IllegalArgumentException if either positions, distances or quality
260 * scores are null, don't have the same length or their length is smaller
261 * than required (4 points).
262 */
263 public PROMedSRobustLateration3DSolver(
264 final double[] qualityScores, final Point3D[] positions, final double[] distances) {
265 super(positions, distances);
266 internalSetQualityScores(qualityScores);
267 }
268
269 /**
270 * Constructor.
271 *
272 * @param qualityScores quality scores corresponding to each provided
273 * sample. The larger the score value the better
274 * the quality of the sample.
275 * @param positions known positions of static nodes.
276 * @param distances euclidean distances from static nodes to mobile node to be
277 * estimated.
278 * @param distanceStandardDeviations standard deviations of provided measured distances.
279 * @throws IllegalArgumentException if either positions, distances, quality scores or
280 * standard deviations are null, don't have the same length or their length is
281 * smaller than required (4 points).
282 */
283 public PROMedSRobustLateration3DSolver(
284 final double[] qualityScores, final Point3D[] positions, final double[] distances,
285 final double[] distanceStandardDeviations) {
286 super(positions, distances, distanceStandardDeviations);
287 internalSetQualityScores(qualityScores);
288 }
289
290 /**
291 * Constructor.
292 *
293 * @param qualityScores quality scores corresponding to each provided
294 * sample. The larger the score value the better
295 * the quality of the sample.
296 * @param positions known positions of static nodes.
297 * @param distances euclidean distances from static nodes to mobile node.
298 * @param distanceStandardDeviations standard deviations of provided measured distances.
299 * @param listener listener to be notified of events such as when estimation starts,
300 * ends or its progress significantly changes.
301 * @throws IllegalArgumentException if either positions, distances or
302 * standard deviations are null, don't have the same length or their length is
303 * smaller than required (4 points).
304 */
305 public PROMedSRobustLateration3DSolver(
306 final double[] qualityScores, final Point3D[] positions, final double[] distances,
307 final double[] distanceStandardDeviations, final RobustLaterationSolverListener<Point3D> listener) {
308 super(positions, distances, distanceStandardDeviations, listener);
309 internalSetQualityScores(qualityScores);
310 }
311
312 /**
313 * Constructor.
314 *
315 * @param qualityScores quality scores corresponding to each provided
316 * sample. The larger the score value the better
317 * the quality of the sample.
318 * @param positions known positions of static nodes.
319 * @param distances euclidean distances from static nodes to mobile node.
320 * @param listener listener to be notified of events such as when
321 * estimation starts, ends or its progress significantly changes.
322 * @throws IllegalArgumentException if either positions, distances,
323 * quality scores or standard deviations are null, don't have the same
324 * length or their length is smaller than required (4 points).
325 */
326 public PROMedSRobustLateration3DSolver(
327 final double[] qualityScores, final Point3D[] positions, final double[] distances,
328 final RobustLaterationSolverListener<Point3D> listener) {
329 super(positions, distances, listener);
330 internalSetQualityScores(qualityScores);
331 }
332
333 /**
334 * Constructor.
335 *
336 * @param qualityScores quality scores corresponding to each provided
337 * sample. The larger the score value the better
338 * the quality of the sample.
339 * @param spheres spheres defining positions and distances.
340 * @throws IllegalArgumentException if either spheres or quality scores
341 * are null don't have the same length or their length is less than
342 * required (4 points).
343 */
344 public PROMedSRobustLateration3DSolver(final double[] qualityScores, final Sphere[] spheres) {
345 super(spheres);
346 internalSetQualityScores(qualityScores);
347 }
348
349 /**
350 * Constructor.
351 *
352 * @param qualityScores quality scores corresponding to each provided
353 * sample. The larger the score value the better
354 * the quality of the sample.
355 * @param spheres spheres defining positions and distances.
356 * @param distanceStandardDeviations standard deviations of provided measured distances.
357 * @throws IllegalArgumentException if either spheres, quality scores or
358 * standard deviations are null, don't have the same length or their
359 * length is less than required (4 points).
360 */
361 public PROMedSRobustLateration3DSolver(
362 final double[] qualityScores, final Sphere[] spheres, final double[] distanceStandardDeviations) {
363 super(spheres, distanceStandardDeviations);
364 internalSetQualityScores(qualityScores);
365 }
366
367 /**
368 * Constructor.
369 *
370 * @param qualityScores quality scores corresponding to each provided
371 * sample. The larger the score value the better
372 * the quality of the sample.
373 * @param spheres spheres defining positions and distances.
374 * @param listener listener to be notified of events such as when estimation starts,
375 * ends or its progress significantly changes.
376 * @throws IllegalArgumentException if either spheres or quality scores
377 * are null, don't have the same length or their length is less than
378 * required (4 points).
379 */
380 public PROMedSRobustLateration3DSolver(
381 final double[] qualityScores, final Sphere[] spheres,
382 final RobustLaterationSolverListener<Point3D> listener) {
383 super(spheres, listener);
384 internalSetQualityScores(qualityScores);
385 }
386
387 /**
388 * Constructor.
389 *
390 * @param qualityScores quality scores corresponding to each provided
391 * sample. The larger the score value the better
392 * the quality of the sample.
393 * @param spheres spheres defining positions and distances.
394 * @param distanceStandardDeviations standard deviations of provided measured distances.
395 * @param listener listener to be notified of events such as when estimation starts,
396 * ends or its progress significantly changes.
397 * @throws IllegalArgumentException if either spheres, quality scores
398 * or standard deviations are null, don't have the same length or their
399 * length is less than required (4 points).
400 */
401 public PROMedSRobustLateration3DSolver(
402 final double[] qualityScores, final Sphere[] spheres, final double[] distanceStandardDeviations,
403 final RobustLaterationSolverListener<Point3D> listener) {
404 super(spheres, distanceStandardDeviations, listener);
405 internalSetQualityScores(qualityScores);
406 }
407
408 /**
409 * Returns threshold to be used to keep the algorithm iterating in case that
410 * best estimated threshold using median of residuals is not small enough.
411 * Once a solution is found that generates a threshold below this value, the
412 * algorithm will stop.
413 * The stop threshold can be used to prevent the LMedS algorithm to iterate
414 * too many times in cases where samples have a very similar accuracy.
415 * For instance, in cases where proportion of outliers is very small (close
416 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
417 * iterate for a long time trying to find the best solution when indeed
418 * there is no need to do that if a reasonable threshold has already been
419 * reached.
420 * Because of this behaviour the stop threshold can be set to a value much
421 * lower than the one typically used in RANSAC, and yet the algorithm could
422 * still produce even smaller thresholds in estimated results.
423 *
424 * @return stop threshold to stop the algorithm prematurely when a certain
425 * accuracy has been reached.
426 */
427 public double getStopThreshold() {
428 return stopThreshold;
429 }
430
431 /**
432 * Sets threshold to be used to keep the algorithm iterating in case that
433 * best estimated threshold using median of residuals is not small enough.
434 * Once a solution is found that generates a threshold below this value,
435 * the algorithm will stop.
436 * The stop threshold can be used to prevent the LMedS algorithm to iterate
437 * too many times in cases where samples have a very similar accuracy.
438 * For instance, in cases where proportion of outliers is very small (close
439 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
440 * iterate for a long time trying to find the best solution when indeed
441 * there is no need to do that if a reasonable threshold has already been
442 * reached.
443 * Because of this behaviour the stop threshold can be set to a value much
444 * lower than the one typically used in RANSAC, and yet the algorithm could
445 * still produce even smaller thresholds in estimated results.
446 *
447 * @param stopThreshold stop threshold to stop the algorithm prematurely
448 * when a certain accuracy has been reached.
449 * @throws IllegalArgumentException if provided value is zero or negative.
450 * @throws LockedException if this solver is locked.
451 */
452 public void setStopThreshold(final double stopThreshold) throws LockedException {
453 if (isLocked()) {
454 throw new LockedException();
455 }
456 if (stopThreshold <= MIN_STOP_THRESHOLD) {
457 throw new IllegalArgumentException();
458 }
459
460 this.stopThreshold = stopThreshold;
461 }
462
463 /**
464 * Returns quality scores corresponding to each pair of
465 * positions and distances (i.e. sample).
466 * The larger the score value the better the quality of the sample.
467 *
468 * @return quality scores corresponding to each sample.
469 */
470 @Override
471 public double[] getQualityScores() {
472 return qualityScores;
473 }
474
475 /**
476 * Sets quality scores corresponding to each pair of positions and
477 * distances (i.e. sample).
478 * The larger the score value the better the quality of the sample.
479 *
480 * @param qualityScores quality scores corresponding to each pair of
481 * matched points.
482 * @throws IllegalArgumentException if provided quality scores length
483 * is smaller than minimum required samples.
484 * @throws LockedException if robust solver is locked because an
485 * estimation is already in progress.
486 */
487 @Override
488 public void setQualityScores(final double[] qualityScores) throws LockedException {
489 if (isLocked()) {
490 throw new LockedException();
491 }
492 internalSetQualityScores(qualityScores);
493 }
494
495 /**
496 * Indicates whether solver is ready to find a solution.
497 *
498 * @return true if solver is ready, false otherwise.
499 */
500 @Override
501 public boolean isReady() {
502 return super.isReady() && qualityScores != null && qualityScores.length == distances.length;
503 }
504
505 /**
506 * Solves the lateration problem.
507 *
508 * @return estimated position.
509 * @throws LockedException if instance is busy solving the lateration problem.
510 * @throws NotReadyException is solver is not ready.
511 * @throws RobustEstimatorException if estimation fails for any reason
512 * (i.e. numerical instability, no solution available, etc).
513 */
514 @Override
515 public Point3D solve() throws LockedException, NotReadyException, RobustEstimatorException {
516 if (isLocked()) {
517 throw new LockedException();
518 }
519 if (!isReady()) {
520 throw new NotReadyException();
521 }
522
523 final var innerEstimator = new PROMedSRobustEstimator<>(new PROMedSRobustEstimatorListener<Point3D>() {
524 @Override
525 public double[] getQualityScores() {
526 return qualityScores;
527 }
528
529 @Override
530 public double getThreshold() {
531 return stopThreshold;
532 }
533
534 @Override
535 public int getTotalSamples() {
536 return distances.length;
537 }
538
539 @Override
540 public int getSubsetSize() {
541 return preliminarySubsetSize;
542 }
543
544 @Override
545 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
546 solvePreliminarySolutions(samplesIndices, solutions);
547 }
548
549 @Override
550 public double computeResidual(final Point3D currentEstimation, final int i) {
551 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
552 }
553
554 @Override
555 public boolean isReady() {
556 return PROMedSRobustLateration3DSolver.this.isReady();
557 }
558
559 @Override
560 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
561 // no action needed
562 }
563
564 @Override
565 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
566 // no action needed
567 }
568
569 @Override
570 public void onEstimateNextIteration(final RobustEstimator<Point3D> estimator, final int iteration) {
571 if (listener != null) {
572 listener.onSolveNextIteration(PROMedSRobustLateration3DSolver.this, iteration);
573 }
574 }
575
576 @Override
577 public void onEstimateProgressChange(final RobustEstimator<Point3D> estimator, final float progress) {
578 if (listener != null) {
579 listener.onSolveProgressChange(PROMedSRobustLateration3DSolver.this, progress);
580 }
581 }
582 });
583
584 try {
585 locked = true;
586
587 if (listener != null) {
588 listener.onSolveStart(this);
589 }
590
591 inliersData = null;
592 innerEstimator.setConfidence(confidence);
593 innerEstimator.setMaxIterations(maxIterations);
594 innerEstimator.setProgressDelta(progressDelta);
595 innerEstimator.setUseInlierThresholds(false);
596 var result = innerEstimator.estimate();
597 inliersData = innerEstimator.getInliersData();
598 result = attemptRefine(result);
599
600 if (listener != null) {
601 listener.onSolveEnd(this);
602 }
603
604 return result;
605
606 } catch (final com.irurueta.numerical.LockedException e) {
607 throw new LockedException(e);
608 } catch (final com.irurueta.numerical.NotReadyException e) {
609 throw new NotReadyException(e);
610 } finally {
611 locked = false;
612 }
613 }
614
615 /**
616 * Returns method being used for robust estimation.
617 *
618 * @return method being used for robust estimation.
619 */
620 @Override
621 public RobustEstimatorMethod getMethod() {
622 return RobustEstimatorMethod.PROMEDS;
623 }
624
625 /**
626 * Sets quality scores corresponding to each provided sample.
627 * This method is used internally and does not check whether instance is
628 * locked or not.
629 *
630 * @param qualityScores quality scores to be set.
631 * @throws IllegalArgumentException if provided quality scores length
632 * is smaller than 3 samples.
633 */
634 private void internalSetQualityScores(final double[] qualityScores) {
635 if (qualityScores == null || qualityScores.length < getMinRequiredPositionsAndDistances()) {
636 throw new IllegalArgumentException();
637 }
638
639 this.qualityScores = qualityScores;
640 }
641 }
642