1 /*
2 * Copyright (C) 2019 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.position;
17
18 import com.irurueta.geometry.Point3D;
19 import com.irurueta.navigation.LockedException;
20 import com.irurueta.navigation.indoor.RadioSource;
21 import com.irurueta.navigation.indoor.RadioSourceLocated;
22 import com.irurueta.navigation.indoor.RangingAndRssiFingerprint;
23 import com.irurueta.navigation.indoor.RangingAndRssiReading;
24 import com.irurueta.navigation.lateration.LMedSRobustLateration3DSolver;
25 import com.irurueta.numerical.robust.RobustEstimatorMethod;
26
27 import java.util.List;
28
29 /**
30 * Robustly estimates 3D position using located radio sources and their ranging+RSSI
31 * readings at unknown locations and using LMedS algorithm to discard outliers.
32 * This kind of estimator can be used to robustly determine the 3D position of a given
33 * device by getting ranging+RSSI readings at an unknown location of different radio
34 * sources whose 3D locations are known.
35 */
36 public class LMedSRobustRangingAndRssiPositionEstimator3D extends RobustRangingAndRssiPositionEstimator3D {
37
38 /**
39 * Constructor.
40 */
41 public LMedSRobustRangingAndRssiPositionEstimator3D() {
42 super();
43 init();
44 }
45
46 /**
47 * Constructor.
48 *
49 * @param sources located radio sources used for lateration.
50 * @throws IllegalArgumentException if provided sources is null or the number of
51 * provided sources is less than the required minimum.
52 */
53 public LMedSRobustRangingAndRssiPositionEstimator3D(final List<? extends RadioSourceLocated<Point3D>> sources) {
54 super();
55 init();
56 internalSetSources(sources);
57 }
58
59 /**
60 * Constructor.
61 *
62 * @param fingerprint fingerprint containing ranging+RSSI readings at an unknown
63 * location for provided located radio sources.
64 * @throws IllegalArgumentException if provided fingerprint is null.
65 */
66 public LMedSRobustRangingAndRssiPositionEstimator3D(
67 final RangingAndRssiFingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
68 extends RadioSource>> fingerprint) {
69 super();
70 init();
71 internalSetFingerprint(fingerprint);
72 }
73
74 /**
75 * Constructor.
76 *
77 * @param sources located radio sources used for lateration.
78 * @param fingerprint fingerprint containing ranging+RSSI readings at an unknown
79 * location for provided located radio sources.
80 * @throws IllegalArgumentException if either provided sources or fingerprint is null
81 * or the number of provided sources is less than the required minimum.
82 */
83 public LMedSRobustRangingAndRssiPositionEstimator3D(
84 final List<? extends RadioSourceLocated<Point3D>> sources,
85 final RangingAndRssiFingerprint<? extends RadioSource, ? extends RangingAndRssiReading<?
86 extends RadioSource>> fingerprint) {
87 super();
88 init();
89 internalSetSources(sources);
90 internalSetFingerprint(fingerprint);
91 }
92
93 /**
94 * Constructor.
95 *
96 * @param listener listener in charge of handling events.
97 */
98 public LMedSRobustRangingAndRssiPositionEstimator3D(
99 final RobustRangingAndRssiPositionEstimatorListener<Point3D> listener) {
100 super(listener);
101 init();
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param sources located radio sources used for lateration.
108 * @param listener listener in charge of handling events.
109 * @throws IllegalArgumentException if provided sources is null or the number of
110 * provided sources is less than the required minimum.
111 */
112 public LMedSRobustRangingAndRssiPositionEstimator3D(
113 final List<? extends RadioSourceLocated<Point3D>> sources,
114 final RobustRangingAndRssiPositionEstimatorListener<Point3D> listener) {
115 super(listener);
116 init();
117 internalSetSources(sources);
118 }
119
120 /**
121 * Constructor.
122 *
123 * @param fingerprint fingerprint containing ranging+RSSI readings at an unknown
124 * location for provided location radio sources.
125 * @param listener listener in charge of handling events.
126 * @throws IllegalArgumentException if provided fingerprint is null.
127 */
128 public LMedSRobustRangingAndRssiPositionEstimator3D(
129 final RangingAndRssiFingerprint<? extends RadioSource, ? extends RangingAndRssiReading<? extends RadioSource>> fingerprint,
130 final RobustRangingAndRssiPositionEstimatorListener<Point3D> listener) {
131 super(listener);
132 init();
133 internalSetFingerprint(fingerprint);
134 }
135
136 /**
137 * Constructor.
138 *
139 * @param sources located radio sources used for lateration.
140 * @param fingerprint fingerprint containing ranging+RSSI readings at an unknown
141 * location for provided located radio sources.
142 * @param listener listener in charge of handling events.
143 * @throws IllegalArgumentException if either provided sources or fingerprint is
144 * null or the number of provided sources is less than the required minimum.
145 */
146 public LMedSRobustRangingAndRssiPositionEstimator3D(
147 final List<? extends RadioSourceLocated<Point3D>> sources,
148 final RangingAndRssiFingerprint<? extends RadioSource, ? extends RangingAndRssiReading<? extends RadioSource>> fingerprint,
149 final RobustRangingAndRssiPositionEstimatorListener<Point3D> listener) {
150 super(listener);
151 init();
152 internalSetSources(sources);
153 internalSetFingerprint(fingerprint);
154 }
155
156 /**
157 * Returns threshold to be used to keep the algorithm iterating in case that
158 * best estimated threshold using median of residuals is not small enough.
159 * Once a solution is found that generates a threshold below this value, the
160 * algorithm will stop.
161 * The stop threshold can be used to prevent the LMedS algorithm to iterate
162 * too many times in cases where samples have a very similar accuracy.
163 * For instance, in cases where proportion of outliers is very small (close
164 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
165 * iterate for a long time trying to find the best solution when indeed
166 * there is no need to do that if a reasonable threshold has already been
167 * reached.
168 * Because of this behaviour the stop threshold can be set to a value much
169 * lower than the one typically used in RANSAC, and yet the algorithm could
170 * still produce even smaller thresholds in estimated results.
171 *
172 * @return stop threshold to stop the algorithm prematurely when a certain
173 * accuracy has been reached.
174 */
175 public double getStopThreshold() {
176 return ((LMedSRobustLateration3DSolver) laterationSolver).getStopThreshold();
177 }
178
179 /**
180 * Sets threshold to be used to keep the algorithm iterating in case that
181 * best estimated threshold using median of residuals is not small enough.
182 * Once a solution is found that generates a threshold below this value,
183 * the algorithm will stop.
184 * The stop threshold can be used to prevent the LMedS algorithm to iterate
185 * too many times in cases where samples have a very similar accuracy.
186 * For instance, in cases where proportion of outliers is very small (close
187 * to 0%), and samples are very accurate (i.e. 1e-6), the algorithm would
188 * iterate for a long time trying to find the best solution when indeed
189 * there is no need to do that if a reasonable threshold has already been
190 * reached.
191 * Because of this behaviour the stop threshold can be set to a value much
192 * lower than the one typically used in RANSAC, and yet the algorithm could
193 * still produce even smaller thresholds in estimated results.
194 *
195 * @param stopThreshold stop threshold to stop the algorithm prematurely
196 * when a certain accuracy has been reached.
197 * @throws IllegalArgumentException if provided value is zero or negative.
198 * @throws LockedException if this solver is locked.
199 */
200 public void setStopThreshold(final double stopThreshold) throws LockedException {
201 ((LMedSRobustLateration3DSolver) laterationSolver).setStopThreshold(stopThreshold);
202 }
203
204 /**
205 * Returns method being used for robust estimation.
206 *
207 * @return method being used for robust estimation.
208 */
209 @Override
210 public RobustEstimatorMethod getMethod() {
211 return RobustEstimatorMethod.LMEDS;
212 }
213
214 /**
215 * Initializes robust lateration solver.
216 */
217 private void init() {
218 laterationSolver = new LMedSRobustLateration3DSolver(trilaterationSolverListener);
219 }
220 }
221