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.indoor.radiosource;
17
18 import com.irurueta.geometry.Point2D;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.NotReadyException;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RangingReadingLocated;
23 import com.irurueta.numerical.robust.PROSACRobustEstimator;
24 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28
29 import java.util.List;
30
31 /**
32 * Robustly estimated 2D position of a radio source (e.g. Wi-Fi
33 * access point or bluetooth beacon), by discarding outliers using PROSAC
34 * algorithm.
35 *
36 * @param <S> a {@link RadioSource} type.
37 */
38 public class PROSACRobustRangingRadioSourceEstimator2D<S extends RadioSource> extends
39 RobustRangingRadioSourceEstimator2D<S> {
40
41 /**
42 * Constant defining default threshold to determine whether samples are inliers or not.
43 */
44 public static final double DEFAULT_THRESHOLD = 0.1;
45
46 /**
47 * Minimum value that can be set as threshold.
48 * Threshold must be strictly greater than 0.0.
49 */
50 public static final double MIN_THRESHOLD = 0.0;
51
52 /**
53 * Indicates that by default inliers will only be computed but not kept.
54 */
55 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
56
57 /**
58 * Indicates that by default residuals will only be computed but not kept.
59 */
60 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
61
62 /**
63 * Threshold to determine whether samples are inliers or not when testing possible solutions.
64 * The threshold refers to the amount of error on distance between estimated position and
65 * distances provided for each sample.
66 */
67 private double threshold = DEFAULT_THRESHOLD;
68
69 /**
70 * Indicates whether inliers must be computed and kept.
71 */
72 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
73
74 /**
75 * Indicates whether residuals must be computed and kept.
76 */
77 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
78
79 /**
80 * Quality scores corresponding to each provided sample.
81 * The larger the score value the better the quality of the sample.
82 */
83 private double[] qualityScores;
84
85 /**
86 * Constructor.
87 */
88 public PROSACRobustRangingRadioSourceEstimator2D() {
89 super();
90 }
91
92 /**
93 * Constructor.
94 * Sets radio signal ranging readings belonging to the same radio source.
95 *
96 * @param readings radio signal ranging readings belonging to the same
97 * radio source.
98 * @throws IllegalArgumentException if readings are not valid.
99 */
100 public PROSACRobustRangingRadioSourceEstimator2D(final List<? extends RangingReadingLocated<S, Point2D>> readings) {
101 super(readings);
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param listener listener in charge of attending events raised by this instance.
108 */
109 public PROSACRobustRangingRadioSourceEstimator2D(
110 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
111 super(listener);
112 }
113
114 /**
115 * Constructor.
116 * Sets radio signal readings belonging to the same radio source.
117 *
118 * @param readings radio signal readings belonging to the same radio source.
119 * @param listener listener in charge of attending events raised by this instance.
120 * @throws IllegalArgumentException if readings are not valid.
121 */
122 public PROSACRobustRangingRadioSourceEstimator2D(
123 final List<? extends RangingReadingLocated<S, Point2D>> readings,
124 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
125 super(readings, listener);
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param initialPosition initial position to start the estimation or radio
132 * source position.
133 */
134 public PROSACRobustRangingRadioSourceEstimator2D(final Point2D initialPosition) {
135 super(initialPosition);
136 }
137
138 /**
139 * Constructor.
140 * Sets radio signal readings belonging to the same radio source.
141 *
142 * @param readings radio signal readings belonging to the same radio source.
143 * @param initialPosition initial position to start the estimation of radio
144 * source position.
145 * @throws IllegalArgumentException if readings are not valid.
146 */
147 public PROSACRobustRangingRadioSourceEstimator2D(
148 final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition) {
149 super(readings, initialPosition);
150 }
151
152 /**
153 * Constructor.
154 *
155 * @param initialPosition initial position to start the estimation of radio
156 * source position.
157 * @param listener listener in charge of attending events raised by this instance.
158 */
159 public PROSACRobustRangingRadioSourceEstimator2D(
160 final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
161 super(initialPosition, listener);
162 }
163
164 /**
165 * Constructor.
166 * Sets radio signal ranging readings belonging to the same radio source.
167 *
168 * @param readings radio signal ranging readings belonging to the same radio source.
169 * @param initialPosition initial position to start the estimation of radio source
170 * position.
171 * @param listener listener in charge of attending events raised by this instance.
172 * @throws IllegalArgumentException if readings are not valid.
173 */
174 public PROSACRobustRangingRadioSourceEstimator2D(
175 final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
176 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
177 super(readings, initialPosition, listener);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param qualityScores quality scores corresponding to each provided
184 * sample. The larger the score value the better
185 * the quality of the sample.
186 * @throws IllegalArgumentException if quality scores is null, or length
187 * of quality scores is less than required minimum.
188 */
189 public PROSACRobustRangingRadioSourceEstimator2D(final double[] qualityScores) {
190 super();
191 internalSetQualityScores(qualityScores);
192 }
193
194 /**
195 * Constructor.
196 * Sets radio signal ranging readings belonging to the same radio source.
197 *
198 * @param qualityScores quality scores corresponding to each provided
199 * sample. The larger the score value the better
200 * the quality of the sample.
201 * @param readings radio signal ranging readings belonging to the same
202 * radio source.
203 * @throws IllegalArgumentException if readings are not valid, quality scores
204 * is null, or length of quality scores is less than required minimum.
205 */
206 public PROSACRobustRangingRadioSourceEstimator2D(
207 final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings) {
208 super(readings);
209 internalSetQualityScores(qualityScores);
210 }
211
212 /**
213 * Constructor.
214 *
215 * @param qualityScores quality scores corresponding to each provided
216 * sample. The larger the score value the better
217 * the quality of the sample.
218 * @param listener listener in charge of attending events raised by this instance.
219 * @throws IllegalArgumentException if quality scores is null, or length
220 * of quality scores is less than required minimum.
221 */
222 public PROSACRobustRangingRadioSourceEstimator2D(
223 final double[] qualityScores, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
224 super(listener);
225 internalSetQualityScores(qualityScores);
226 }
227
228 /**
229 * Constructor.
230 * Sets radio signal readings belonging to the same radio source.
231 *
232 * @param qualityScores quality scores corresponding to each provided
233 * sample. The larger the score value the better
234 * the quality of the sample.
235 * @param readings radio signal readings belonging to the same radio source.
236 * @param listener listener in charge of attending events raised by this instance.
237 * @throws IllegalArgumentException if readings are not valid, quality scores
238 * is null, or length of quality scores is less than required minimum.
239 */
240 public PROSACRobustRangingRadioSourceEstimator2D(
241 final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
242 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
243 super(readings, listener);
244 internalSetQualityScores(qualityScores);
245 }
246
247 /**
248 * Constructor.
249 *
250 * @param qualityScores quality scores corresponding to each provided
251 * sample. The larger the score value the better
252 * the quality of the sample.
253 * @param initialPosition initial position to start the estimation or radio
254 * source position.
255 * @throws IllegalArgumentException if quality scores is null, or length
256 * of quality scores is less than required minimum.
257 */
258 public PROSACRobustRangingRadioSourceEstimator2D(final double[] qualityScores, final Point2D initialPosition) {
259 super(initialPosition);
260 internalSetQualityScores(qualityScores);
261 }
262
263 /**
264 * Constructor.
265 * Sets radio signal readings belonging to the same radio source.
266 *
267 * @param qualityScores quality scores corresponding to each provided
268 * sample. The larger the score value the better
269 * the quality of the sample.
270 * @param readings radio signal readings belonging to the same radio source.
271 * @param initialPosition initial position to start the estimation of radio
272 * source position.
273 * @throws IllegalArgumentException if readings are not valid, quality scores
274 * is null, or length of quality scores is less than required minimum.
275 */
276 public PROSACRobustRangingRadioSourceEstimator2D(
277 final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
278 final Point2D initialPosition) {
279 super(readings, initialPosition);
280 internalSetQualityScores(qualityScores);
281 }
282
283 /**
284 * Constructor.
285 *
286 * @param qualityScores quality scores corresponding to each provided
287 * sample. The larger the score value the better
288 * the quality of the sample.
289 * @param initialPosition initial position to start the estimation of radio
290 * source position.
291 * @param listener listener in charge of attending events raised by this instance.
292 * @throws IllegalArgumentException if readings are not valid, quality scores
293 * is null, or length of quality scores is less than required minimum.
294 */
295 public PROSACRobustRangingRadioSourceEstimator2D(
296 final double[] qualityScores, final Point2D initialPosition,
297 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
298 super(initialPosition, listener);
299 internalSetQualityScores(qualityScores);
300 }
301
302 /**
303 * Constructor.
304 * Sets radio signal ranging readings belonging to the same radio source.
305 *
306 * @param qualityScores quality scores corresponding to each provided
307 * sample. The larger the score value the better
308 * the quality of the sample.
309 * @param readings radio signal ranging readings belonging to the same radio source.
310 * @param initialPosition initial position to start the estimation of radio source
311 * position.
312 * @param listener listener in charge of attending events raised by this instance.
313 * @throws IllegalArgumentException if readings are not valid, quality scores
314 * is null, or length of quality scores is less than required minimum.
315 */
316 public PROSACRobustRangingRadioSourceEstimator2D(
317 final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
318 final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
319 super(readings, initialPosition, listener);
320 internalSetQualityScores(qualityScores);
321 }
322
323 /**
324 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
325 * The threshold refers to the amount of error on distance between estimated position and distances
326 * provided for each sample.
327 *
328 * @return threshold to determine whether samples are inliers or not.
329 */
330 public double getThreshold() {
331 return threshold;
332 }
333
334 /**
335 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
336 * The threshold refers to the amount of error on distance between estimated position and distances
337 * provided for each sample.
338 *
339 * @param threshold threshold to determine whether samples are inliers or not.
340 * @throws IllegalArgumentException if provided value is equal or less than zero.
341 * @throws LockedException if this solver is locked.
342 */
343 public void setThreshold(final double threshold) throws LockedException {
344 if (isLocked()) {
345 throw new LockedException();
346 }
347 if (threshold <= MIN_THRESHOLD) {
348 throw new IllegalArgumentException();
349 }
350 this.threshold = threshold;
351 }
352
353 /**
354 * Returns quality scores corresponding to each pair of
355 * positions and distances (i.e. sample).
356 * The larger the score value the better the quality of the sample.
357 * This implementation always returns null.
358 * Subclasses using quality scores must implement proper behavior.
359 *
360 * @return quality scores corresponding to each sample.
361 */
362 @Override
363 public double[] getQualityScores() {
364 return qualityScores;
365 }
366
367 /**
368 * Sets quality scores corresponding to each pair of positions and
369 * distances (i.e. sample).
370 * The larger the score value the better the quality of the sample.
371 * This implementation makes no action.
372 * Subclasses using quality scores must implement proper behaviour.
373 *
374 * @param qualityScores quality scores corresponding to each pair of
375 * matched points.
376 * @throws IllegalArgumentException if provided quality scores length
377 * is smaller than minimum required samples.
378 * @throws LockedException if robust solver is locked because an
379 * estimation is already in progress.
380 */
381 @Override
382 public void setQualityScores(final double[] qualityScores) throws LockedException {
383 if (isLocked()) {
384 throw new LockedException();
385 }
386 internalSetQualityScores(qualityScores);
387 }
388
389 /**
390 * Indicates whether solver is ready to find a solution.
391 *
392 * @return true if solver is ready, false otherwise.
393 */
394 @Override
395 public boolean isReady() {
396 return super.isReady() && qualityScores != null && qualityScores.length == readings.size();
397 }
398
399 /**
400 * Indicates whether inliers must be computed and kept.
401 *
402 * @return true if inliers must be computed and kept, false if inliers
403 * only need to be computed but not kept.
404 */
405 public boolean isComputeAndKeepInliersEnabled() {
406 return computeAndKeepInliers;
407 }
408
409 /**
410 * Specifies whether inliers must be computed and kept.
411 *
412 * @param computeAndKeepInliers true if inliers must be computed and kept,
413 * false if inliers only need to be computed but not kept.
414 * @throws LockedException if this solver is locked.
415 */
416 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
417 if (isLocked()) {
418 throw new LockedException();
419 }
420 this.computeAndKeepInliers = computeAndKeepInliers;
421 }
422
423 /**
424 * Indicates whether residuals must be computed and kept.
425 *
426 * @return true if residuals must be computed and kept, false if residuals
427 * only need to be computed but not kept.
428 */
429 public boolean isComputeAndKeepResidualsEnabled() {
430 return computeAndKeepResiduals;
431 }
432
433 /**
434 * Specifies whether residuals must be computed and kept.
435 *
436 * @param computeAndKeepResiduals true if residuals must be computed and kept,
437 * false if residuals only need to be computed but not kept.
438 * @throws LockedException if this solver is locked.
439 */
440 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
441 if (isLocked()) {
442 throw new LockedException();
443 }
444 this.computeAndKeepResiduals = computeAndKeepResiduals;
445 }
446
447 /**
448 * Robustly estimates position for a radio source.
449 *
450 * @throws LockedException if instance is busy during estimation.
451 * @throws NotReadyException if estimator is not ready.
452 * @throws RobustEstimatorException if estimation fails for any reason
453 * (i.e. numerical instability, no solution available, etc).
454 */
455 @SuppressWarnings("DuplicatedCode")
456 @Override
457 public void estimate() throws LockedException, NotReadyException, RobustEstimatorException {
458 if (isLocked()) {
459 throw new LockedException();
460 }
461 if (!isReady()) {
462 throw new NotReadyException();
463 }
464
465 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<Solution<Point2D>>() {
466
467 @Override
468 public double[] getQualityScores() {
469 return qualityScores;
470 }
471
472 @Override
473 public double getThreshold() {
474 return threshold;
475 }
476
477 @Override
478 public int getTotalSamples() {
479 return readings.size();
480 }
481
482 @Override
483 public int getSubsetSize() {
484 return Math.max(preliminarySubsetSize, getMinReadings());
485 }
486
487 @Override
488 public void estimatePreliminarSolutions(
489 final int[] samplesIndices, final List<Solution<Point2D>> solutions) {
490 solvePreliminarySolutions(samplesIndices, solutions);
491 }
492
493 @Override
494 public double computeResidual(final Solution<Point2D> currentEstimation, final int i) {
495 return residual(currentEstimation, i);
496 }
497
498 @Override
499 public boolean isReady() {
500 return PROSACRobustRangingRadioSourceEstimator2D.this.isReady();
501 }
502
503 @Override
504 public void onEstimateStart(final RobustEstimator<Solution<Point2D>> estimator) {
505 // no action needed
506 }
507
508 @Override
509 public void onEstimateEnd(final RobustEstimator<Solution<Point2D>> estimator) {
510 // no action needed
511 }
512
513 @Override
514 public void onEstimateNextIteration(
515 final RobustEstimator<Solution<Point2D>> estimator, final int iteration) {
516 if (listener != null) {
517 listener.onEstimateNextIteration(
518 PROSACRobustRangingRadioSourceEstimator2D.this, iteration);
519 }
520 }
521
522 @Override
523 public void onEstimateProgressChange(
524 final RobustEstimator<Solution<Point2D>> estimator, final float progress) {
525 if (listener != null) {
526 listener.onEstimateProgressChange(
527 PROSACRobustRangingRadioSourceEstimator2D.this, progress);
528 }
529 }
530 });
531
532 try {
533 locked = true;
534
535 if (listener != null) {
536 listener.onEstimateStart(this);
537 }
538
539 inliersData = null;
540 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
541 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
542 innerEstimator.setConfidence(confidence);
543 innerEstimator.setMaxIterations(maxIterations);
544 innerEstimator.setProgressDelta(progressDelta);
545 final var result = innerEstimator.estimate();
546 inliersData = innerEstimator.getInliersData();
547 attemptRefine(result);
548
549 if (listener != null) {
550 listener.onEstimateEnd(this);
551 }
552
553 } catch (final com.irurueta.numerical.LockedException e) {
554 throw new LockedException(e);
555 } catch (final com.irurueta.numerical.NotReadyException e) {
556 throw new NotReadyException(e);
557 } finally {
558 locked = false;
559 }
560 }
561
562
563 /**
564 * Returns method being used for robust estimation.
565 *
566 * @return method being used for robust estimation.
567 */
568 @Override
569 public RobustEstimatorMethod getMethod() {
570 return RobustEstimatorMethod.PROSAC;
571 }
572
573 /**
574 * Sets quality scores corresponding to each provided sample.
575 * This method is used internally and does not check whether instance is
576 * locked or not.
577 *
578 * @param qualityScores quality scores to be set.
579 * @throws IllegalArgumentException if provided quality scores length
580 * is smaller than 3 samples.
581 */
582 private void internalSetQualityScores(final double[] qualityScores) {
583 if (qualityScores == null || qualityScores.length < getMinReadings()) {
584 throw new IllegalArgumentException();
585 }
586
587 this.qualityScores = qualityScores;
588 }
589 }