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.Point3D;
19 import com.irurueta.geometry.Sphere;
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 3D
32 * positions and distances among the provided ones using MSAC algorithm to
33 * discard outliers.
34 */
35 @SuppressWarnings("Duplicates")
36 public class MSACRobustLateration3DSolver extends RobustLateration3DSolver {
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 MSACRobustLateration3DSolver() {
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 MSACRobustLateration3DSolver(final RobustLaterationSolverListener<Point3D> 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 (4 points).
81 */
82 public MSACRobustLateration3DSolver(final Point3D[] 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 (4 points).
95 */
96 public MSACRobustLateration3DSolver(final Point3D[] 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 (4 points).
112 */
113 public MSACRobustLateration3DSolver(
114 final Point3D[] positions, final double[] distances,
115 final double[] distanceStandardDeviations,
116 final RobustLaterationSolverListener<Point3D> listener) {
117 super(positions, distances, distanceStandardDeviations, listener);
118 }
119
120 /**
121 * Constructor.
122 *
123 * @param positions known positions of static nodes.
124 * @param distances euclidean distances from static nodes to mobile node.
125 * @param listener listener to be notified of events such as when estimation starts,
126 * ends or its progress significantly changes.
127 * @throws IllegalArgumentException if either positions or distances are null,
128 * don't have the same length or their length is smaller than required (4 points).
129 */
130 public MSACRobustLateration3DSolver(final Point3D[] positions, final double[] distances,
131 final RobustLaterationSolverListener<Point3D> listener) {
132 super(positions, distances, listener);
133 }
134
135 /**
136 * Constructor.
137 *
138 * @param spheres spheres defining positions and distances.
139 * @throws IllegalArgumentException if spheres is null or if length of spheres array
140 * is less than required (4 points).
141 */
142 public MSACRobustLateration3DSolver(final Sphere[] spheres) {
143 super(spheres);
144 }
145
146 /**
147 * Constructor.
148 *
149 * @param spheres spheres defining positions and distances.
150 * @param distanceStandardDeviations standard deviations of provided measured distances.
151 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
152 * than required (4 points) or don't have the same length.
153 */
154 public MSACRobustLateration3DSolver(final Sphere[] spheres,
155 final double[] distanceStandardDeviations) {
156 super(spheres, distanceStandardDeviations);
157 }
158
159 /**
160 * Constructor.
161 *
162 * @param spheres spheres defining positions and distances.
163 * @param listener listener to be notified of events such as when estimation starts,
164 * ends or its progress significantly changes.
165 * @throws IllegalArgumentException if spheres is null or if length of spheres array
166 * is less than required (4 points).
167 */
168 public MSACRobustLateration3DSolver(
169 final Sphere[] spheres, final RobustLaterationSolverListener<Point3D> listener) {
170 super(spheres, listener);
171 }
172
173 /**
174 * Constructor.
175 *
176 * @param spheres spheres defining positions and distances.
177 * @param distanceStandardDeviations standard deviations of provided measured distances.
178 * @param listener listener to be notified of events such as when estimation starts,
179 * ends or its progress significantly changes.
180 * @throws IllegalArgumentException if spheres is null, length of spheres array is less
181 * than required (4 points) or don't have the same length.
182 */
183 public MSACRobustLateration3DSolver(
184 final Sphere[] spheres, final double[] distanceStandardDeviations,
185 final RobustLaterationSolverListener<Point3D> listener) {
186 super(spheres, distanceStandardDeviations, listener);
187 }
188
189 /**
190 * Returns threshold to determine whether samples are inliers or not.
191 *
192 * @return threshold to determine whether samples are inliers or not.
193 */
194 public double getThreshold() {
195 return threshold;
196 }
197
198 /**
199 * Sets threshold to determine whether samples are inliers or not.
200 *
201 * @param threshold threshold to be set.
202 * @throws IllegalArgumentException if provided value is equal or less than
203 * zero.
204 * @throws LockedException if robust estimator is locked because an
205 * estimation is already in progress.
206 */
207 public void setThreshold(final double threshold) throws LockedException {
208 if (isLocked()) {
209 throw new LockedException();
210 }
211 if (threshold <= MIN_THRESHOLD) {
212 throw new IllegalArgumentException();
213 }
214 this.threshold = threshold;
215 }
216
217 /**
218 * Solves the lateration problem.
219 *
220 * @return estimated position.
221 * @throws LockedException if instance is busy solving the lateration problem.
222 * @throws NotReadyException is solver is not ready.
223 * @throws RobustEstimatorException if estimation fails for any reason
224 * (i.e. numerical instability, no solution available, etc).
225 */
226 @Override
227 public Point3D solve() throws LockedException, NotReadyException, RobustEstimatorException {
228 if (isLocked()) {
229 throw new LockedException();
230 }
231 if (!isReady()) {
232 throw new NotReadyException();
233 }
234
235 final var innerEstimator = new MSACRobustEstimator<>(new MSACRobustEstimatorListener<Point3D>() {
236 @Override
237 public double getThreshold() {
238 return threshold;
239 }
240
241 @Override
242 public int getTotalSamples() {
243 return distances.length;
244 }
245
246 @Override
247 public int getSubsetSize() {
248 return preliminarySubsetSize;
249 }
250
251 @Override
252 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Point3D> solutions) {
253 solvePreliminarySolutions(samplesIndices, solutions);
254 }
255
256 @Override
257 public double computeResidual(final Point3D currentEstimation, final int i) {
258 return Math.abs(currentEstimation.distanceTo(positions[i]) - distances[i]);
259 }
260
261 @Override
262 public boolean isReady() {
263 return MSACRobustLateration3DSolver.this.isReady();
264 }
265
266 @Override
267 public void onEstimateStart(final RobustEstimator<Point3D> estimator) {
268 // no action needed
269 }
270
271 @Override
272 public void onEstimateEnd(final RobustEstimator<Point3D> estimator) {
273 // no action needed
274 }
275
276 @Override
277 public void onEstimateNextIteration(final RobustEstimator<Point3D> estimator, final int iteration) {
278 if (listener != null) {
279 listener.onSolveNextIteration(MSACRobustLateration3DSolver.this, iteration);
280 }
281 }
282
283 @Override
284 public void onEstimateProgressChange(final RobustEstimator<Point3D> estimator, final float progress) {
285 if (listener != null) {
286 listener.onSolveProgressChange(MSACRobustLateration3DSolver.this, progress);
287 }
288 }
289 });
290
291 try {
292 locked = true;
293
294 if (listener != null) {
295 listener.onSolveStart(this);
296 }
297
298 inliersData = null;
299 innerEstimator.setConfidence(confidence);
300 innerEstimator.setMaxIterations(maxIterations);
301 innerEstimator.setProgressDelta(progressDelta);
302 var result = innerEstimator.estimate();
303 inliersData = innerEstimator.getInliersData();
304 result = attemptRefine(result);
305
306 if (listener != null) {
307 listener.onSolveEnd(this);
308 }
309
310 return result;
311
312 } catch (final com.irurueta.numerical.LockedException e) {
313 throw new LockedException(e);
314 } catch (final com.irurueta.numerical.NotReadyException e) {
315 throw new NotReadyException(e);
316 } finally {
317 locked = false;
318 }
319 }
320
321 /**
322 * Returns method being used for robust estimation.
323 *
324 * @return method being used for robust estimation.
325 */
326 @Override
327 public RobustEstimatorMethod getMethod() {
328 return RobustEstimatorMethod.MSAC;
329 }
330 }