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