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.NavigationException;
22 import com.irurueta.numerical.robust.RobustEstimatorMethod;
23
24 import java.util.List;
25
26 /**
27 * This is an abstract class to robustly solve the lateration problem by
28 * finding the best pairs of 2D positions and distances among the provided
29 * ones.
30 * Implementations of this class should be able to detect and discard outliers
31 * in order to find the best solution.
32 */
33 @SuppressWarnings("DuplicatedCode")
34 public abstract class RobustLateration2DSolver extends RobustLaterationSolver<Point2D> {
35
36 /**
37 * Linear lateration solver internally used by a robust algorithm.
38 */
39 protected InhomogeneousLinearLeastSquaresLateration2DSolver inhomogeneousLinearSolver;
40
41 /**
42 * Homogeneous linear lateration solver internally used by a robust algorithm.
43 */
44 protected HomogeneousLinearLeastSquaresLateration2DSolver homogeneousLinearSolver;
45
46 /**
47 * Non-linear lateration solver internally used to refine solution
48 * found by robust algorithm.
49 */
50 protected NonLinearLeastSquaresLateration2DSolver nonLinearSolver;
51
52 /**
53 * Positions for linear inner solver used during robust estimation.
54 */
55 protected Point2D[] innerPositions;
56
57 /**
58 * Distances for linear inner solver used during robust estimation.
59 */
60 protected double[] innerDistances;
61
62 /**
63 * Standard deviations for non-linear inner solver used during robust estimation.
64 */
65 protected double[] innerDistanceStandardDeviations;
66
67 /**
68 * Constructor.
69 */
70 protected RobustLateration2DSolver() {
71 init();
72 }
73
74 /**
75 * Constructor.
76 *
77 * @param listener listener to be notified of events such as when estimation
78 * starts, ends or its progress significantly changes.
79 */
80 protected RobustLateration2DSolver(final RobustLaterationSolverListener<Point2D> listener) {
81 super(listener);
82 init();
83 }
84
85 /**
86 * Constructor.
87 *
88 * @param positions known positions of static nodes.
89 * @param distances euclidean distances from static nodes to mobile node to be
90 * estimated.
91 * @throws IllegalArgumentException if either positions or distances are null,
92 * don't have the same length or their length is smaller than required (3 points).
93 */
94 protected RobustLateration2DSolver(final Point2D[] positions, final double[] distances) {
95 super(positions, distances);
96 init();
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 * @param distanceStandardDeviations standard deviations of provided measured distances.
106 * @throws IllegalArgumentException if either positions or distances are null,
107 * don't have the same length or their length is smaller than required (3 points).
108 */
109 protected RobustLateration2DSolver(final Point2D[] positions, final double[] distances,
110 final double[] distanceStandardDeviations) {
111 super(positions, distances, distanceStandardDeviations);
112 init();
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param positions known positions of static nodes.
119 * @param distances euclidean distances from static nodes to mobile node.
120 * @param listener listener to be notified of events such as when estimation starts,
121 * ends or its progress significantly changes.
122 * @throws IllegalArgumentException if either positions or distances are null,
123 * don't have the same length or their length is smaller than required (3 points).
124 */
125 protected RobustLateration2DSolver(final Point2D[] positions, final double[] distances,
126 final RobustLaterationSolverListener<Point2D> listener) {
127 super(positions, distances, listener);
128 init();
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 starts,
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 protected RobustLateration2DSolver(final Point2D[] positions, final double[] distances,
144 final double[] distanceStandardDeviations,
145 final RobustLaterationSolverListener<Point2D> listener) {
146 super(positions, distances, distanceStandardDeviations, listener);
147 init();
148 }
149
150 /**
151 * Constructor.
152 *
153 * @param circles circles defining positions and distances.
154 * @throws IllegalArgumentException if circles is null or if length of circles array
155 * is less than required (3 points).
156 */
157 protected RobustLateration2DSolver(final Circle[] circles) {
158 this();
159 internalSetCircles(circles);
160 }
161
162 /**
163 * Constructor.
164 *
165 * @param circles circles defining positions and distances.
166 * @param distanceStandardDeviations standard deviations of provided measured distances.
167 * @throws IllegalArgumentException if circles is null, length of circles array is less
168 * than required (3 points) or don't have the same length.
169 */
170 protected RobustLateration2DSolver(final Circle[] circles, final double[] distanceStandardDeviations) {
171 this();
172 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
173 }
174
175 /**
176 * Constructor.
177 *
178 * @param circles circles defining positions and distances.
179 * @param listener listener to be notified of events such as when estimation starts,
180 * ends or its progress significantly changes.
181 * @throws IllegalArgumentException if circles is null or if length of circles array
182 * is less than required (3 points).
183 */
184 protected RobustLateration2DSolver(final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener) {
185 this(listener);
186 internalSetCircles(circles);
187 }
188
189 /**
190 * Constructor.
191 *
192 * @param circles circles defining positions and distances.
193 * @param distanceStandardDeviations standard deviations of provided measured distances.
194 * @param listener listener to be notified of events such as when estimation starts,
195 * ends or its progress significantly changes.
196 * @throws IllegalArgumentException if circles is null, length of circles array is less
197 * than required (3 points) or don't have the same length.
198 */
199 protected RobustLateration2DSolver(final Circle[] circles,
200 final double[] distanceStandardDeviations,
201 final RobustLaterationSolverListener<Point2D> listener) {
202 this(listener);
203 internalSetCirclesAndStandardDeviations(circles, distanceStandardDeviations);
204 }
205
206 /**
207 * Gets number of dimensions of provided points.
208 *
209 * @return always returns 2 dimensions.
210 */
211 @Override
212 public int getNumberOfDimensions() {
213 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
214 }
215
216 /**
217 * Minimum required number of positions and distances.
218 * At least 3 positions will be required.
219 *
220 * @return minimum required number of positions and distances.
221 */
222 @Override
223 public int getMinRequiredPositionsAndDistances() {
224 return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
225 }
226
227 /**
228 * Sets size of subsets to be checked during robust estimation.
229 * This has to be at least {@link #getMinRequiredPositionsAndDistances()}.
230 *
231 * @param preliminarySubsetSize size of subsets to be checked during robust estimation.
232 * @throws LockedException if instance is busy solving the lateration problem.
233 * @throws IllegalArgumentException if provided value is less than {@link #getMinRequiredPositionsAndDistances()}.
234 */
235 @Override
236 public void setPreliminarySubsetSize(final int preliminarySubsetSize) throws LockedException {
237 super.setPreliminarySubsetSize(preliminarySubsetSize);
238
239 innerPositions = new Point2D[preliminarySubsetSize];
240 innerDistances = new double[preliminarySubsetSize];
241 innerDistanceStandardDeviations = new double[preliminarySubsetSize];
242 }
243
244 /**
245 * Gets circles defined by provided positions and distances.
246 *
247 * @return circles defined by provided positions and distances.
248 */
249 public Circle[] getCircles() {
250 if (positions == null) {
251 return null;
252 }
253
254 final var result = new Circle[positions.length];
255
256 for (var i = 0; i < positions.length; i++) {
257 result[i] = new Circle(positions[i], distances[i]);
258 }
259 return result;
260 }
261
262 /**
263 * Sets circles defining positions and euclidean distances.
264 *
265 * @param circles circles defining positions and distances.
266 * @throws IllegalArgumentException if circles is null or length of array of circles
267 * is less than 3.
268 * @throws LockedException if instance is busy solving the lateration problem.
269 */
270 public void setCircles(final Circle[] circles) throws LockedException {
271 if (isLocked()) {
272 throw new LockedException();
273 }
274 internalSetCircles(circles);
275 }
276
277 /**
278 * Sets circles defining positions and Euclidean distances along with the standard
279 * deviations of provided circles radii.
280 *
281 * @param circles circles defining positions and distances.
282 * @param radiusStandardDeviations standard deviations of circles radii.
283 * @throws IllegalArgumentException if circles is null, length of arrays is less than
284 * 3 or don't have the same length.
285 * @throws LockedException if instance is busy solving the lateration problem.
286 */
287 public void setCirclesAndStandardDeviations(
288 final Circle[] circles, final double[] radiusStandardDeviations) throws LockedException {
289 if (isLocked()) {
290 throw new LockedException();
291 }
292 internalSetCirclesAndStandardDeviations(circles, radiusStandardDeviations);
293 }
294
295 /**
296 * Creates a robust 2D lateration solver.
297 *
298 * @param method robust estimator method.
299 * @return a new robust 2D lateration solver.
300 */
301 public static RobustLateration2DSolver create(final RobustEstimatorMethod method) {
302 return switch (method) {
303 case RANSAC -> new RANSACRobustLateration2DSolver();
304 case LMEDS -> new LMedSRobustLateration2DSolver();
305 case MSAC -> new MSACRobustLateration2DSolver();
306 case PROSAC -> new PROSACRobustLateration2DSolver();
307 default -> new PROMedSRobustLateration2DSolver();
308 };
309 }
310
311 /**
312 * Creates a robust 2D lateration solver.
313 *
314 * @param listener listener to be notified of events such as when estimation
315 * starts, ends or its progress significantly changes.
316 * @param method robust estimator method.
317 * @return a new robust 2D lateration solver.
318 */
319 public static RobustLateration2DSolver create(
320 final RobustLaterationSolverListener<Point2D> listener, final RobustEstimatorMethod method) {
321 return switch (method) {
322 case RANSAC -> new RANSACRobustLateration2DSolver(listener);
323 case LMEDS -> new LMedSRobustLateration2DSolver(listener);
324 case MSAC -> new MSACRobustLateration2DSolver(listener);
325 case PROSAC -> new PROSACRobustLateration2DSolver(listener);
326 default -> new PROMedSRobustLateration2DSolver(listener);
327 };
328 }
329
330 /**
331 * Creates a robust 2D lateration solver.
332 *
333 * @param positions known positions of static nodes.
334 * @param distances euclidean distances from static nodes to mobile node to be
335 * estimated.
336 * @param method robust estimator method.
337 * @return a new robust 2D lateration solver.
338 * @throws IllegalArgumentException if either positions or distances are null,
339 * don't have the same length or their length is smaller than required
340 * (3 points).
341 */
342 public static RobustLateration2DSolver create(
343 final Point2D[] positions, final double[] distances, final RobustEstimatorMethod method) {
344 return switch (method) {
345 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances);
346 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances);
347 case MSAC -> new MSACRobustLateration2DSolver(positions, distances);
348 case PROSAC -> new PROSACRobustLateration2DSolver(positions, distances);
349 default -> new PROMedSRobustLateration2DSolver(positions, distances);
350 };
351 }
352
353 /**
354 * Creates a robust 2D lateration solver.
355 *
356 * @param positions known positions of static nodes.
357 * @param distances euclidean distances from static nodes to mobile node to be
358 * estimated.
359 * @param distanceStandardDeviations if either positions or distances are null,
360 * don't have the same length or their length
361 * is smaller than required (3 points).
362 * @param method robust estimator method.
363 * @return a new robust 2D lateration solver.
364 * @throws IllegalArgumentException if either positions or distances are null,
365 * don't have the same length or their length is smaller than required
366 * (3 points).
367 */
368 public static RobustLateration2DSolver create(
369 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
370 final RobustEstimatorMethod method) {
371 return switch (method) {
372 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
373 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
374 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
375 case PROSAC -> new PROSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
376 default -> new PROMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
377 };
378 }
379
380 /**
381 * Creates a robust 2D lateration solver.
382 *
383 * @param positions known positions of static nodes.
384 * @param distances euclidean distances from static nodes to mobile node to be
385 * estimated.
386 * @param listener listener to be notified of events such as when estimation
387 * starts, ends or its progress significantly changes.
388 * @param method robust estimator method.
389 * @return a new robust 2D lateration solver.
390 * @throws IllegalArgumentException if either positions or distances are null,
391 * don't have the same length or their length is smaller than required (3 points).
392 */
393 public static RobustLateration2DSolver create(
394 final Point2D[] positions, final double[] distances, final RobustLaterationSolverListener<Point2D> listener,
395 final RobustEstimatorMethod method) {
396 return switch (method) {
397 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, listener);
398 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, listener);
399 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, listener);
400 case PROSAC -> new PROSACRobustLateration2DSolver(positions, distances, listener);
401 default -> new PROMedSRobustLateration2DSolver(positions, distances, listener);
402 };
403 }
404
405 /**
406 * Creates a robust 2D lateration solver.
407 *
408 * @param positions known positions of static nodes.
409 * @param distances euclidean distances from static nodes to mobile node to be
410 * estimated.
411 * @param distanceStandardDeviations standard deviations of provided measured
412 * distances.
413 * @param listener listener to be notified of events such as when estimation
414 * starts, ends or its progress significantly changes.
415 * @param method robust estimator method.
416 * @return a new robust 2D lateration solver.
417 * @throws IllegalArgumentException if either positions, distances or
418 * standard deviations are null, don't have the same length or their length
419 * is smaller than required (3 points).
420 */
421 public static RobustLateration2DSolver create(
422 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
423 final RobustLaterationSolverListener<Point2D> listener, final RobustEstimatorMethod method) {
424 return switch (method) {
425 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations,
426 listener);
427 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations, listener);
428 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations, listener);
429 case PROSAC -> new PROSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations,
430 listener);
431 default -> new PROMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations, listener);
432 };
433 }
434
435 /**
436 * Creates a robust 2D lateration solver.
437 *
438 * @param circles circles defining positions and distances.
439 * @param method robust estimator method.
440 * @return a new robust 2D lateration solver.
441 * @throws IllegalArgumentException if circles is null, length of circles array
442 * is less than required (3 points) or don't have the same length.
443 */
444 public static RobustLateration2DSolver create(final Circle[] circles, final RobustEstimatorMethod method) {
445 return switch (method) {
446 case RANSAC -> new RANSACRobustLateration2DSolver(circles);
447 case LMEDS -> new LMedSRobustLateration2DSolver(circles);
448 case MSAC -> new MSACRobustLateration2DSolver(circles);
449 case PROSAC -> new PROSACRobustLateration2DSolver(circles);
450 default -> new PROMedSRobustLateration2DSolver(circles);
451 };
452 }
453
454 /**
455 * Creates a robust 2D lateration solver.
456 *
457 * @param circles circles defining positions and distances.
458 * @param distanceStandardDeviations standard deviations of provided measured
459 * distances.
460 * @param method robust estimator method.
461 * @return a new robust 2D lateration solver.
462 * @throws IllegalArgumentException if circles is null, length of circles array
463 * is less than required (3 points) or don't have the same length.
464 */
465 public static RobustLateration2DSolver create(
466 final Circle[] circles, final double[] distanceStandardDeviations, final RobustEstimatorMethod method) {
467 return switch (method) {
468 case RANSAC -> new RANSACRobustLateration2DSolver(circles, distanceStandardDeviations);
469 case LMEDS -> new LMedSRobustLateration2DSolver(circles, distanceStandardDeviations);
470 case MSAC -> new MSACRobustLateration2DSolver(circles, distanceStandardDeviations);
471 case PROSAC -> new PROSACRobustLateration2DSolver(circles, distanceStandardDeviations);
472 default -> new PROMedSRobustLateration2DSolver(circles, distanceStandardDeviations);
473 };
474 }
475
476 /**
477 * Creates a robust 2D lateration solver.
478 *
479 * @param circles circles defining positions and distances.
480 * @param listener listener to be notified of events such as when estimation
481 * starts, ends or its progress significantly changes.
482 * @param method robust estimator method.
483 * @return a new robust 2D lateration solver.
484 * @throws IllegalArgumentException if circles is null or if length of circles
485 * array is less than required (3 points).
486 */
487 public static RobustLateration2DSolver create(
488 final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener,
489 final RobustEstimatorMethod method) {
490 return switch (method) {
491 case RANSAC -> new RANSACRobustLateration2DSolver(circles, listener);
492 case LMEDS -> new LMedSRobustLateration2DSolver(circles, listener);
493 case MSAC -> new MSACRobustLateration2DSolver(circles, listener);
494 case PROSAC -> new PROSACRobustLateration2DSolver(circles, listener);
495 default -> new PROMedSRobustLateration2DSolver(circles, listener);
496 };
497 }
498
499 /**
500 * Creates a robust 2D lateration solver.
501 *
502 * @param circles circles defining positions and distances.
503 * @param distanceStandardDeviations standard deviations of provided measured
504 * distances.
505 * @param listener listener to be notified of events such as when estimation
506 * starts, ends or its progress significantly changes.
507 * @param method robust estimator method.
508 * @return a new robust 2D lateration solver.
509 * @throws IllegalArgumentException if circles is null, length of circles array
510 * is less than required (3 points) or don't have the same length.
511 */
512 public static RobustLateration2DSolver create(
513 final Circle[] circles, final double[] distanceStandardDeviations,
514 final RobustLaterationSolverListener<Point2D> listener, final RobustEstimatorMethod method) {
515 return switch (method) {
516 case RANSAC -> new RANSACRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
517 case LMEDS -> new LMedSRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
518 case MSAC -> new MSACRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
519 case PROSAC -> new PROSACRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
520 default -> new PROMedSRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
521 };
522 }
523
524 /**
525 * Creates a robust 2D lateration solver.
526 *
527 * @param qualityScores quality scores corresponding to each provided sample.
528 * The larger the score value the better the quality of
529 * the sample.
530 * @param method robust estimator method.
531 * @return a new robust 2D lateration solver.
532 * @throws IllegalArgumentException if quality scores is null, length of
533 * quality scores is less than required minimum (3 samples).
534 */
535 public static RobustLateration2DSolver create(final double[] qualityScores, final RobustEstimatorMethod method) {
536 return switch (method) {
537 case RANSAC -> new RANSACRobustLateration2DSolver();
538 case LMEDS -> new LMedSRobustLateration2DSolver();
539 case MSAC -> new MSACRobustLateration2DSolver();
540 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores);
541 default -> new PROMedSRobustLateration2DSolver(qualityScores);
542 };
543 }
544
545 /**
546 * Creates a robust 2D lateration solver.
547 *
548 * @param qualityScores quality scores corresponding to each provided sample.
549 * The larger the score value the better the quality of
550 * the sample.
551 * @param listener listener to be notified of events such as when estimation
552 * starts, ends or its progress significantly changes.
553 * @param method robust estimator method.
554 * @return a new robust 2D lateration solver.
555 * @throws IllegalArgumentException if quality scores is null, length of
556 * quality scores is less than required minimum (3 samples).
557 */
558 public static RobustLateration2DSolver create(
559 final double[] qualityScores, final RobustLaterationSolverListener<Point2D> listener,
560 final RobustEstimatorMethod method) {
561 return switch (method) {
562 case RANSAC -> new RANSACRobustLateration2DSolver(listener);
563 case LMEDS -> new LMedSRobustLateration2DSolver(listener);
564 case MSAC -> new MSACRobustLateration2DSolver(listener);
565 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, listener);
566 default -> new PROMedSRobustLateration2DSolver(qualityScores, listener);
567 };
568 }
569
570 /**
571 * Creates a robust 2D lateration solver.
572 *
573 * @param qualityScores quality scores corresponding to each provided sample.
574 * The larger the score value the better the quality of
575 * the sample.
576 * @param positions known positions of static nodes.
577 * @param distances euclidean distances from static nodes to mobile node to be
578 * estimated.
579 * @param method robust estimator method.
580 * @return a new robust 2D lateration solver.
581 * @throws IllegalArgumentException if either positions, distances or quality
582 * scores are null, don't have the same length or their length is smaller than
583 * required (3 points).
584 */
585 public static RobustLateration2DSolver create(
586 final double[] qualityScores, final Point2D[] positions, final double[] distances,
587 final RobustEstimatorMethod method) {
588 return switch (method) {
589 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances);
590 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances);
591 case MSAC -> new MSACRobustLateration2DSolver(positions, distances);
592 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, positions, distances);
593 default -> new PROMedSRobustLateration2DSolver(qualityScores, positions, distances);
594 };
595 }
596
597 /**
598 * Creates a robust 2D lateration solver.
599 *
600 * @param qualityScores quality scores corresponding to each provided sample.
601 * The larger the score value the better the quality of
602 * the sample.
603 * @param positions known positions of static nodes.
604 * @param distances euclidean distances from static nodes to mobile node to be
605 * estimated.
606 * @param distanceStandardDeviations standard deviations of provided measured
607 * distances.
608 * @param method robust estimator method.
609 * @return a new robust 2D lateration solver.
610 * @throws IllegalArgumentException if either positions, distances, quality
611 * scores or standard deviations are null, don't have the same length or the
612 * length is smaller than required (3 points).
613 */
614 public static RobustLateration2DSolver create(
615 final double[] qualityScores, final Point2D[] positions, final double[] distances,
616 final double[] distanceStandardDeviations, final RobustEstimatorMethod method) {
617 return switch (method) {
618 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
619 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
620 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations);
621 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, positions, distances,
622 distanceStandardDeviations);
623 default -> new PROMedSRobustLateration2DSolver(qualityScores, positions, distances,
624 distanceStandardDeviations);
625 };
626 }
627
628 /**
629 * Creates a robust 2D lateration solver.
630 *
631 * @param qualityScores quality scores corresponding to each provided sample.
632 * The larger the score value the better the quality of
633 * the sample.
634 * @param positions known positions of static nodes.
635 * @param distances euclidean distances from static nodes to mobile node.
636 * @param distanceStandardDeviations standard deviations of provided measured
637 * distances.
638 * @param listener listener to be notified of events such as when estimation
639 * starts, ends or its progress significantly changes.
640 * @param method robust estimator method.
641 * @return a new robust 2D lateration solver.
642 * @throws IllegalArgumentException if either positions, distances or standard
643 * deviations are null, don't have the same length or their length is smaller
644 * than required (3 points).
645 */
646 public static RobustLateration2DSolver create(
647 final double[] qualityScores, final Point2D[] positions, final double[] distances,
648 final double[] distanceStandardDeviations, final RobustLaterationSolverListener<Point2D> listener,
649 final RobustEstimatorMethod method) {
650 return switch (method) {
651 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations,
652 listener);
653 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, distanceStandardDeviations, listener);
654 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, distanceStandardDeviations, listener);
655 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, positions, distances,
656 distanceStandardDeviations, listener);
657 default -> new PROMedSRobustLateration2DSolver(qualityScores, positions, distances,
658 distanceStandardDeviations, listener);
659 };
660 }
661
662 /**
663 * Creates a robust 2D lateration solver.
664 *
665 * @param qualityScores quality scores corresponding to each provided sample.
666 * The larger the score value the better the quality of
667 * the sample.
668 * @param positions known positions of static nodes.
669 * @param distances euclidean distances from static nodes to mobile node.
670 * @param listener listener to be notified of events such as when estimation
671 * starts, ends or its progress significantly changes.
672 * @param method robust estimator method.
673 * @return a new robust 2D lateration solver.
674 * @throws IllegalArgumentException if either positions, distances, quality
675 * scores or standard deviations are null, don't have the same length or their
676 * length is smaller than required (3 points).
677 */
678 public static RobustLateration2DSolver create(
679 final double[] qualityScores, final Point2D[] positions, final double[] distances,
680 final RobustLaterationSolverListener<Point2D> listener, final RobustEstimatorMethod method) {
681 return switch (method) {
682 case RANSAC -> new RANSACRobustLateration2DSolver(positions, distances, listener);
683 case LMEDS -> new LMedSRobustLateration2DSolver(positions, distances, listener);
684 case MSAC -> new MSACRobustLateration2DSolver(positions, distances, listener);
685 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, positions, distances, listener);
686 default -> new PROMedSRobustLateration2DSolver(qualityScores, positions, distances, listener);
687 };
688 }
689
690 /**
691 * Creates a robust 2D lateration solver.
692 *
693 * @param qualityScores quality scores corresponding to each provided sample.
694 * The larger the score value the better the quality of
695 * the sample.
696 * @param circles circles defining positions and distances.
697 * @param method robust estimator method.
698 * @return a new robust 2D lateration solver.
699 * @throws IllegalArgumentException if either circles or quality scores are
700 * null don't have the same length or their length is less than required
701 * (3 points).
702 */
703 public static RobustLateration2DSolver create(
704 final double[] qualityScores, final Circle[] circles, final RobustEstimatorMethod method) {
705 return switch (method) {
706 case RANSAC -> new RANSACRobustLateration2DSolver(circles);
707 case LMEDS -> new LMedSRobustLateration2DSolver(circles);
708 case MSAC -> new MSACRobustLateration2DSolver(circles);
709 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, circles);
710 default -> new PROMedSRobustLateration2DSolver(qualityScores, circles);
711 };
712 }
713
714 /**
715 * Creates a robust 2D lateration solver.
716 *
717 * @param qualityScores quality scores corresponding to each provided sample.
718 * The larger the score value the better the quality of
719 * the sample.
720 * @param circles circles defining positions and distances.
721 * @param distanceStandardDeviations standard deviations of provided measured
722 * distances.
723 * @param method robust estimator method.
724 * @return a new robust 2D lateration solver.
725 * @throws IllegalArgumentException if either circles, quality scores or
726 * standard deviations are null, don't have the same length or their length
727 * is less than required (3 points).
728 */
729 public static RobustLateration2DSolver create(
730 final double[] qualityScores, final Circle[] circles, final double[] distanceStandardDeviations,
731 final RobustEstimatorMethod method) {
732 return switch (method) {
733 case RANSAC -> new RANSACRobustLateration2DSolver(circles, distanceStandardDeviations);
734 case LMEDS -> new LMedSRobustLateration2DSolver(circles, distanceStandardDeviations);
735 case MSAC -> new MSACRobustLateration2DSolver(circles, distanceStandardDeviations);
736 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, circles, distanceStandardDeviations);
737 default -> new PROMedSRobustLateration2DSolver(qualityScores, circles, distanceStandardDeviations);
738 };
739 }
740
741 /**
742 * Creates a robust 2D lateration solver.
743 *
744 * @param qualityScores quality scores corresponding to each provided sample.
745 * The larger the score value the better the quality of
746 * the sample.
747 * @param circles circles defining positions and distances.
748 * @param distanceStandardDeviations standard deviations of provided measured
749 * distances.
750 * @param listener listener to be notified of events such as when estimation
751 * starts, ends or its progress significantly changes.
752 * @param method robust estimator method.
753 * @return a new robust 2D lateration solver.
754 * @throws IllegalArgumentException if either circles, quality scores or
755 * standard deviations are null, don't have the same length or their length
756 * is less than required (3 points).
757 */
758 public static RobustLateration2DSolver create(
759 final double[] qualityScores, final Circle[] circles, final double[] distanceStandardDeviations,
760 final RobustLaterationSolverListener<Point2D> listener, final RobustEstimatorMethod method) {
761 return switch (method) {
762 case RANSAC -> new RANSACRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
763 case LMEDS -> new LMedSRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
764 case MSAC -> new MSACRobustLateration2DSolver(circles, distanceStandardDeviations, listener);
765 case PROSAC -> new PROSACRobustLateration2DSolver(qualityScores, circles, distanceStandardDeviations,
766 listener);
767 default -> new PROMedSRobustLateration2DSolver(qualityScores, circles, distanceStandardDeviations,
768 listener);
769 };
770 }
771
772 /**
773 * Creates a robust 2D lateration solver using default robust method.
774 *
775 * @return a new robust 2D lateration solver.
776 */
777 public static RobustLateration2DSolver create() {
778 return create(DEFAULT_ROBUST_METHOD);
779 }
780
781 /**
782 * Creates a robust 2D lateration solver using default robust method.
783 *
784 * @param listener listener to be notified of events such as when estimation
785 * starts, ends or its progress significantly changes.
786 * @return a new robust 2D lateration solver.
787 */
788 public static RobustLateration2DSolver create(final RobustLaterationSolverListener<Point2D> listener) {
789 return create(listener, DEFAULT_ROBUST_METHOD);
790 }
791
792 /**
793 * Creates a robust 2D lateration solver using default robust method.
794 *
795 * @param positions known positions of static nodes.
796 * @param distances euclidean distances from static nodes to mobile node to be
797 * estimated.
798 * @return a new robust 2D lateration solver.
799 * @throws IllegalArgumentException if either positions or distances are null,
800 * don't have the same length or their length is smaller than required
801 * (3 points).
802 */
803 public static RobustLateration2DSolver create(final Point2D[] positions, final double[] distances) {
804 return create(positions, distances, DEFAULT_ROBUST_METHOD);
805 }
806
807 /**
808 * Creates a robust 2D lateration solver using default robust method.
809 *
810 * @param positions known positions of static nodes.
811 * @param distances euclidean distances from static nodes to mobile node to be
812 * estimated.
813 * @param distanceStandardDeviations if either positions or distances are null,
814 * don't have the same length or their length
815 * is smaller than required (3 points).
816 * @return a new robust 2D lateration solver.
817 * @throws IllegalArgumentException if either positions or distances are null,
818 * don't have the same length or their length is smaller than required
819 * (3 points).
820 */
821 public static RobustLateration2DSolver create(
822 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations) {
823 return create(positions, distances, distanceStandardDeviations, DEFAULT_ROBUST_METHOD);
824 }
825
826 /**
827 * Creates a robust 2D lateration solver using default robust method.
828 *
829 * @param positions known positions of static nodes.
830 * @param distances euclidean distances from static nodes to mobile node to be
831 * estimated.
832 * @param listener listener to be notified of events such as when estimation
833 * starts, ends or its progress significantly changes.
834 * @return a new robust 2D lateration solver.
835 * @throws IllegalArgumentException if either positions or distances are null,
836 * don't have the same length or their length is smaller than required
837 * (3 points).
838 */
839 public static RobustLateration2DSolver create(
840 final Point2D[] positions, final double[] distances,
841 final RobustLaterationSolverListener<Point2D> listener) {
842 return create(positions, distances, listener, DEFAULT_ROBUST_METHOD);
843 }
844
845 /**
846 * Creates a robust 2D lateration solver using default robust method.
847 *
848 * @param positions known positions of static nodes.
849 * @param distances euclidean distances from static nodes to mobile node to be
850 * estimated.
851 * @param distanceStandardDeviations standard deviations of provided measured
852 * distances.
853 * @param listener listener to be notified of events such as when estimation
854 * starts, ends or its progress significantly changes.
855 * @return a new robust 2D lateration solver.
856 * @throws IllegalArgumentException if either positions, distances or
857 * standard deviations are null, don't have the same length or their length
858 * is smaller than required (3 points).
859 */
860 public static RobustLateration2DSolver create(
861 final Point2D[] positions, final double[] distances, final double[] distanceStandardDeviations,
862 final RobustLaterationSolverListener<Point2D> listener) {
863 return create(positions, distances, distanceStandardDeviations, listener, DEFAULT_ROBUST_METHOD);
864 }
865
866 /**
867 * Creates a robust 2D lateration solver using default robust method.
868 *
869 * @param circles circles defining positions and distances.
870 * @return a new robust 2D lateration solver.
871 * @throws IllegalArgumentException if circles is null, length of circles array
872 * is less than required (3 points) or don't have the same length.
873 */
874 public static RobustLateration2DSolver create(final Circle[] circles) {
875 return create(circles, DEFAULT_ROBUST_METHOD);
876 }
877
878 /**
879 * Creates a robust 2D lateration solver using default robust method.
880 *
881 * @param circles circles defining positions and distances.
882 * @param distanceStandardDeviations standard deviations of provided measured
883 * distances.
884 * @return a new robust 2D lateration solver.
885 * @throws IllegalArgumentException if circles is null, length of circles array
886 * is less than required (3 points) or don't have the same length.
887 */
888 public static RobustLateration2DSolver create(
889 final Circle[] circles, final double[] distanceStandardDeviations) {
890 return create(circles, distanceStandardDeviations, DEFAULT_ROBUST_METHOD);
891 }
892
893 /**
894 * Creates a robust 2D lateration solver using default robust method.
895 *
896 * @param circles circles defining positions and distances.
897 * @param listener listener to be notified of events such as when estimation
898 * starts, ends or its progress significantly changes.
899 * @return a new robust 2D lateration solver.
900 * @throws IllegalArgumentException if circles is null or if length of circles
901 * array is less than required (3 points).
902 */
903 public static RobustLateration2DSolver create(
904 final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener) {
905 return create(circles, listener, DEFAULT_ROBUST_METHOD);
906 }
907
908 /**
909 * Creates a robust 2D lateration solver using default robust method.
910 *
911 * @param circles circles defining positions and distances.
912 * @param distanceStandardDeviations standard deviations of provided measured
913 * distances.
914 * @param listener listener to be notified of events such as when estimation
915 * starts, ends or its progress significantly changes.
916 * @return a new robust 2D lateration solver.
917 * @throws IllegalArgumentException if circles is null, length of circles array
918 * is less than required (3 points) or don't have the same length.
919 */
920 public static RobustLateration2DSolver create(
921 final Circle[] circles, final double[] distanceStandardDeviations,
922 final RobustLaterationSolverListener<Point2D> listener) {
923 return create(circles, distanceStandardDeviations, listener, DEFAULT_ROBUST_METHOD);
924 }
925
926 /**
927 * Creates a robust 2D lateration solver using default robust method.
928 *
929 * @param qualityScores quality scores corresponding to each provided sample.
930 * The larger the score value the better the quality of
931 * the sample.
932 * @return a new robust 2D lateration solver.
933 * @throws IllegalArgumentException if quality scores is null, length of
934 * quality scores is less than required (3 samples).
935 */
936 public static RobustLateration2DSolver create(final double[] qualityScores) {
937 return create(qualityScores, DEFAULT_ROBUST_METHOD);
938 }
939
940 /**
941 * Creates a robust 2D lateration solver using default robust method.
942 *
943 * @param qualityScores quality scores corresponding to each provided sample.
944 * The larger the score value the better the quality of
945 * the sample.
946 * @param listener listener to be notified of events such as when estimation
947 * starts, ends or its progress significantly changes.
948 * @return a new robust 2D lateration solver.
949 * @throws IllegalArgumentException if quality scores is null, length of
950 * quality scores is less than required minimum (3 samples).
951 */
952 public static RobustLateration2DSolver create(
953 final double[] qualityScores, final RobustLaterationSolverListener<Point2D> listener) {
954 return create(qualityScores, listener, DEFAULT_ROBUST_METHOD);
955 }
956
957 /**
958 * Creates a robust 2D lateration solver using default robust method.
959 *
960 * @param qualityScores quality scores corresponding to each provided sample.
961 * The larger the score value the better the quality of
962 * the sample.
963 * @param positions known positions of static nodes.
964 * @param distances euclidean distances from static nodes to mobile node to be
965 * estimated.
966 * @return a new robust 2D lateration solver.
967 * @throws IllegalArgumentException if either positions, distances or quality
968 * scores are null, don't have the same length or their length is smaller
969 * than required (3 points).
970 */
971 public static RobustLateration2DSolver create(
972 final double[] qualityScores, final Point2D[] positions, final double[] distances) {
973 return create(qualityScores, positions, distances, DEFAULT_ROBUST_METHOD);
974 }
975
976 /**
977 * Creates a robust 2D lateration solver using default robust method.
978 *
979 * @param qualityScores quality scores corresponding to each provided sample.
980 * The larger the score value the better the quality of
981 * the sample.
982 * @param positions known positions of static nodes.
983 * @param distances euclidean distance from static nodes to mobile node to be
984 * estimated.
985 * @param distanceStandardDeviations standard deviations of provided measured
986 * distances.
987 * @return a new robust 2D lateration solver.
988 * @throws IllegalArgumentException if either positions, distances, quality
989 * scores or standard deviations are null, don't have the same length or their
990 * length is smaller than required (3 points).
991 */
992 public static RobustLateration2DSolver create(
993 final double[] qualityScores, final Point2D[] positions, final double[] distances,
994 final double[] distanceStandardDeviations) {
995 return create(qualityScores, positions, distances, distanceStandardDeviations, DEFAULT_ROBUST_METHOD);
996 }
997
998 /**
999 * Creates a robust 2D lateration solver using default robust method.
1000 *
1001 * @param qualityScores quality scores corresponding to each provided sample.
1002 * The larger the score value the better the quality of
1003 * the sample.
1004 * @param positions known positions of static nodes.
1005 * @param distances euclidean distances from static nodes to mobile node.
1006 * @param distanceStandardDeviations standard deviations of provided measured
1007 * distances.
1008 * @param listener listener to be notified of events such as when estimation
1009 * starts, ends or its progress significantly changes.
1010 * @return a new robust 3D lateration solver.
1011 * @throws IllegalArgumentException if either positions, distances or standard
1012 * deviations are null, don't have the same length or their length is smaller
1013 * than required (3 points).
1014 */
1015 public static RobustLateration2DSolver create(
1016 final double[] qualityScores, final Point2D[] positions, final double[] distances,
1017 final double[] distanceStandardDeviations, final RobustLaterationSolverListener<Point2D> listener) {
1018 return create(qualityScores, positions, distances, distanceStandardDeviations, listener, DEFAULT_ROBUST_METHOD);
1019 }
1020
1021 /**
1022 * Creates a robust 2D lateration solver using default robust method.
1023 *
1024 * @param qualityScores quality scores corresponding to each provided sample.
1025 * The larger the score value the better the quality of
1026 * the sample.
1027 * @param positions known positions of static nodes.
1028 * @param distances euclidean distances from static nodes to mobile node.
1029 * @param listener listener to be notified of events such as when estimation
1030 * starts, ends or its progress significantly changes.
1031 * @return a new robust 2D lateration solver.
1032 * @throws IllegalArgumentException if either positions, distances, quality
1033 * scores or standard deviations are null, don't have the same length or their
1034 * length is smaller than required (3 points).
1035 */
1036 public static RobustLateration2DSolver create(
1037 final double[] qualityScores, final Point2D[] positions, final double[] distances,
1038 final RobustLaterationSolverListener<Point2D> listener) {
1039 return create(qualityScores, positions, distances, listener, DEFAULT_ROBUST_METHOD);
1040 }
1041
1042 /**
1043 * Creates a robust 2D lateration solver using default robust method.
1044 *
1045 * @param qualityScores quality scores corresponding to each provided sample.
1046 * The larger the score value the better the quality of
1047 * the sample.
1048 * @param circles circles defining positions and distances.
1049 * @return a new robust 2D lateration solver.
1050 * @throws IllegalArgumentException if either circles or quality scores are
1051 * null, don't have the same length or their length is less than required
1052 * (3 points).
1053 */
1054 public static RobustLateration2DSolver create(final double[] qualityScores, final Circle[] circles) {
1055 return create(qualityScores, circles, DEFAULT_ROBUST_METHOD);
1056 }
1057
1058 /**
1059 * Creates a robust 2D lateration solver using default robust method.
1060 *
1061 * @param qualityScores quality scores corresponding to each provided sample.
1062 * The larger the score value the better the quality of
1063 * the sample.
1064 * @param circles circles defining positions and distances.
1065 * @param distanceStandardDeviations standard deviations of provided measured
1066 * distances.
1067 * @return a new robust 2D lateration solver.
1068 * @throws IllegalArgumentException if either circles, quality scores or
1069 * standard deviations are null, don't have the same length or their length
1070 * is less than required (3 points).
1071 */
1072 public static RobustLateration2DSolver create(
1073 final double[] qualityScores, final Circle[] circles, final double[] distanceStandardDeviations) {
1074 return create(qualityScores, circles, distanceStandardDeviations, DEFAULT_ROBUST_METHOD);
1075 }
1076
1077 /**
1078 * Creates a robust 2D lateration solver using default robust method.
1079 *
1080 * @param qualityScores quality scores corresponding to each provided sample.
1081 * The larger the score value the better the quality of
1082 * the sample.
1083 * @param circles circles defining positions and distances.
1084 * @param distanceStandardDeviations standard deviations of provided measured
1085 * distances.
1086 * @param listener listener to be notified of events such as when estimation
1087 * starts, ends or its progress significantly changes.
1088 * @return a new robust 2D lateration solver.
1089 * @throws IllegalArgumentException if either circles, quality scores or
1090 * standard deviations are null, don't have the same length or their length
1091 * is less than required (3 points).
1092 */
1093 public static RobustLateration2DSolver create(
1094 final double[] qualityScores, final Circle[] circles, final double[] distanceStandardDeviations,
1095 final RobustLaterationSolverListener<Point2D> listener) {
1096 return create(qualityScores, circles, distanceStandardDeviations, listener, DEFAULT_ROBUST_METHOD);
1097 }
1098
1099 /**
1100 * Attempts to refine estimated position if refinement is requested.
1101 * This method returns a refined solution or provided input if refinement is not
1102 * requested or has failed.
1103 * If refinement is enabled, and it is requested to keep covariance, this method
1104 * will also keep covariance of refined position.
1105 *
1106 * @param position position estimated by a robust estimator without refinement.
1107 * @return solution after refinement (if requested) or provided non-refined
1108 * estimated position if not requested or refinement failed.
1109 */
1110 protected Point2D attemptRefine(final Point2D position) {
1111 if (refineResult && inliersData != null) {
1112 final var inliers = inliersData.getInliers();
1113 final var nSamples = distances.length;
1114 final var nInliers = inliersData.getNumInliers();
1115 final var inlierPositions = new Point2D[nInliers];
1116 final var inlierDistances = new double[nInliers];
1117 double[] inlierStandardDeviations = null;
1118 if (distanceStandardDeviations != null) {
1119 inlierStandardDeviations = new double[nInliers];
1120 }
1121 var pos = 0;
1122 for (var i = 0; i < nSamples; i++) {
1123 if (inliers.get(i)) {
1124 // sample is inlier
1125 inlierPositions[pos] = positions[i];
1126 inlierDistances[pos] = distances[i];
1127 if (inlierStandardDeviations != null) {
1128 inlierStandardDeviations[pos] = distanceStandardDeviations[i];
1129 }
1130 pos++;
1131 }
1132 }
1133
1134 try {
1135 nonLinearSolver.setInitialPosition(position);
1136 if (inlierStandardDeviations != null) {
1137 nonLinearSolver.setPositionsDistancesAndStandardDeviations(inlierPositions, inlierDistances,
1138 inlierStandardDeviations);
1139 } else {
1140 nonLinearSolver.setPositionsAndDistances(inlierPositions, inlierDistances);
1141 }
1142 nonLinearSolver.solve();
1143
1144 if (keepCovariance) {
1145 // keep covariance
1146 covariance = nonLinearSolver.getCovariance();
1147 } else {
1148 covariance = null;
1149 }
1150
1151 estimatedPosition = nonLinearSolver.getEstimatedPosition();
1152 } catch (Exception e) {
1153 // refinement failed, so we return input value
1154 covariance = null;
1155 estimatedPosition = position;
1156 }
1157 } else {
1158 covariance = null;
1159 estimatedPosition = position;
1160 }
1161
1162 return estimatedPosition;
1163 }
1164
1165 /**
1166 * Solves a preliminary solution for a subset of samples picked by a robust estimator.
1167 *
1168 * @param samplesIndices indices of samples picked by the robust estimator.
1169 * @param solutions list where estimated preliminary solution will be stored.
1170 */
1171 protected void solvePreliminarySolutions(final int[] samplesIndices, final List<Point2D> solutions) {
1172 try {
1173 final var length = samplesIndices.length;
1174 for (var i = 0; i < length; i++) {
1175 final var index = samplesIndices[i];
1176 innerPositions[i] = positions[index];
1177 innerDistances[i] = distances[index];
1178 innerDistanceStandardDeviations[i] = distanceStandardDeviations != null
1179 ? distanceStandardDeviations[index]
1180 : NonLinearLeastSquaresLaterationSolver.DEFAULT_DISTANCE_STANDARD_DEVIATION;
1181 }
1182
1183 var estimatedPosition = initialPosition;
1184 if (useLinearSolver) {
1185 if (useHomogeneousLinearSolver) {
1186 homogeneousLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
1187 homogeneousLinearSolver.solve();
1188 estimatedPosition = homogeneousLinearSolver.getEstimatedPosition();
1189 } else {
1190 inhomogeneousLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
1191 inhomogeneousLinearSolver.solve();
1192 estimatedPosition = inhomogeneousLinearSolver.getEstimatedPosition();
1193 }
1194 }
1195
1196 if (refinePreliminarySolutions || estimatedPosition == null) {
1197 nonLinearSolver.setInitialPosition(estimatedPosition);
1198 if (distanceStandardDeviations != null) {
1199 nonLinearSolver.setPositionsDistancesAndStandardDeviations(innerPositions,
1200 innerDistances, innerDistanceStandardDeviations);
1201 } else {
1202 nonLinearSolver.setPositionsAndDistances(innerPositions, innerDistances);
1203 }
1204 nonLinearSolver.solve();
1205 estimatedPosition = nonLinearSolver.getEstimatedPosition();
1206 }
1207
1208 solutions.add(estimatedPosition);
1209 } catch (final NavigationException ignore) {
1210 // if anything fails, no solution is added
1211 }
1212 }
1213
1214 /**
1215 * Internally sets circles defining positions and Euclidean distances.
1216 *
1217 * @param circles circles defining positions and distances.
1218 * @throws IllegalArgumentException if circles is null or length of array of circles
1219 * is less than {@link #getMinRequiredPositionsAndDistances}.
1220 */
1221 private void internalSetCircles(final Circle[] circles) {
1222 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
1223 throw new IllegalArgumentException();
1224 }
1225
1226 final var positions = new Point2D[circles.length];
1227 final var distances = new double[circles.length];
1228 for (var i = 0; i < circles.length; i++) {
1229 final var circle = circles[i];
1230 positions[i] = circle.getCenter();
1231 distances[i] = circle.getRadius();
1232 }
1233
1234 internalSetPositionsAndDistances(positions, distances);
1235 }
1236
1237 /**
1238 * Internally sets circles defining positions and Euclidean distances along with the standard
1239 * deviations of provided circles radii.
1240 *
1241 * @param circles circles defining positions and distances.
1242 * @param radiusStandardDeviations standard deviations of circles radii.
1243 * @throws IllegalArgumentException if circles is null, length of arrays is less than
1244 * 3 or don't have the same length.
1245 */
1246 private void internalSetCirclesAndStandardDeviations(
1247 final Circle[] circles, final double[] radiusStandardDeviations) {
1248 if (circles == null || circles.length < getMinRequiredPositionsAndDistances()) {
1249 throw new IllegalArgumentException();
1250 }
1251
1252 if (radiusStandardDeviations == null) {
1253 throw new IllegalArgumentException();
1254 }
1255
1256 if (radiusStandardDeviations.length != circles.length) {
1257 throw new IllegalArgumentException();
1258 }
1259
1260 final var positions = new Point2D[circles.length];
1261 final var distances = new double[circles.length];
1262 for (var i = 0; i < circles.length; i++) {
1263 final var circle = circles[i];
1264 positions[i] = circle.getCenter();
1265 distances[i] = circle.getRadius();
1266 }
1267
1268 internalSetPositionsDistancesAndStandardDeviations(positions, distances, radiusStandardDeviations);
1269 }
1270
1271 /**
1272 * Setup inner positions and distances.
1273 */
1274 private void init() {
1275 final var points = getMinRequiredPositionsAndDistances();
1276 preliminarySubsetSize = points;
1277 innerPositions = new Point2D[points];
1278 innerDistances = new double[points];
1279 innerDistanceStandardDeviations = new double[points];
1280
1281 inhomogeneousLinearSolver = new InhomogeneousLinearLeastSquaresLateration2DSolver();
1282 homogeneousLinearSolver = new HomogeneousLinearLeastSquaresLateration2DSolver();
1283 nonLinearSolver = new NonLinearLeastSquaresLateration2DSolver();
1284 }
1285 }