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.algebra.Matrix;
19 import com.irurueta.geometry.Point;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.numerical.robust.InliersData;
23 import com.irurueta.numerical.robust.RobustEstimatorException;
24 import com.irurueta.numerical.robust.RobustEstimatorMethod;
25
26 /**
27 * This is an abstract class to robustly solve the lateration problem by finding the best
28 * pairs of positions and distances among the provided ones.
29 * Implementations of this class should be able to detect and discard outliers in order to find
30 * the best solution.
31 *
32 * @param <P> a {@link Point} type.
33 */
34 public abstract class RobustLaterationSolver<P extends Point<?>> {
35
36 /**
37 * Indicates that by default a linear solver is used for preliminary solution estimation.
38 * The result obtained on each preliminary solution might be later refined.
39 */
40 private static final boolean DEFAULT_USE_LINEAR_SOLVER = true;
41
42 /**
43 * Indicates that by default an homogeneous linear solver is used either to estimate preliminary
44 * solutions or an initial solution for preliminary solutions that will be later refined.
45 */
46 public static final boolean DEFAULT_USE_HOMOGENEOUS_LINEAR_SOLVER = false;
47
48 /**
49 * Indicates that by default preliminary solutions are refined.
50 */
51 public static final boolean DEFAULT_REFINE_PRELIMINARY_SOLUTIONS = true;
52
53 /**
54 * Default robust estimator method when none is provided.
55 */
56 public static final RobustEstimatorMethod DEFAULT_ROBUST_METHOD = RobustEstimatorMethod.PROMEDS;
57
58 /**
59 * Indicates that result is refined by default using a non-linear lateration solver
60 * (which uses Levenberg-Marquardt fitter).
61 */
62 public static final boolean DEFAULT_REFINE_RESULT = true;
63
64 /**
65 * Indicates that covariance is kept by default after refining result.
66 */
67 public static final boolean DEFAULT_KEEP_COVARIANCE = true;
68
69 /**
70 * Default amount of progress variation before notifying a change in estimation progress.
71 * By default, this is set to 5%.
72 */
73 public static final float DEFAULT_PROGRESS_DELTA = 0.05f;
74
75 /**
76 * Minimum allowed value for progress delta.
77 */
78 public static final float MIN_PROGRESS_DELTA = 0.0f;
79
80 /**
81 * Maximum allowed value for progress delta.
82 */
83 public static final float MAX_PROGRESS_DELTA = 1.0f;
84
85 /**
86 * Constant defining default confidence of the estimated result, which is
87 * 99%. This means that with a probability of 99% estimation will be
88 * accurate because chosen sub-samples will be inliers.
89 */
90 public static final double DEFAULT_CONFIDENCE = 0.99;
91
92 /**
93 * Default maximum allowed number of iterations.
94 */
95 public static final int DEFAULT_MAX_ITERATIONS = 5000;
96
97 /**
98 * Minimum allowed confidence value.
99 */
100 public static final double MIN_CONFIDENCE = 0.0;
101
102 /**
103 * Maximum allowed confidence value.
104 */
105 public static final double MAX_CONFIDENCE = 1.0;
106
107 /**
108 * Minimum allowed number of iterations.
109 */
110 public static final int MIN_ITERATIONS = 1;
111
112 /**
113 * Minimum allowed distance for a given circle or sphere.
114 */
115 public static final double EPSILON = 1e-7;
116
117 /**
118 * Known positions of static nodes.
119 */
120 protected P[] positions;
121
122 /**
123 * Euclidean distances from static nodes to mobile node.
124 */
125 protected double[] distances;
126
127 /**
128 * Listener to be notified of events such as when solving starts, ends or its
129 * progress significantly changes.
130 */
131 protected RobustLaterationSolverListener<P> listener;
132
133 /**
134 * Initial position to use as a starting point to find a new solution.
135 * This is optional, but if provided, when no linear solvers are used, this is
136 * taken into account. If linear solvers are used, this is ignored.
137 */
138 protected P initialPosition;
139
140 /**
141 * Indicates whether a linear solver is used or not (either homogeneous or inhomogeneous)
142 * for preliminary solutions.
143 */
144 protected boolean useLinearSolver = DEFAULT_USE_LINEAR_SOLVER;
145
146 /**
147 * Indicates whether an homogeneous linear solver is used either to estimate preliminary solutions
148 * or an initial solution for preliminary solutions that will be later refined.
149 */
150 protected boolean useHomogeneousLinearSolver = DEFAULT_USE_HOMOGENEOUS_LINEAR_SOLVER;
151
152 /**
153 * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
154 */
155 protected boolean refinePreliminarySolutions = DEFAULT_REFINE_PRELIMINARY_SOLUTIONS;
156
157 /**
158 * Estimated position.
159 */
160 protected P estimatedPosition;
161
162 /**
163 * Indicates if this instance is locked because lateration is being solved.
164 */
165 protected boolean locked;
166
167 /**
168 * Amount of progress variation before notifying a progress change during estimation.
169 */
170 protected float progressDelta = DEFAULT_PROGRESS_DELTA;
171
172 /**
173 * Amount of confidence expressed as a value between 0.0 and 1.0 (which is equivalent
174 * to 100%). The amount of confidence indicates the probability that the estimated
175 * result is correct. Usually this value will be close to 1.0, but not exactly 1.0.
176 */
177 protected double confidence = DEFAULT_CONFIDENCE;
178
179 /**
180 * Maximum allowed number of iterations. When the maximum number of iterations is
181 * exceeded, result will not be available, however an approximate result will be
182 * available for retrieval.
183 */
184 protected int maxIterations = DEFAULT_MAX_ITERATIONS;
185
186 /**
187 * Data related to inliers found after estimation.
188 */
189 protected InliersData inliersData;
190
191 /**
192 * Indicates whether result must be refined using a non-linear lateration solver over
193 * found inliers.
194 * If true, inliers will be computed and kept in any implementation regardless of the
195 * settings.
196 */
197 protected boolean refineResult = DEFAULT_REFINE_RESULT;
198
199 /**
200 * Indicates whether covariance must be kept after refining result.
201 * This setting is only taken into account if result is refined.
202 */
203 protected boolean keepCovariance = DEFAULT_KEEP_COVARIANCE;
204
205 /**
206 * Estimated covariance of estimated position.
207 * This is only available when result has been refined and covariance is kept.
208 */
209 protected Matrix covariance;
210
211 /**
212 * Standard deviations of provided distances.
213 */
214 protected double[] distanceStandardDeviations;
215
216 /**
217 * Size of subsets to be checked during robust estimation.
218 */
219 protected int preliminarySubsetSize;
220
221 /**
222 * Constructor.
223 */
224 protected RobustLaterationSolver() {
225 }
226
227 /**
228 * Constructor.
229 *
230 * @param listener listener to be notified of events such as when estimation
231 * starts, ends or its progress significantly changes.
232 */
233 protected RobustLaterationSolver(final RobustLaterationSolverListener<P> listener) {
234 this.listener = listener;
235 }
236
237 /**
238 * Constructor.
239 *
240 * @param positions known positions of static nodes.
241 * @param distances euclidean distances from static nodes to mobile node.
242 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
243 * length is smaller than required (3 for 2D points or 4 for 3D points).
244 */
245 protected RobustLaterationSolver(final P[] positions, final double[] distances) {
246 internalSetPositionsAndDistances(positions, distances);
247 }
248
249 /**
250 * Constructor.
251 *
252 * @param positions known positions of static nodes.
253 * @param distances euclidean distances from static nodes to mobile node.
254 * @param distanceStandardDeviations standard deviations of provided measured distances.
255 * @throws IllegalArgumentException if either positions, distances or standard deviations
256 * are null, don't have the same length or their length is smaller than required
257 * (3 for 2D points or 4 for 3D points).
258 */
259 protected RobustLaterationSolver(
260 final P[] positions, final double[] distances, final double[] distanceStandardDeviations) {
261 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
262 }
263
264
265 /**
266 * Constructor.
267 *
268 * @param positions known positions of static nodes.
269 * @param distances euclidean distances from static nodes to mobile node.
270 * @param listener listener to be notified of events such as when estimation starts, ends or its progress
271 * significantly changes.
272 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
273 * length is smaller than required (3 for 2D points or 4 for 3D points).
274 */
275 protected RobustLaterationSolver(
276 final P[] positions, final double[] distances, final RobustLaterationSolverListener<P> listener) {
277 internalSetPositionsAndDistances(positions, distances);
278 this.listener = listener;
279 }
280
281 /**
282 * Constructor.
283 *
284 * @param positions known positions of static nodes.
285 * @param distances euclidean distances from static nodes to mobile node.
286 * @param distanceStandardDeviations standard deviations of provided measured distances.
287 * @param listener listener to be notified of events raised by this instance.
288 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
289 * length is smaller than required (3 for 2D points or 4 for 3D points).
290 */
291 protected RobustLaterationSolver(
292 final P[] positions, final double[] distances, final double[] distanceStandardDeviations,
293 final RobustLaterationSolverListener<P> listener) {
294 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
295 this.listener = listener;
296 }
297
298 /**
299 * Gets listener to be notified of events raised by this instance.
300 *
301 * @return listener to be notified of events raised by this instance.
302 */
303 public RobustLaterationSolverListener<P> getListener() {
304 return listener;
305 }
306
307 /**
308 * Sets listener to be notified of events raised by this instance.
309 *
310 * @param listener listener to be notified of events raised by this instance.
311 * @throws LockedException if instance is busy solving the lateration problem.
312 */
313 public void setListener(final RobustLaterationSolverListener<P> listener) throws LockedException {
314 if (isLocked()) {
315 throw new LockedException();
316 }
317 this.listener = listener;
318 }
319
320 /**
321 * Gets initial position to use as a starting point to find a new solution.
322 * This is optional, but if provided, when no linear solvers are used, this is
323 * taken into account. If linear solvers are used, this is ignored.
324 *
325 * @return an initial position.
326 */
327 public P getInitialPosition() {
328 return initialPosition;
329 }
330
331 /**
332 * Sets initial position to use as a starting point to find a new solution.
333 * This is optional, but if provided, when no linear solvers are used, this is
334 * taken into account. If linear solvers are used, this is ignored.
335 *
336 * @param initialPosition an initial position.
337 * @throws LockedException if instance is busy solving the lateration problem.
338 */
339 public void setInitialPosition(final P initialPosition) throws LockedException {
340 if (isLocked()) {
341 throw new LockedException();
342 }
343
344 this.initialPosition = initialPosition;
345 }
346
347 /**
348 * Indicates whether a linear solver is used or not (either homogeneous or inhomogeneous)
349 * for preliminary solutions.
350 *
351 * @return true if a linear solver is used, false otherwise.
352 */
353 public boolean isLinearSolverUsed() {
354 return useLinearSolver;
355 }
356
357 /**
358 * Specifies whether a linear solver is used or not (either homogeneous or inhomogeneous)
359 * for preliminary solutions.
360 *
361 * @param linearSolverUsed true if a linear solver is used, false otherwise.
362 * @throws LockedException if instance is busy solving the lateration problem.
363 */
364 public void setLinearSolverUsed(final boolean linearSolverUsed) throws LockedException {
365 if (isLocked()) {
366 throw new LockedException();
367 }
368
369 useLinearSolver = linearSolverUsed;
370 }
371
372 /**
373 * Indicates whether an homogeneous linear solver is used either to estimate preliminary solutions
374 * or an initial solution for preliminary solutions that will be later refined.
375 *
376 * @return true if homogeneous linear solver is used, false otherwise.
377 */
378 public boolean isHomogeneousLinearSolverUsed() {
379 return useHomogeneousLinearSolver;
380 }
381
382 /**
383 * Specifies whether an homogeneous linear solver is used either to estimate preliminary solutions
384 * or an initial solution for preliminary solutions that will be later refined.
385 *
386 * @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
387 * otherwise.
388 * @throws LockedException if instance is busy solving the lateration problem.
389 */
390 public void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver) throws LockedException {
391 if (isLocked()) {
392 throw new LockedException();
393 }
394
395 this.useHomogeneousLinearSolver = useHomogeneousLinearSolver;
396 }
397
398 /**
399 * Indicates whether preliminary solutions must be refined after an initial linear solution is found.
400 * If no initial solution is found using a linear solver, a non-linear solver will be
401 * used regardless of this value using an average solution as the initial value to be
402 * refined.
403 *
404 * @return true if preliminary solutions must be refined after an initial linear solution, false
405 * otherwise.
406 */
407 public boolean isPreliminarySolutionRefined() {
408 return refinePreliminarySolutions;
409 }
410
411 /**
412 * Specifies whether preliminary solutions must be refined after an initial linear solution is found.
413 * If no initial solution is found using a linear solver, a non-linear solver will be
414 * used regardless of this value using an average solution as the initial value to be
415 * refined.
416 *
417 * @param preliminarySolutionRefined true if preliminary solutions must be refined after an
418 * initial linear solution, false otherwise.
419 * @throws LockedException if instance is busy solving the lateration problem.
420 */
421 public void setPreliminarySolutionRefined(final boolean preliminarySolutionRefined) throws LockedException {
422 if (isLocked()) {
423 throw new LockedException();
424 }
425
426 refinePreliminarySolutions = preliminarySolutionRefined;
427 }
428
429 /**
430 * Returns boolean indicating if solver is locked because estimation is under
431 * progress.
432 *
433 * @return true if solver is locked, false otherwise.
434 */
435 public boolean isLocked() {
436 return locked;
437 }
438
439 /**
440 * Returns amount of progress variation before notifying a progress change during
441 * estimation.
442 *
443 * @return amount of progress variation before notifying a progress change during
444 * estimation.
445 */
446 public float getProgressDelta() {
447 return progressDelta;
448 }
449
450 /**
451 * Sets amount of progress variation before notifying a progress change during
452 * estimation.
453 *
454 * @param progressDelta amount of progress variation before notifying a progress
455 * change during estimation.
456 * @throws IllegalArgumentException if progress delta is less than zero or greater than 1.
457 * @throws LockedException if this solver is locked because an estimation is being computed.
458 */
459 public void setProgressDelta(final float progressDelta) throws LockedException {
460 if (isLocked()) {
461 throw new LockedException();
462 }
463 if (progressDelta < MIN_PROGRESS_DELTA || progressDelta > MAX_PROGRESS_DELTA) {
464 throw new IllegalArgumentException();
465 }
466 this.progressDelta = progressDelta;
467 }
468
469 /**
470 * Returns amount of confidence expressed as a value between 0.0 and 1.0
471 * (which is equivalent to 100%). The amount of confidence indicates the probability
472 * that the estimated result is correct. Usually this value will be close to 1.0, but
473 * not exactly 1.0.
474 *
475 * @return amount of confidence as a value between 0.0 and 1.0.
476 */
477 public double getConfidence() {
478 return confidence;
479 }
480
481 /**
482 * Sets amount of confidence expressed as a value between 0.0 and 1.0 (which is
483 * equivalent to 100%). The amount of confidence indicates the probability that
484 * the estimated result is correct. Usually this value will be close to 1.0, but
485 * not exactly 1.0.
486 *
487 * @param confidence confidence to be set as a value between 0.0 and 1.0.
488 * @throws IllegalArgumentException if provided value is not between 0.0 and 1.0.
489 * @throws LockedException if solver is locked because an estimation is being computed.
490 */
491 public void setConfidence(final double confidence) throws LockedException {
492 if (isLocked()) {
493 throw new LockedException();
494 }
495 if (confidence < MIN_CONFIDENCE || confidence > MAX_CONFIDENCE) {
496 throw new IllegalArgumentException();
497 }
498 this.confidence = confidence;
499 }
500
501 /**
502 * Returns maximum allowed number of iterations. If maximum allowed number of
503 * iterations is achieved without converging to a result when calling solve(),
504 * a RobustEstimatorException will be raised.
505 *
506 * @return maximum allowed number of iterations.
507 */
508 public int getMaxIterations() {
509 return maxIterations;
510 }
511
512 /**
513 * Sets maximum allowed number of iterations. When the maximum number of iterations
514 * is exceeded, result will not be available, however an approximate result will be
515 * available for retrieval.
516 *
517 * @param maxIterations maximum allowed number of iterations to be set.
518 * @throws IllegalArgumentException if provided value is less than 1.
519 * @throws LockedException if this solver is locked because an estimation is being
520 * computed.
521 */
522 public void setMaxIterations(final int maxIterations) throws LockedException {
523 if (isLocked()) {
524 throw new LockedException();
525 }
526 if (maxIterations < MIN_ITERATIONS) {
527 throw new IllegalArgumentException();
528 }
529 this.maxIterations = maxIterations;
530 }
531
532 /**
533 * Gets data related to inliers found after estimation.
534 *
535 * @return data related to inliers found after estimation.
536 */
537 public InliersData getInliersData() {
538 return inliersData;
539 }
540
541 /**
542 * Indicates whether result must be refined using a non-linear solver over found inliers.
543 *
544 * @return true to refine result, false to simply use result found by robust estimator
545 * without further refining.
546 */
547 public boolean isResultRefined() {
548 return refineResult;
549 }
550
551 /**
552 * Specifies whether result must be refined using a non-linear solver over found inliers.
553 *
554 * @param refineResult true to refine result, false to simply use result found by robust
555 * estimator without further refining.
556 * @throws LockedException if solver is locked.
557 */
558 public void setResultRefined(final boolean refineResult) throws LockedException {
559 if (isLocked()) {
560 throw new LockedException();
561 }
562 this.refineResult = refineResult;
563 }
564
565 /**
566 * Indicates whether covariance must be kept after refining result.
567 * This setting is only taken into account if result is refined.
568 *
569 * @return true if covariance must be kept after refining result, false otherwise.
570 */
571 public boolean isCovarianceKept() {
572 return keepCovariance;
573 }
574
575 /**
576 * Specifies whether covariance must be kept after refining result.
577 * This setting is only taken into account if result is refined.
578 *
579 * @param keepCovariance true if covariance must be kept after refining result,
580 * false otherwise.
581 * @throws LockedException if estimator is locked.
582 */
583 public void setCovarianceKept(final boolean keepCovariance) throws LockedException {
584 if (isLocked()) {
585 throw new LockedException();
586 }
587 this.keepCovariance = keepCovariance;
588 }
589
590 /**
591 * Gets known positions of static nodes.
592 *
593 * @return known positions of static nodes.
594 */
595 public P[] getPositions() {
596 return positions;
597 }
598
599 /**
600 * Gets euclidean distances from static nodes to mobile node.
601 *
602 * @return euclidean distances from static nodes to mobile node.
603 */
604 public double[] getDistances() {
605 return distances;
606 }
607
608 /**
609 * Gets standard deviations of provided distances.
610 * This is used during refinement.
611 *
612 * @return standard deviations of provided distances.
613 */
614 public double[] getDistanceStandardDeviations() {
615 return distanceStandardDeviations;
616 }
617
618 /**
619 * Indicates whether solver is ready to find a solution.
620 *
621 * @return true if solver is ready, false otherwise.
622 */
623 public boolean isReady() {
624 return positions != null && distances != null && positions.length >= getMinRequiredPositionsAndDistances();
625 }
626
627 /**
628 * Returns quality scores corresponding to each pair of
629 * positions and distances (i.e. sample).
630 * The larger the score value the better the quality of the sample.
631 * This implementation always returns null.
632 * Subclasses using quality scores must implement proper behavior.
633 *
634 * @return quality scores corresponding to each sample.
635 */
636 public double[] getQualityScores() {
637 return null;
638 }
639
640 /**
641 * Sets quality scores corresponding to each pair of positions and
642 * distances (i.e. sample).
643 * The larger the score value the better the quality of the sample.
644 * This implementation makes no action.
645 * Subclasses using quality scores must implement proper behaviour.
646 *
647 * @param qualityScores quality scores corresponding to each pair of
648 * matched points.
649 * @throws IllegalArgumentException if provided quality scores length
650 * is smaller than minimum required samples.
651 * @throws LockedException if robust solver is locked because an
652 * estimation is already in progress.
653 */
654 public void setQualityScores(final double[] qualityScores) throws LockedException {
655 }
656
657 /**
658 * Gets estimated covariance of estimated position if available.
659 * This is only available when result has been refined and covariance
660 * is kept.
661 *
662 * @return estimated covariance or null.
663 */
664 public Matrix getCovariance() {
665 return covariance;
666 }
667
668 /**
669 * Sets known positions and Euclidean distances.
670 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
671 *
672 * @param positions known positions of static nodes.
673 * @param distances euclidean distances from static nodes to mobile node.
674 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
675 * length is smaller than required (2 points).
676 * @throws LockedException if instance is busy solving the lateration problem.
677 */
678 public void setPositionsAndDistances(final P[] positions, final double[] distances) throws LockedException {
679 if (isLocked()) {
680 throw new LockedException();
681 }
682 internalSetPositionsAndDistances(positions, distances);
683 }
684
685 /**
686 * Sets known positions, Euclidean distances and the respective standard deviations of
687 * measured distances.
688 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
689 *
690 * @param positions known positions of static nodes.
691 * @param distances euclidean distances from static nodes to mobile node.
692 * @param distanceStandardDeviations standard deviations of provided measured distances.
693 * @throws IllegalArgumentException if either positions, distances or standard deviations
694 * are null, don't have the same length of their length is smaller than required
695 * (2 points).
696 * @throws LockedException if instance is busy solving the lateration problem.
697 */
698 public void setPositionsDistancesAndStandardDeviations(
699 final P[] positions, final double[] distances, final double[] distanceStandardDeviations)
700 throws LockedException {
701 if (isLocked()) {
702 throw new LockedException();
703 }
704 internalSetPositionsDistancesAndStandardDeviations(positions, distances, distanceStandardDeviations);
705 }
706
707 /**
708 * Gets estimated position.
709 *
710 * @return estimated position.
711 */
712 public P getEstimatedPosition() {
713 return estimatedPosition;
714 }
715
716 /**
717 * Gets number of dimensions of provided points.
718 *
719 * @return number of dimensions of provided points.
720 */
721 public abstract int getNumberOfDimensions();
722
723 /**
724 * Minimum required number of positions and distances.
725 * This value will depend on actual implementation and whether we are solving a 2D or 3D problem.
726 *
727 * @return minimum required number of positions and distances.
728 */
729 public abstract int getMinRequiredPositionsAndDistances();
730
731 /**
732 * Gets size of subsets to be checked during robust estimation.
733 * This has to be at least {@link #getMinRequiredPositionsAndDistances()}.
734 *
735 * @return size of subsets to be checked during robust estimation.
736 */
737 public int getPreliminarySubsetSize() {
738 return preliminarySubsetSize;
739 }
740
741 /**
742 * Sets size of subsets to be checked during robust estimation.
743 * This has to be at least {@link #getMinRequiredPositionsAndDistances()}.
744 *
745 * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
746 * @throws LockedException if instance is busy solving the lateration problem.
747 * @throws IllegalArgumentException if provided value is less than {@link #getMinRequiredPositionsAndDistances()}.
748 */
749 public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
750 if (isLocked()) {
751 throw new LockedException();
752 }
753 if (preliminarySubsetSize < getMinRequiredPositionsAndDistances()) {
754 throw new IllegalArgumentException();
755 }
756
757 this.preliminarySubsetSize = preliminarySubsetSize;
758 }
759
760 /**
761 * Solves the lateration problem.
762 *
763 * @return estimated position.
764 * @throws LockedException if instance is busy solving the lateration problem.
765 * @throws NotReadyException if solver is not ready.
766 * @throws RobustEstimatorException if estimation fails for any reason
767 * (i.e. numerical instability, no solution available, etc).
768 */
769 public abstract P solve() throws LockedException, NotReadyException, RobustEstimatorException;
770
771 /**
772 * Returns method being used for robust estimation.
773 *
774 * @return method being used for robust estimation.
775 */
776 public abstract RobustEstimatorMethod getMethod();
777
778 /**
779 * Internally sets known positions and Euclidean distances.
780 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
781 *
782 * @param positions known positions of static nodes.
783 * @param distances euclidean distances from static nodes to mobile node.
784 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
785 * length is smaller than required (2 points).
786 */
787 protected void internalSetPositionsAndDistances(final P[] positions, final double[] distances) {
788 if (positions == null || distances == null) {
789 throw new IllegalArgumentException();
790 }
791
792 if (positions.length < getMinRequiredPositionsAndDistances()) {
793 throw new IllegalArgumentException();
794 }
795
796 if (positions.length != distances.length) {
797 throw new IllegalArgumentException();
798 }
799
800 this.positions = positions;
801 this.distances = distances;
802
803 // fix distances if needed
804 for (var i = 0; i < this.distances.length; i++) {
805 if (this.distances[i] < EPSILON) {
806 this.distances[i] = EPSILON;
807 }
808 }
809 }
810
811 /**
812 * Internally sets known positions, Euclidean distances and the respective standard
813 * deviations of measured distances.
814 * If any distance value is zero or negative, it will be fixed assuming an EPSILON value.
815 *
816 * @param positions known positions of static nodes.
817 * @param distances euclidean distances from static nodes to mobile node.
818 * @param distanceStandardDeviations standard deviations of provided measured distances.
819 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
820 * length is smaller than required (2 points).
821 */
822 protected void internalSetPositionsDistancesAndStandardDeviations(
823 final P[] positions, final double[] distances, final double[] distanceStandardDeviations) {
824 if (distanceStandardDeviations == null || distances == null) {
825 throw new IllegalArgumentException();
826 }
827 if (distances.length != distanceStandardDeviations.length) {
828 throw new IllegalArgumentException();
829 }
830 internalSetPositionsAndDistances(positions, distances);
831 this.distanceStandardDeviations = distanceStandardDeviations;
832 }
833 }