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.MSACRobustEstimator;
24 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
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 MSAC
34 * algorithm.
35 *
36 * @param <S> a {@link RadioSource} type.
37 */
38 public class MSACRobustRangingRadioSourceEstimator2D<S extends RadioSource> extends
39 RobustRangingRadioSourceEstimator2D<S> {
40
41 /**
42 * Constant defining default threshold to determine whether samples are
43 * inliers or not.
44 */
45 public static final double DEFAULT_THRESHOLD = 0.1;
46
47 /**
48 * Minimum value that can be set as threshold.
49 * Threshold must be strictly greater than 0.0.
50 */
51 public static final double MIN_THRESHOLD = 0.0;
52
53 /**
54 * Threshold to determine whether samples are inliers or not when
55 * testing possible estimation solutions.
56 */
57 private double threshold = DEFAULT_THRESHOLD;
58
59 /**
60 * Constructor.
61 */
62 public MSACRobustRangingRadioSourceEstimator2D() {
63 super();
64 }
65
66 /**
67 * Constructor.
68 * <p>
69 * Sets radio signal ranging readings belonging to the same radio source.
70 *
71 * @param readings radio signal ranging readings belonging to the same
72 * radio source.
73 * @throws IllegalArgumentException if readings are not valid.
74 */
75 public MSACRobustRangingRadioSourceEstimator2D(final List<? extends RangingReadingLocated<S, Point2D>> readings) {
76 super(readings);
77 }
78
79 /**
80 * Constructor.
81 *
82 * @param listener listener in charge of attending events raised by this instance.
83 */
84 public MSACRobustRangingRadioSourceEstimator2D(
85 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
86 super(listener);
87 }
88
89 /**
90 * Constructor.
91 * Sets radio signal readings belonging to the same radio source.
92 *
93 * @param readings radio signal readings belonging to the same radio source.
94 * @param listener listener in charge of attending events raised by this instance.
95 * @throws IllegalArgumentException if readings are not valid.
96 */
97 public MSACRobustRangingRadioSourceEstimator2D(
98 final List<? extends RangingReadingLocated<S, Point2D>> readings,
99 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
100 super(readings, listener);
101 }
102
103 /**
104 * Constructor.
105 *
106 * @param initialPosition initial position to start the estimation or radio
107 * source position.
108 */
109 public MSACRobustRangingRadioSourceEstimator2D(final Point2D initialPosition) {
110 this.initialPosition = initialPosition;
111 }
112
113 /**
114 * Constructor.
115 * Sets radio signal readings belonging to the same radio source.
116 *
117 * @param readings radio signal readings belonging to the same radio source.
118 * @param initialPosition initial position to start the estimation of radio
119 * source position.
120 * @throws IllegalArgumentException if readings are not valid.
121 */
122 public MSACRobustRangingRadioSourceEstimator2D(
123 final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition) {
124 super(readings, initialPosition);
125 }
126
127 /**
128 * Constructor.
129 *
130 * @param initialPosition initial position to start the estimation of radio
131 * source position.
132 * @param listener listener in charge of attending events raised by this instance.
133 */
134 public MSACRobustRangingRadioSourceEstimator2D(
135 final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
136 super(initialPosition, listener);
137 }
138
139 /**
140 * Constructor.
141 * Sets radio signal ranging readings belonging to the same radio source.
142 *
143 * @param readings radio signal ranging readings belonging to the same radio source.
144 * @param initialPosition initial position to start the estimation of radio source
145 * position.
146 * @param listener listener in charge of attending events raised by this instance.
147 * @throws IllegalArgumentException if readings are not valid.
148 */
149 public MSACRobustRangingRadioSourceEstimator2D(
150 final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
151 final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
152 super(readings, initialPosition, listener);
153 }
154
155 /**
156 * Returns threshold to determine whether samples are inliers or not.
157 *
158 * @return threshold to determine whether samples are inliers or not.
159 */
160 public double getThreshold() {
161 return threshold;
162 }
163
164 /**
165 * Sets threshold to determine whether samples are inliers or not.
166 *
167 * @param threshold threshold to be set.
168 * @throws IllegalArgumentException if provided value is equal or less than
169 * zero.
170 * @throws LockedException if robust estimator is locked because an
171 * estimation is already in progress.
172 */
173 public void setThreshold(final double threshold) throws LockedException {
174 if (isLocked()) {
175 throw new LockedException();
176 }
177 if (threshold <= MIN_THRESHOLD) {
178 throw new IllegalArgumentException();
179 }
180 this.threshold = threshold;
181 }
182
183 /**
184 * Robustly estimates position for a radio source.
185 *
186 * @throws LockedException if instance is busy during estimation.
187 * @throws NotReadyException if estimator is not ready.
188 * @throws RobustEstimatorException if estimation fails for any reason
189 * (i.e. numerical instability, no solution available, etc).
190 */
191 @SuppressWarnings("DuplicatedCode")
192 @Override
193 public void estimate() throws LockedException, NotReadyException,
194 RobustEstimatorException {
195 if (isLocked()) {
196 throw new LockedException();
197 }
198 if (!isReady()) {
199 throw new NotReadyException();
200 }
201
202 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Solution<Point2D>>() {
203 @Override
204 public double getThreshold() {
205 return threshold;
206 }
207
208 @Override
209 public int getTotalSamples() {
210 return readings.size();
211 }
212
213 @Override
214 public int getSubsetSize() {
215 return Math.max(preliminarySubsetSize, getMinReadings());
216 }
217
218 @Override
219 public void estimatePreliminarSolutions(
220 final int[] sampleIndices, final List<Solution<Point2D>> solutions) {
221 solvePreliminarySolutions(sampleIndices, solutions);
222 }
223
224 @Override
225 public double computeResidual(final Solution<Point2D> currentEstimation, final int i) {
226 return residual(currentEstimation, i);
227 }
228
229 @Override
230 public boolean isReady() {
231 return MSACRobustRangingRadioSourceEstimator2D.this.isReady();
232 }
233
234 @Override
235 public void onEstimateStart(final RobustEstimator<Solution<Point2D>> estimator) {
236 // no action needed
237 }
238
239 @Override
240 public void onEstimateEnd(final RobustEstimator<Solution<Point2D>> estimator) {
241 // no action needed
242 }
243
244 @Override
245 public void onEstimateNextIteration(
246 final RobustEstimator<Solution<Point2D>> estimator, final int iteration) {
247 if (listener != null) {
248 listener.onEstimateNextIteration(
249 MSACRobustRangingRadioSourceEstimator2D.this, iteration);
250 }
251 }
252
253 @Override
254 public void onEstimateProgressChange(
255 final RobustEstimator<Solution<Point2D>> robustEstimator, final float progress) {
256 if (listener != null) {
257 listener.onEstimateProgressChange(
258 MSACRobustRangingRadioSourceEstimator2D.this, progress);
259 }
260 }
261 });
262
263 try {
264 locked = true;
265
266 if (listener != null) {
267 listener.onEstimateStart(this);
268 }
269
270 inliersData = null;
271 innerEstimator.setConfidence(confidence);
272 innerEstimator.setMaxIterations(maxIterations);
273 innerEstimator.setProgressDelta(progressDelta);
274 final var result = innerEstimator.estimate();
275 inliersData = innerEstimator.getInliersData();
276 attemptRefine(result);
277
278 if (listener != null) {
279 listener.onEstimateEnd(this);
280 }
281
282 } catch (final com.irurueta.numerical.LockedException e) {
283 throw new LockedException(e);
284 } catch (final com.irurueta.numerical.NotReadyException e) {
285 throw new NotReadyException(e);
286 } finally {
287 locked = false;
288 }
289 }
290
291 /**
292 * Returns method being used for robust estimation.
293 *
294 * @return method being used for robust estimation.
295 */
296 @Override
297 public RobustEstimatorMethod getMethod() {
298 return RobustEstimatorMethod.MSAC;
299 }
300 }