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.InhomogeneousPoint3D;
19 import com.irurueta.geometry.Point3D;
20 import com.irurueta.geometry.Sphere;
21 import com.irurueta.navigation.LockedException;
22
23 /**
24 * Solves a Trilateration problem with an instance of the least squares optimizer.
25 */
26 public class NonLinearLeastSquaresLateration3DSolver extends NonLinearLeastSquaresLaterationSolver<Point3D> {
27
28 /**
29 * Constructor.
30 */
31 public NonLinearLeastSquaresLateration3DSolver() {
32 super();
33 }
34
35 /**
36 * Constructor.
37 *
38 * @param positions known positions of static nodes.
39 * @param distances euclidean distances from static nodes to mobile node.
40 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
41 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
42 * is null.
43 */
44 public NonLinearLeastSquaresLateration3DSolver(final Point3D[] positions, final double[] distances) {
45 super(positions, distances);
46 }
47
48 /**
49 * Constructor.
50 *
51 * @param initialPosition initial position to start lateration solving.
52 */
53 public NonLinearLeastSquaresLateration3DSolver(final Point3D initialPosition) {
54 super(initialPosition);
55 }
56
57 /**
58 * Constructor.
59 *
60 * @param positions known positions of static nodes.
61 * @param distances euclidean distances from static nodes to mobile node.
62 * @param initialPosition initial position to start lateration solving.
63 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
64 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
65 * is null.
66 */
67 public NonLinearLeastSquaresLateration3DSolver(
68 final Point3D[] positions, final double[] distances, final Point3D initialPosition) {
69 super(positions, distances, initialPosition);
70 }
71
72 /**
73 * Constructor.
74 *
75 * @param listener listener to be notified of events raised by this instance.
76 */
77 public NonLinearLeastSquaresLateration3DSolver(final LaterationSolverListener<Point3D> listener) {
78 super(listener);
79 }
80
81 /**
82 * Constructor.
83 *
84 * @param positions known positions of static nodes.
85 * @param distances euclidean distances from static nodes to mobile node.
86 * @param listener listener to be notified of events raised by this instance.
87 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
88 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
89 * is null.
90 */
91 public NonLinearLeastSquaresLateration3DSolver(
92 final Point3D[] positions, final double[] distances, final LaterationSolverListener<Point3D> listener) {
93 super(positions, distances, listener);
94 }
95
96 /**
97 * Constructor.
98 *
99 * @param initialPosition initial position to start lateration solving.
100 * @param listener listener to be notified of events raised by this instance.
101 */
102 public NonLinearLeastSquaresLateration3DSolver(
103 final Point3D initialPosition, final LaterationSolverListener<Point3D> listener) {
104 super(initialPosition, listener);
105 }
106
107 /**
108 * Constructor.
109 *
110 * @param positions known positions of static nodes.
111 * @param distances euclidean distances from static nodes to mobile node.
112 * @param initialPosition initial position to start lateration solving.
113 * @param listener listener to be notified of events raised by this instance.
114 * @throws IllegalArgumentException if either positions or distances are null, don't have the same length or their
115 * length is smaller than required (3 for 2D points or 4 for 3D points) or fitter
116 * is null.
117 */
118 public NonLinearLeastSquaresLateration3DSolver(
119 final Point3D[] positions, final double[] distances, final Point3D initialPosition,
120 final LaterationSolverListener<Point3D> listener) {
121 super(positions, distances, initialPosition, listener);
122 }
123
124 /**
125 * Constructor.
126 *
127 * @param spheres spheres defining positions and distances.
128 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
129 */
130 public NonLinearLeastSquaresLateration3DSolver(final Sphere[] spheres) {
131 super();
132 internalSetSpheres(spheres);
133 }
134
135 /**
136 * Constructor.
137 *
138 * @param spheres spheres defining positions and distances.
139 * @param initialPosition initial position to start lateration solving.
140 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
141 */
142 public NonLinearLeastSquaresLateration3DSolver(final Sphere[] spheres, final Point3D initialPosition) {
143 super(initialPosition);
144 internalSetSpheres(spheres);
145 }
146
147 /**
148 * Constructor.
149 *
150 * @param spheres spheres defining positions and distances.
151 * @param listener listener to be notified of events raised by this instance.
152 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
153 */
154 public NonLinearLeastSquaresLateration3DSolver(
155 final Sphere[] spheres, final LaterationSolverListener<Point3D> listener) {
156 super(listener);
157 internalSetSpheres(spheres);
158 }
159
160 /**
161 * Constructor.
162 *
163 * @param spheres spheres defining positions and distances.
164 * @param initialPosition initial position to start lateration solving.
165 * @param listener listener to be notified of events raised by this instance.
166 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
167 */
168 public NonLinearLeastSquaresLateration3DSolver(
169 final Sphere[] spheres, final Point3D initialPosition, final LaterationSolverListener<Point3D> listener) {
170 super(initialPosition, listener);
171 internalSetSpheres(spheres);
172 }
173
174 /**
175 * Constructor.
176 *
177 * @param positions known positions of static nodes.
178 * @param distances euclidean distances from static nodes to mobile node.
179 * @param distanceStandardDeviations standard deviations of provided measured distances.
180 * @throws IllegalArgumentException if either positions, distances or standard deviations
181 * are null, don't have the same length of their length is smaller than required
182 * (2 points).
183 */
184 public NonLinearLeastSquaresLateration3DSolver(
185 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations) {
186 super(positions, distances, distanceStandardDeviations);
187 }
188
189 /**
190 * Constructor.
191 *
192 * @param positions known positions of static nodes.
193 * @param distances euclidean distances from static nodes to mobile node.
194 * @param distanceStandardDeviations standard deviations of provided measured distances.
195 * @param initialPosition initial position to start lateration solving.
196 * @throws IllegalArgumentException if either positions, distances or standard deviations
197 * are null, don't have the same length of their length is smaller than required
198 * (2 points).
199 */
200 public NonLinearLeastSquaresLateration3DSolver(
201 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations,
202 final Point3D initialPosition) {
203 super(positions, distances, distanceStandardDeviations, initialPosition);
204 }
205
206 /**
207 * Constructor.
208 *
209 * @param positions known positions of static nodes.
210 * @param distances euclidean distances from static nodes to mobile node.
211 * @param distanceStandardDeviations standard deviations of provided measured distances.
212 * @param listener listener to be notified of events raised by this instance.
213 * @throws IllegalArgumentException if either positions, distances or standard deviations
214 * are null, don't have the same length of their length is smaller than required
215 * (2 points).
216 */
217 public NonLinearLeastSquaresLateration3DSolver(
218 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations,
219 final LaterationSolverListener<Point3D> listener) {
220 super(positions, distances, distanceStandardDeviations, listener);
221 }
222
223 /**
224 * Constructor.
225 *
226 * @param positions known positions of static nodes.
227 * @param distances euclidean distances from static nodes to mobile node.
228 * @param distanceStandardDeviations standard deviations of provided measured distances.
229 * @param initialPosition initial position to start lateration solving.
230 * @param listener listener to be notified of events raised by this instance.
231 * @throws IllegalArgumentException if either positions, distances or standard deviations
232 * are null, don't have the same length of their length is smaller than required
233 * (2 points).
234 */
235 public NonLinearLeastSquaresLateration3DSolver(
236 final Point3D[] positions, final double[] distances, final double[] distanceStandardDeviations,
237 final Point3D initialPosition, final LaterationSolverListener<Point3D> listener) {
238 super(positions, distances, distanceStandardDeviations, initialPosition, listener);
239 }
240
241 /**
242 * Constructor.
243 *
244 * @param spheres spheres defining positions and distances.
245 * @param distanceStandardDeviations standard deviations of provided measured distances.
246 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
247 */
248 public NonLinearLeastSquaresLateration3DSolver(
249 final Sphere[] spheres, final double[] distanceStandardDeviations) {
250 super();
251 internalSetSpheresAndStandardDeviations(spheres, distanceStandardDeviations);
252 }
253
254 /**
255 * Constructor.
256 *
257 * @param spheres spheres defining positions and distances.
258 * @param distanceStandardDeviations standard deviations of provided measured distances.
259 * @param initialPosition initial position to start lateration solving.
260 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
261 */
262 public NonLinearLeastSquaresLateration3DSolver(
263 final Sphere[] spheres, final double[] distanceStandardDeviations, final Point3D initialPosition) {
264 super(initialPosition);
265 internalSetSpheresAndStandardDeviations(spheres, distanceStandardDeviations);
266 }
267
268 /**
269 * Constructor.
270 *
271 * @param spheres spheres defining positions and distances.
272 * @param distanceStandardDeviations standard deviations of provided measured distances.
273 * @param listener listener to be notified of events raised by this instance.
274 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
275 */
276 public NonLinearLeastSquaresLateration3DSolver(
277 final Sphere[] spheres, final double[] distanceStandardDeviations,
278 final LaterationSolverListener<Point3D> listener) {
279 super(listener);
280 internalSetSpheresAndStandardDeviations(spheres, distanceStandardDeviations);
281 }
282
283 /**
284 * Constructor.
285 *
286 * @param spheres spheres defining positions and distances.
287 * @param distanceStandardDeviations standard deviations of provided measured distances.
288 * @param initialPosition initial position to start lateration solving.
289 * @param listener listener to be notified of events raised by this instance.
290 * @throws IllegalArgumentException if spheres is null or if length of spheres array is less than 2.
291 */
292 public NonLinearLeastSquaresLateration3DSolver(
293 final Sphere[] spheres, final double[] distanceStandardDeviations, final Point3D initialPosition,
294 final LaterationSolverListener<Point3D> listener) {
295 super(initialPosition, listener);
296 internalSetSpheresAndStandardDeviations(spheres, distanceStandardDeviations);
297 }
298
299 /**
300 * Gets spheres defined by provided positions and distances.
301 *
302 * @return spheres defined by provided positions and distances.
303 */
304 public Sphere[] getSpheres() {
305 if (positions == null) {
306 return null;
307 }
308
309 final var result = new Sphere[positions.length];
310
311 for (var i = 0; i < positions.length; i++) {
312 result[i] = new Sphere(positions[i], distances[i]);
313 }
314 return result;
315 }
316
317 /**
318 * Sets spheres defining positions and Euclidean distances.
319 *
320 * @param spheres spheres defining positions and distances.
321 * @throws IllegalArgumentException if spheres is null or length of array of spheres
322 * is less than 2.
323 * @throws LockedException if instance is busy solving the lateration problem.
324 */
325 public void setSpheres(final Sphere[] spheres) throws LockedException {
326 if (isLocked()) {
327 throw new LockedException();
328 }
329 internalSetSpheres(spheres);
330 }
331
332 /**
333 * Sets spheres defining positions and Euclidean distances along with the standard
334 * deviations of provided spheres radii.
335 *
336 * @param spheres spheres defining positions and distances.
337 * @param radiusStandardDeviations standard deviations of circles radii.
338 * @throws IllegalArgumentException if spheres is null, length of arrays is less than
339 * 2 or don't have the same length.
340 * @throws LockedException if instance is busy solving the lateration problem.
341 */
342 public void setSpheresAndStandardDeviations(final Sphere[] spheres, final double[] radiusStandardDeviations)
343 throws LockedException {
344 if (isLocked()) {
345 throw new LockedException();
346 }
347 internalSetSpheresAndStandardDeviations(spheres, radiusStandardDeviations);
348 }
349
350 /**
351 * Gets number of dimensions of provided points.
352 *
353 * @return always returns 3 dimensions.
354 */
355 @Override
356 public int getNumberOfDimensions() {
357 return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
358 }
359
360 /**
361 * Minimum required number of positions and distances.
362 * At least 4 positions and distances will be required to solve a 3D problem.
363 *
364 * @return minimum required number of positions and distances.
365 */
366 @Override
367 public int getMinRequiredPositionsAndDistances() {
368 return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
369 }
370
371 /**
372 * Gets estimated position.
373 *
374 * @return estimated position.
375 */
376 @Override
377 public Point3D getEstimatedPosition() {
378 if (estimatedPositionCoordinates == null) {
379 return null;
380 }
381
382 final var position = new InhomogeneousPoint3D();
383 getEstimatedPosition(position);
384 return position;
385 }
386
387 /**
388 * Internally sets spheres defining positions and Euclidean distances.
389 *
390 * @param spheres spheres defining positions and distances.
391 * @throws IllegalArgumentException if spheres is null or length of array of spheres
392 * is less than 2.
393 */
394 public void internalSetSpheres(final Sphere[] spheres) {
395 if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
396 throw new IllegalArgumentException();
397 }
398
399 final var positions = new Point3D[spheres.length];
400 final var distances = new double[spheres.length];
401 for (var i = 0; i < spheres.length; i++) {
402 final var sphere = spheres[i];
403 positions[i] = sphere.getCenter();
404 distances[i] = sphere.getRadius();
405 }
406
407 internalSetPositionsAndDistances(positions, distances);
408 }
409
410 /**
411 * Internally sets spheres defining positions and Euclidean distances along with the standard
412 * deviations of provided spheres radii.
413 *
414 * @param spheres spheres defining positions and distances.
415 * @param radiusStandardDeviations standard deviations of circles radii.
416 * @throws IllegalArgumentException if spheres is null, length of arrays is less than
417 * 2 or don't have the same length.
418 */
419 private void internalSetSpheresAndStandardDeviations(
420 final Sphere[] spheres, final double[] radiusStandardDeviations) {
421 if (spheres == null || spheres.length < getMinRequiredPositionsAndDistances()) {
422 throw new IllegalArgumentException();
423 }
424
425 if (radiusStandardDeviations == null) {
426 throw new IllegalArgumentException();
427 }
428
429 if (radiusStandardDeviations.length != spheres.length) {
430 throw new IllegalArgumentException();
431 }
432
433 final var positions = new Point3D[spheres.length];
434 final var distances = new double[spheres.length];
435 for (var i = 0; i < spheres.length; i++) {
436 final var sphere = spheres[i];
437 positions[i] = sphere.getCenter();
438 distances[i] = sphere.getRadius();
439 }
440
441 internalSetPositionsDistancesAndStandardDeviations(positions, distances, radiusStandardDeviations);
442 }
443 }