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