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.lateration;
17
18 import com.irurueta.geometry.Circle;
19 import com.irurueta.geometry.Point2D;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.numerical.robust.MSACRobustEstimator;
23 import com.irurueta.numerical.robust.MSACRobustEstimatorListener;
24 import com.irurueta.numerical.robust.RobustEstimator;
25 import com.irurueta.numerical.robust.RobustEstimatorException;
26 import com.irurueta.numerical.robust.RobustEstimatorMethod;
27
28 import java.util.List;
29
30 /**
31 * Robustly solves the lateration problem by finding the best pairs of 2D
32 * positions and distances among the provided ones using MSAC algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class MSACRobustLateration2DSolver extends RobustLateration2DSolver {
37
38 /**
39 * Constant defining default threshold to determine whether samples are
40 * inliers or not.
41 */
42 public static final double DEFAULT_THRESHOLD = 1e-2;
43
44 /**
45 * Minimum value that can be set as threshold.
46 * Threshold must be strictly greater than 0.0.
47 */
48 public static final double MIN_THRESHOLD = 0.0;
49
50 /**
51 * Threshold to determine whether samples are inliers or not when
52 * testing possible estimation solutions.
53 */
54 private double threshold = DEFAULT_THRESHOLD;
55
56 /**
57 * Constructor.
58 */
59 public MSACRobustLateration2DSolver() {
60 super();
61 }
62
63 /**
64 * Constructor.
65 *
66 * @param listener listener to be notified of events such as when estimation
67 * starts, ends or its progress significantly changes.
68 */
69 public MSACRobustLateration2DSolver(final RobustLaterationSolverListener<Point2D> listener) {
70 super(listener);
71 }
72
73 /**
74 * Constructor.
75 *
76 * @param positions known positions of static nodes.
77 * @param distances euclidean distances from static nodes to mobile node to be
78 * estimated.
79 * @throws IllegalArgumentException if either positions or distances are null,
80 * don't have the same length or their length is smaller than required (3 points).
81 */
82 public MSACRobustLateration2DSolver(final Point2D[] positions, final double[] distances) {
83 super(positions, distances);
84 }
85
86 /**
87 * Constructor.
88 *
89 * @param positions known positions of static nodes.
90 * @param distances euclidean distances from static nodes to mobile node to be
91 * estimated.
92 * @param distanceStandardDeviations standard deviations of provided measured distances.
93 * @throws IllegalArgumentException if either positions or distances are null,
94 * don't have the same length or their length is smaller than required (3 points).
95 */
96 public MSACRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
97 final double[] distanceStandardDeviations) {
98 super(positions, distances, distanceStandardDeviations);
99 }
100
101 /**
102 * Constructor.
103 *
104 * @param positions known positions of static nodes.
105 * @param distances euclidean distances from static nodes to mobile node.
106 * @param distanceStandardDeviations standard deviations of provided measured distances.
107 * @param listener listener to be notified of events such as when estimation starts,
108 * ends or its progress significantly changes.
109 * @throws IllegalArgumentException if either positions, distances or
110 * standard deviations are null, don't have the same length or their length is
111 * smaller than required (3 points).
112 */
113 public MSACRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
114 final double[] distanceStandardDeviations,
115 final RobustLaterationSolverListener<Point2D> listener) {
116 super(positions, distances, distanceStandardDeviations, listener);
117 }
118
119 /**
120 * Constructor.
121 *
122 * @param positions known positions of static nodes.
123 * @param distances euclidean distances from static nodes to mobile node.
124 * @param listener listener to be notified of events such as when estimation starts,
125 * ends or its progress significantly changes.
126 * @throws IllegalArgumentException if either positions or distances are null,
127 * don't have the same length or their length is smaller than required (3 points).
128 */
129 public MSACRobustLateration2DSolver(final Point2D[] positions, final double[] distances,
130 final RobustLaterationSolverListener<Point2D> listener) {
131 super(positions, distances, listener);
132 }
133
134 /**
135 * Constructor.
136 *
137 * @param circles circles defining positions and distances.
138 * @throws IllegalArgumentException if circles is null or if length of circles array
139 * is less than required (3 points).
140 */
141 public MSACRobustLateration2DSolver(final Circle[] circles) {
142 super(circles);
143 }
144
145 /**
146 * Constructor.
147 *
148 * @param circles circles defining positions and distances.
149 * @param distanceStandardDeviations standard deviations of provided measured distances.
150 * @throws IllegalArgumentException if circles is null, length of circles array is less
151 * than required (3 points) or don't have the same length.
152 */
153 public MSACRobustLateration2DSolver(
154 final Circle[] circles, final double[] distanceStandardDeviations) {
155 super(circles, distanceStandardDeviations);
156 }
157
158 /**
159 * Constructor.
160 *
161 * @param circles circles defining positions and distances.
162 * @param listener listener to be notified of events such as when estimation starts,
163 * ends or its progress significantly changes.
164 * @throws IllegalArgumentException if circles is null or if length of circles array
165 * is less than required (3 points).
166 */
167 public MSACRobustLateration2DSolver(
168 final Circle[] circles, final RobustLaterationSolverListener<Point2D> listener) {
169 super(circles, listener);
170 }
171
172 /**
173 * Constructor.
174 *
175 * @param circles circles defining positions and distances.
176 * @param distanceStandardDeviations standard deviations of provided measured distances.
177 * @param listener listener to be notified of events such as when estimation starts,
178 * ends or its progress significantly changes.
179 * @throws IllegalArgumentException if circles is null, length of circles array is less
180 * than required (3 points) or don't have the same length.
181 */
182 public MSACRobustLateration2DSolver(
183 final Circle[] circles, final double[] distanceStandardDeviations,
184 final RobustLaterationSolverListener<Point2D> listener) {
185 super(circles, distanceStandardDeviations, listener);
186 }
187
188 /**
189 * Returns threshold to determine whether samples are inliers or not.
190 *
191 * @return threshold to determine whether samples are inliers or not.
192 */
193 public double getThreshold() {
194 return threshold;
195 }
196
197 /**
198 * Sets threshold to determine whether samples are inliers or not.
199 *
200 * @param threshold threshold to be set.
201 * @throws IllegalArgumentException if provided value is equal or less than
202 * zero.
203 * @throws LockedException if robust estimator is locked because an
204 * estimation is already in progress.
205 */
206 public void setThreshold(final double threshold) throws LockedException {
207 if (isLocked()) {
208 throw new LockedException();
209 }
210 if (threshold <= MIN_THRESHOLD) {
211 throw new IllegalArgumentException();
212 }
213 this.threshold = threshold;
214 }
215
216 /**
217 * Solves the lateration problem.
218 *
219 * @return estimated position.
220 * @throws LockedException if instance is busy solving the lateration problem.
221 * @throws NotReadyException is solver is not ready.
222 * @throws RobustEstimatorException if estimation fails for any reason
223 * (i.e. numerical instability, no solution available, etc).
224 */
225 @Override
226 public Point2D solve() throws LockedException, NotReadyException, RobustEstimatorException {
227 if (isLocked()) {
228 throw new LockedException();
229 }
230 if (!isReady()) {
231 throw new NotReadyException();
232 }
233
234 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point2D>() {
235 @Override
236 public double getThreshold() {
237 return threshold;
238 }
239
240 @Override
241 public int getTotalSamples() {
242 return distances.length;
243 }
244
245 @Override
246 public int getSubsetSize() {
247 return preliminarySubsetSize;
248 }
249
250 @Override
251 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point2D> solutions) {
252 solvePreliminarySolutions(samplesIndices, solutions);
253 }
254
255 @Override
256 public double computeResidual(final Point2D currentEstimation, final int i) {
257 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
258 }
259
260 @Override
261 public boolean isReady() {
262 return MSACRobustLateration2DSolver.this.isReady();
263 }
264
265 @Override
266 public void onEstimateStart(final RobustEstimator<Point2D> estimator) {
267 // no action needed
268 }
269
270 @Override
271 public void onEstimateEnd(final RobustEstimator<Point2D> estimator) {
272 // no action needed
273 }
274
275 @Override
276 public void onEstimateNextIteration(final RobustEstimator<Point2D> estimator, final int iteration) {
277 if (listener != null) {
278 listener.onSolveNextIteration(MSACRobustLateration2DSolver.this, iteration);
279 }
280 }
281
282 @Override
283 public void onEstimateProgressChange(final RobustEstimator<Point2D> estimator, final float progress) {
284 if (listener != null) {
285 listener.onSolveProgressChange(MSACRobustLateration2DSolver.this, progress);
286 }
287 }
288 });
289
290 try {
291 locked = true;
292
293 if (listener != null) {
294 listener.onSolveStart(this);
295 }
296
297 inliersData = null;
298 innerEstimator.setConfidence(confidence);
299 innerEstimator.setMaxIterations(maxIterations);
300 innerEstimator.setProgressDelta(progressDelta);
301 var result = innerEstimator.estimate();
302 inliersData = innerEstimator.getInliersData();
303 result = attemptRefine(result);
304
305 if (listener != null) {
306 listener.onSolveEnd(this);
307 }
308
309 return result;
310
311 } catch (final com.irurueta.numerical.LockedException e) {
312 throw new LockedException(e);
313 } catch (final com.irurueta.numerical.NotReadyException e) {
314 throw new NotReadyException(e);
315 } finally {
316 locked = false;
317 }
318 }
319
320 /**
321 * Returns method being used for robust estimation.
322 *
323 * @return method being used for robust estimation.
324 */
325 @Override
326 public RobustEstimatorMethod getMethod() {
327 return RobustEstimatorMethod.MSAC;
328 }
329 }