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