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