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