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.Fingerprint;
21 import com.irurueta.navigation.indoor.RadioSource;
22 import com.irurueta.navigation.indoor.RadioSourceLocated;
23 import com.irurueta.navigation.indoor.Reading;
24
25 import java.util.List;
26
27 /**
28 * Robustly estimates 3D position, using RSSI readings first to obtain an initial coarse
29 * position estimation, and then ranging readings to refine such estimation.
30 * <p>
31 * This implementation is like SequentialRobustRangingAndRssiPositionEstimator but
32 * allows mixing different kinds of readings (ranging, RSSI or ranging+RSSI).
33 */
34 public class SequentialRobustMixedPositionEstimator3D extends SequentialRobustMixedPositionEstimator<Point3D> {
35
36 /**
37 * Constructor.
38 */
39 public SequentialRobustMixedPositionEstimator3D() {
40 super();
41 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
42 }
43
44 /**
45 * Constructor.
46 *
47 * @param sources located radio sources used for lateration.
48 * @throws IllegalArgumentException if provided sources is null or the number of
49 * provided sources is less than the required minimum.
50 */
51 public SequentialRobustMixedPositionEstimator3D(final List<? extends RadioSourceLocated<Point3D>> sources) {
52 super(sources);
53 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
54 }
55
56 /**
57 * Constructor.
58 *
59 * @param fingerprint fingerprint containing RSSI readings at an unknown location
60 * for provided located radio sources.
61 * @throws IllegalArgumentException if provided fingerprint is null.
62 */
63 public SequentialRobustMixedPositionEstimator3D(
64 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
65 super(fingerprint);
66 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
67 }
68
69 /**
70 * Constructor.
71 *
72 * @param sources located radio sources used for lateration.
73 * @param fingerprint fingerprint containing readings at an unknown location
74 * for provided located radio sources.
75 * @throws IllegalArgumentException if either provided sources or fingerprint is
76 * null or the number of provided sources is less
77 * than the required minimum.
78 */
79 public SequentialRobustMixedPositionEstimator3D(
80 final List<? extends RadioSourceLocated<Point3D>> sources,
81 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
82 super(sources, fingerprint);
83 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param listener listener in charge of handling events.
90 */
91 public SequentialRobustMixedPositionEstimator3D(
92 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
93 super(listener);
94 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param sources located radio sources used for lateration.
101 * @param listener listener in charge of handling events.
102 * @throws IllegalArgumentException if provided sources is null or the number of
103 * provided sources is less than the required
104 * minimum.
105 */
106 public SequentialRobustMixedPositionEstimator3D(
107 final List<? extends RadioSourceLocated<Point3D>> sources,
108 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
109 super(sources, listener);
110 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
111 }
112
113 /**
114 * Constructor.
115 *
116 * @param fingerprint fingerprint containing readings at an unknown location for
117 * provided located radio sources.
118 * @param listener listener in charge of handling events.
119 * @throws IllegalArgumentException if provided fingerprint is null.
120 */
121 public SequentialRobustMixedPositionEstimator3D(
122 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
123 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
124 super(fingerprint, listener);
125 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
126 }
127
128 /**
129 * Constructor.
130 *
131 * @param sources located radio sources used for lateration.
132 * @param fingerprint fingerprint containing readings at an unknown location
133 * for provided located radio sources.
134 * @param listener listener in charge of handling events.
135 * @throws IllegalArgumentException if either provided sources or fingerprint is
136 * null or the number of provided sources is less
137 * than the required minimum.
138 */
139 public SequentialRobustMixedPositionEstimator3D(
140 final List<? extends RadioSourceLocated<Point3D>> sources,
141 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
142 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
143 super(sources, fingerprint, listener);
144 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
145 }
146
147 /**
148 * Constructor.
149 *
150 * @param sourceQualityScores quality scores corresponding to each
151 * provided located radio source. The
152 * larger the score value the better the
153 * quality of the radio source.
154 * @param fingerprintReadingQualityScores quality scores corresponding to
155 * readings within provided fingerprint.
156 * The larger the score the better the
157 * quality of the reading.
158 */
159 public SequentialRobustMixedPositionEstimator3D(
160 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores) {
161 super(sourceQualityScores, fingerprintReadingQualityScores);
162 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
163 }
164
165 /**
166 * Constructor.
167 *
168 * @param sourceQualityScores quality scores corresponding to each
169 * provided located radio source. The
170 * larger the score value the better the
171 * quality of the radio source.
172 * @param fingerprintReadingQualityScores quality scores corresponding to readings
173 * within provided fingerprint. The larger
174 * the score the better the quality of the
175 * reading.
176 * @param sources located radio sources used for
177 * lateration.
178 * @throws IllegalArgumentException if provided sources is null or the number of
179 * provided sources is less than the required minimum.
180 */
181 public SequentialRobustMixedPositionEstimator3D(
182 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
183 final List<? extends RadioSourceLocated<Point3D>> sources) {
184 super(sourceQualityScores, fingerprintReadingQualityScores, sources);
185 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
186 }
187
188 /**
189 * Constructor.
190 *
191 * @param sourceQualityScores quality scores corresponding to each
192 * provided located radio source. The
193 * larger the score value the better the
194 * quality of the radio source.
195 * @param fingerprintReadingQualityScores quality scores corresponding to readings
196 * within provided fingerprint. The larger
197 * the score the better the quality of the
198 * reading.
199 * @param fingerprint fingerprint containing readings at an
200 * unknown location for provided located
201 * radio sources.
202 * @throws IllegalArgumentException if provided fingerprint is null.
203 */
204 public SequentialRobustMixedPositionEstimator3D(
205 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
206 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
207 super(sourceQualityScores, fingerprintReadingQualityScores, fingerprint);
208 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
209 }
210
211 /**
212 * Constructor.
213 *
214 * @param sourceQualityScores quality scores corresponding to each
215 * provided located radio source. The
216 * larger the score value the better the
217 * quality of the radio source.
218 * @param fingerprintReadingQualityScores quality scores corresponding to readings
219 * within provided fingerprint. The larger
220 * the score the better the quality of the
221 * reading.
222 * @param sources located radio sources used for
223 * lateration.
224 * @param fingerprint fingerprint containing readings at an
225 * unknown location for provided located
226 * radio sources.
227 * @throws IllegalArgumentException if either provided sources or fingerprint is null
228 * or the number of provided sources is less than the required minimum.
229 */
230 public SequentialRobustMixedPositionEstimator3D(
231 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
232 final List<? extends RadioSourceLocated<Point3D>> sources,
233 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint) {
234 super(sourceQualityScores, fingerprintReadingQualityScores, sources, fingerprint);
235 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
236 }
237
238 /**
239 * Constructor.
240 *
241 * @param sourceQualityScores quality scores corresponding to each
242 * provided located radio source. The
243 * larger the score value the better the
244 * quality of the radio source.
245 * @param fingerprintReadingQualityScores quality scores corresponding to readings
246 * within provided fingerprint. The larger
247 * the score the better the quality of the
248 * reading.
249 * @param listener listener in charge of handling events.
250 */
251 public SequentialRobustMixedPositionEstimator3D(
252 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
253 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
254 super(sourceQualityScores, fingerprintReadingQualityScores, listener);
255 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
256 }
257
258 /**
259 * Constructor.
260 *
261 * @param sourceQualityScores quality scores corresponding to each
262 * provided located radio source. The
263 * larger the score value the better the
264 * quality of the radio source.
265 * @param fingerprintReadingQualityScores quality scores corresponding to readings
266 * within provided fingerprint. The larger
267 * the score the better the quality of the
268 * reading.
269 * @param sources located radio sources used for
270 * lateration.
271 * @param listener listener in charge of handling events.
272 * @throws IllegalArgumentException if provided sources is null or the number of
273 * provided sources is less than the required minimum.
274 */
275 public SequentialRobustMixedPositionEstimator3D(
276 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
277 final List<? extends RadioSourceLocated<Point3D>> sources,
278 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
279 super(sourceQualityScores, fingerprintReadingQualityScores, sources, listener);
280 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
281 }
282
283 /**
284 * Constructor.
285 *
286 * @param sourceQualityScores quality scores corresponding to each
287 * provided located radio source. The
288 * larger the score value the better the
289 * quality of the radio source.
290 * @param fingerprintReadingQualityScores quality scores corresponding to
291 * readings within provided fingerprint.
292 * The larger the score the better the
293 * quality of the reading.
294 * @param fingerprint fingerprint containing readings at an
295 * unknown location for provided located
296 * radio sources.
297 * @param listener listener in charge of handling events.
298 * @throws IllegalArgumentException if provided fingerprint is null.
299 */
300 public SequentialRobustMixedPositionEstimator3D(
301 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
302 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
303 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
304 super(sourceQualityScores, fingerprintReadingQualityScores, fingerprint, listener);
305 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
306 }
307
308 /**
309 * Constructor.
310 *
311 * @param sourceQualityScores quality scores corresponding to each
312 * provided located radio source. The
313 * larger the score value the better the
314 * quality of the radio source.
315 * @param fingerprintReadingQualityScores quality scores corresponding to readings
316 * within provided fingerprint. The larger
317 * the score the better the quality of the
318 * reading.
319 * @param sources located radio sources used for
320 * lateration.
321 * @param fingerprint fingerprint containing readings at an
322 * unknown location for provided located radio
323 * sources.
324 * @param listener listener in charge of handling events.
325 * @throws IllegalArgumentException if either provided sources or fingerprint is null
326 * or the number of provided sources is less than the required minimum.
327 */
328 public SequentialRobustMixedPositionEstimator3D(
329 final double[] sourceQualityScores, final double[] fingerprintReadingQualityScores,
330 final List<? extends RadioSourceLocated<Point3D>> sources,
331 final Fingerprint<? extends RadioSource, ? extends Reading<? extends RadioSource>> fingerprint,
332 final SequentialRobustMixedPositionEstimatorListener<Point3D> listener) {
333 super(sourceQualityScores, fingerprintReadingQualityScores, sources, fingerprint, listener);
334 rangingPreliminarySubsetSize = rssiPreliminarySubsetSize = getMinRequiredSources();
335 }
336
337 /**
338 * Gets number of dimensions of provided and estimated points.
339 *
340 * @return number of dimensions of provided and estimated points.
341 */
342 @Override
343 public int getNumberOfDimensions() {
344 return Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
345 }
346
347 /**
348 * Gets minimum required number of located radio sources to perform lateration.
349 *
350 * @return minimum required number of located radio sources to perform
351 * lateration.
352 */
353 @Override
354 public int getMinRequiredSources() {
355 return Point3D.POINT3D_HOMOGENEOUS_COORDINATES_LENGTH;
356 }
357
358 /**
359 * Builds ranging internal estimator.
360 */
361 @Override
362 protected void buildRangingEstimator() {
363 rangingEstimator = RobustRangingPositionEstimator3D.create(rangingRobustMethod);
364 }
365
366 /**
367 * Builds RSSI internal estimator.
368 */
369 @Override
370 protected void buildRssiEstimator() {
371 rssiEstimator = RobustRssiPositionEstimator3D.create(rssiRobustMethod);
372 }
373
374 /**
375 * Setup ranging internal estimator.
376 *
377 * @throws LockedException if estimator is locked.
378 */
379 @Override
380 protected void setupRangingEstimator() throws LockedException {
381 super.setupRangingEstimator();
382 if (rangingThreshold != null) {
383 switch (rangingRobustMethod) {
384 case RANSAC:
385 ((RANSACRobustRangingPositionEstimator3D) rangingEstimator).setThreshold(rangingThreshold);
386 break;
387 case LMEDS:
388 ((LMedSRobustRangingPositionEstimator3D) rangingEstimator).setStopThreshold(rangingThreshold);
389 break;
390 case MSAC:
391 ((MSACRobustRangingPositionEstimator3D) rangingEstimator).setThreshold(rangingThreshold);
392 break;
393 case PROSAC:
394 ((PROSACRobustRangingPositionEstimator3D) rangingEstimator).setThreshold(rangingThreshold);
395 break;
396 case PROMEDS:
397 default:
398 ((PROMedSRobustRangingPositionEstimator3D) rangingEstimator).setStopThreshold(rangingThreshold);
399 break;
400 }
401 }
402 }
403
404 /**
405 * Setup RSSI internal estimator.
406 *
407 * @throws LockedException if estimator is locked.
408 */
409 @Override
410 protected void setupRssiEstimator() throws LockedException {
411 super.setupRssiEstimator();
412 if (rssiThreshold != null) {
413 switch (rssiRobustMethod) {
414 case RANSAC:
415 ((RANSACRobustRssiPositionEstimator3D) rssiEstimator).setThreshold(rssiThreshold);
416 break;
417 case LMEDS:
418 ((LMedSRobustRssiPositionEstimator3D) rssiEstimator).setStopThreshold(rssiThreshold);
419 break;
420 case MSAC:
421 ((MSACRobustRssiPositionEstimator3D) rssiEstimator).setThreshold(rssiThreshold);
422 break;
423 case PROSAC:
424 ((PROSACRobustRssiPositionEstimator3D) rssiEstimator).setThreshold(rssiThreshold);
425 break;
426 case PROMEDS:
427 default:
428 ((PROMedSRobustRssiPositionEstimator3D) rssiEstimator).setStopThreshold(rssiThreshold);
429 break;
430 }
431 }
432 }
433 }