1 /*
2 * Copyright (C) 2020 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.inertial.calibration.magnetometer;
17
18 import com.irurueta.navigation.LockedException;
19 import com.irurueta.navigation.NotReadyException;
20 import com.irurueta.navigation.inertial.calibration.CalibrationException;
21 import com.irurueta.navigation.inertial.calibration.StandardDeviationFrameBodyMagneticFluxDensity;
22 import com.irurueta.numerical.robust.PROSACRobustEstimator;
23 import com.irurueta.numerical.robust.PROSACRobustEstimatorListener;
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.io.IOException;
29 import java.util.List;
30
31 /**
32 * Robustly estimates magnetometer hard-iron biases, soft-iron cross
33 * couplings and scaling factors using PROSAC algorithm.
34 * <p>
35 * To use this calibrator at least 4 measurements at different known
36 * frames must be provided. In other words, magnetometer samples must
37 * be obtained at 4 different positions or orientations.
38 * Notice that frame velocities are ignored by this calibrator.
39 * <p>
40 * Measured magnetic flux density is assumed to follow the model shown below:
41 * <pre>
42 * mBmeas = bm + (I + Mm) * mBtrue + w
43 * </pre>
44 * Where:
45 * - mBmeas is the measured magnetic flux density. This is a 3x1 vector.
46 * - bm is magnetometer hard-iron bias. Ideally, on a perfect magnetometer,
47 * this should be a 3x1 zero vector.
48 * - I is the 3x3 identity matrix.
49 * - Mm is the 3x3 soft-iron matrix containing cross-couplings and scaling
50 * factors. Ideally, on a perfect magnetometer, this should be a 3x3 zero
51 * matrix.
52 * - mBtrue is ground-truth magnetic flux density. This is a 3x1 vector.
53 * - w is measurement noise. This is a 3x1 vector.
54 */
55 public class PROSACRobustKnownFrameMagnetometerCalibrator extends RobustKnownFrameMagnetometerCalibrator {
56
57 /**
58 * Constant defining default threshold to determine whether samples are inliers or not.
59 */
60 public static final double DEFAULT_THRESHOLD = 500e-9;
61
62 /**
63 * Minimum value that can be set as threshold.
64 * Threshold must be strictly greater than 0.0.
65 */
66 public static final double MIN_THRESHOLD = 0.0;
67
68 /**
69 * Indicates that by default inliers will only be computed but not kept.
70 */
71 public static final boolean DEFAULT_COMPUTE_AND_KEEP_INLIERS = false;
72
73 /**
74 * Indicates that by default residuals will only be computed but not kept.
75 */
76 public static final boolean DEFAULT_COMPUTE_AND_KEEP_RESIDUALS = false;
77
78 /**
79 * Threshold to determine whether samples are inliers or not when testing possible solutions.
80 * The threshold refers to the amount of error on distance between estimated position and
81 * distances provided for each sample.
82 */
83 private double threshold = DEFAULT_THRESHOLD;
84
85 /**
86 * Indicates whether inliers must be computed and kept.
87 */
88 private boolean computeAndKeepInliers = DEFAULT_COMPUTE_AND_KEEP_INLIERS;
89
90 /**
91 * Indicates whether residuals must be computed and kept.
92 */
93 private boolean computeAndKeepResiduals = DEFAULT_COMPUTE_AND_KEEP_RESIDUALS;
94
95 /**
96 * Quality scores corresponding to each provided sample.
97 * The larger the score value the better the quality of the sample.
98 */
99 private double[] qualityScores;
100
101 /**
102 * Constructor.
103 */
104 public PROSACRobustKnownFrameMagnetometerCalibrator() {
105 super();
106 }
107
108 /**
109 * Constructor.
110 *
111 * @param listener listener to be notified of events such as when estimation
112 * starts, ends or its progress significantly changes.
113 */
114 public PROSACRobustKnownFrameMagnetometerCalibrator(final RobustKnownFrameMagnetometerCalibratorListener listener) {
115 super(listener);
116 }
117
118 /**
119 * Constructor.
120 *
121 * @param measurements list of body magnetic flux density measurements with standard
122 * deviations taken at different frames (positions and
123 * orientations).
124 */
125 public PROSACRobustKnownFrameMagnetometerCalibrator(
126 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) {
127 super(measurements);
128 }
129
130 /**
131 * Constructor.
132 *
133 * @param measurements list of body magnetic flux density measurements with standard
134 * deviations taken at different frames (positions and
135 * orientations).
136 * @param listener listener to handle events raised by this calibrator.
137 */
138 public PROSACRobustKnownFrameMagnetometerCalibrator(
139 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
140 final RobustKnownFrameMagnetometerCalibratorListener listener) {
141 super(measurements, listener);
142 }
143
144 /**
145 * Constructor.
146 *
147 * @param commonAxisUsed indicates whether z-axis is assumed to be common
148 * for the accelerometer, gyroscope and magnetometer.
149 */
150 public PROSACRobustKnownFrameMagnetometerCalibrator(final boolean commonAxisUsed) {
151 super(commonAxisUsed);
152 }
153
154 /**
155 * Constructor.
156 *
157 * @param commonAxisUsed indicates whether z-axis is assumed to be common
158 * for the accelerometer, gyroscope and magnetometer.
159 * @param listener listener to handle events raised by this calibrator.
160 */
161 public PROSACRobustKnownFrameMagnetometerCalibrator(
162 final boolean commonAxisUsed, final RobustKnownFrameMagnetometerCalibratorListener listener) {
163 super(commonAxisUsed, listener);
164 }
165
166 /**
167 * Constructor.
168 *
169 * @param measurements list of body magnetic flux density measurements with standard
170 * deviations taken at different frames (positions and
171 * orientations).
172 * @param commonAxisUsed indicates whether z-axis is assumed to be common
173 * for the accelerometer, gyroscope and magnetometer.
174 */
175 public PROSACRobustKnownFrameMagnetometerCalibrator(
176 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed) {
177 super(measurements, commonAxisUsed);
178 }
179
180 /**
181 * Constructor.
182 *
183 * @param measurements list of body magnetic flux density measurements with standard
184 * deviations taken at different frames (positions and
185 * orientations).
186 * @param commonAxisUsed indicates whether z-axis is assumed to be common
187 * for the accelerometer, gyroscope and magnetometer.
188 * @param listener listener to handle events raised by this calibrator.
189 */
190 public PROSACRobustKnownFrameMagnetometerCalibrator(
191 final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements, final boolean commonAxisUsed,
192 final RobustKnownFrameMagnetometerCalibratorListener listener) {
193 super(measurements, commonAxisUsed, listener);
194 }
195
196 /**
197 * Constructor.
198 *
199 * @param qualityScores quality scores corresponding to each provided
200 * measurement. The larger the score value the better
201 * the quality of the sample.
202 * @throws IllegalArgumentException if provided quality scores length
203 * is smaller than 4 samples.
204 */
205 public PROSACRobustKnownFrameMagnetometerCalibrator(final double[] qualityScores) {
206 super();
207 internalSetQualityScores(qualityScores);
208 }
209
210 /**
211 * Constructor.
212 *
213 * @param qualityScores quality scores corresponding to each provided
214 * measurement. The larger the score value the better
215 * the quality of the sample.
216 * @param listener listener to be notified of events such as when estimation
217 * starts, ends or its progress significantly changes.
218 * @throws IllegalArgumentException if provided quality scores length
219 * is smaller than 4 samples.
220 */
221 public PROSACRobustKnownFrameMagnetometerCalibrator(
222 final double[] qualityScores, final RobustKnownFrameMagnetometerCalibratorListener listener) {
223 super(listener);
224 internalSetQualityScores(qualityScores);
225 }
226
227 /**
228 * Constructor.
229 *
230 * @param qualityScores quality scores corresponding to each provided
231 * measurement. The larger the score value the better
232 * the quality of the sample.
233 * @param measurements list of body magnetic flux density measurements with standard
234 * deviations taken at different frames (positions and
235 * orientations).
236 * @throws IllegalArgumentException if provided quality scores length
237 * is smaller than 4 samples.
238 */
239 public PROSACRobustKnownFrameMagnetometerCalibrator(
240 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements) {
241 super(measurements);
242 internalSetQualityScores(qualityScores);
243 }
244
245 /**
246 * Constructor.
247 *
248 * @param qualityScores quality scores corresponding to each provided
249 * measurement. The larger the score value the better
250 * the quality of the sample.
251 * @param measurements list of body magnetic flux density measurements with standard
252 * deviations taken at different frames (positions and
253 * orientations).
254 * @param listener listener to handle events raised by this calibrator.
255 * @throws IllegalArgumentException if provided quality scores length
256 * is smaller than 4 samples.
257 */
258 public PROSACRobustKnownFrameMagnetometerCalibrator(
259 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
260 final RobustKnownFrameMagnetometerCalibratorListener listener) {
261 super(measurements, listener);
262 internalSetQualityScores(qualityScores);
263 }
264
265 /**
266 * Constructor.
267 *
268 * @param qualityScores quality scores corresponding to each provided
269 * measurement. The larger the score value the better
270 * the quality of the sample.
271 * @param commonAxisUsed indicates whether z-axis is assumed to be common
272 * for the accelerometer, gyroscope and magnetometer.
273 * @throws IllegalArgumentException if provided quality scores length
274 * is smaller than 4 samples.
275 */
276 public PROSACRobustKnownFrameMagnetometerCalibrator(final double[] qualityScores, final boolean commonAxisUsed) {
277 super(commonAxisUsed);
278 internalSetQualityScores(qualityScores);
279 }
280
281 /**
282 * Constructor.
283 *
284 * @param qualityScores quality scores corresponding to each provided
285 * measurement. The larger the score value the better
286 * the quality of the sample.
287 * @param commonAxisUsed indicates whether z-axis is assumed to be common
288 * for the accelerometer, gyroscope and magnetometer.
289 * @param listener listener to handle events raised by this calibrator.
290 * @throws IllegalArgumentException if provided quality scores length
291 * is smaller than 4 samples.
292 */
293 public PROSACRobustKnownFrameMagnetometerCalibrator(
294 final double[] qualityScores, final boolean commonAxisUsed,
295 final RobustKnownFrameMagnetometerCalibratorListener listener) {
296 super(commonAxisUsed, listener);
297 internalSetQualityScores(qualityScores);
298 }
299
300 /**
301 * Constructor.
302 *
303 * @param qualityScores quality scores corresponding to each provided
304 * measurement. The larger the score value the better
305 * the quality of the sample.
306 * @param measurements list of body magnetic flux density measurements with standard
307 * deviations taken at different frames (positions and
308 * orientations).
309 * @param commonAxisUsed indicates whether z-axis is assumed to be common
310 * for the accelerometer, gyroscope and magnetometer.
311 * @throws IllegalArgumentException if provided quality scores length
312 * is smaller than 4 samples.
313 */
314 public PROSACRobustKnownFrameMagnetometerCalibrator(
315 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
316 final boolean commonAxisUsed) {
317 super(measurements, commonAxisUsed);
318 internalSetQualityScores(qualityScores);
319 }
320
321 /**
322 * Constructor.
323 *
324 * @param qualityScores quality scores corresponding to each provided
325 * measurement. The larger the score value the better
326 * the quality of the sample.
327 * @param measurements list of body magnetic flux density measurements with standard
328 * deviations taken at different frames (positions and
329 * orientations).
330 * @param commonAxisUsed indicates whether z-axis is assumed to be common
331 * for the accelerometer, gyroscope and magnetometer.
332 * @param listener listener to handle events raised by this calibrator.
333 * @throws IllegalArgumentException if provided quality scores length
334 * is smaller than 4 samples.
335 */
336 public PROSACRobustKnownFrameMagnetometerCalibrator(
337 final double[] qualityScores, final List<StandardDeviationFrameBodyMagneticFluxDensity> measurements,
338 final boolean commonAxisUsed, final RobustKnownFrameMagnetometerCalibratorListener listener) {
339 super(measurements, commonAxisUsed, listener);
340 internalSetQualityScores(qualityScores);
341 }
342
343 /**
344 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
345 * The threshold refers to the amount of error on norm between measured specific forces and the
346 * ones generated with estimated calibration parameters provided for each sample.
347 *
348 * @return threshold to determine whether samples are inliers or not.
349 */
350 public double getThreshold() {
351 return threshold;
352 }
353
354 /**
355 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
356 * The threshold refers to the amount of error on norm between measured specific forces and the
357 * ones generated with estimated calibration parameters provided for each sample.
358 *
359 * @param threshold threshold to determine whether samples are inliers or not.
360 * @throws IllegalArgumentException if provided value is equal or less than zero.
361 * @throws LockedException if calibrator is currently running.
362 */
363 public void setThreshold(final double threshold) throws LockedException {
364 if (running) {
365 throw new LockedException();
366 }
367 if (threshold <= MIN_THRESHOLD) {
368 throw new IllegalArgumentException();
369 }
370 this.threshold = threshold;
371 }
372
373 /**
374 * Returns quality scores corresponding to each provided sample.
375 * The larger the score value the better the quality of the sample.
376 *
377 * @return quality scores corresponding to each sample.
378 */
379 @Override
380 public double[] getQualityScores() {
381 return qualityScores;
382 }
383
384 /**
385 * Sets quality scores corresponding to each provided sample.
386 * The larger the score value the better the quality of the sample.
387 *
388 * @param qualityScores quality scores corresponding to each sample.
389 * @throws IllegalArgumentException if provided quality scores length
390 * is smaller than minimum required samples.
391 * @throws LockedException if calibrator is currently running.
392 */
393 @Override
394 public void setQualityScores(final double[] qualityScores) throws LockedException {
395 if (running) {
396 throw new LockedException();
397 }
398 internalSetQualityScores(qualityScores);
399 }
400
401 /**
402 * Indicates whether calibrator is ready to find a solution.
403 *
404 * @return true if calibrator is ready, false otherwise.
405 */
406 @Override
407 public boolean isReady() {
408 return super.isReady() && qualityScores != null && qualityScores.length == measurements.size();
409 }
410
411 /**
412 * Indicates whether inliers must be computed and kept.
413 *
414 * @return true if inliers must be computed and kept, false if inliers
415 * only need to be computed but not kept.
416 */
417 public boolean isComputeAndKeepInliersEnabled() {
418 return computeAndKeepInliers;
419 }
420
421 /**
422 * Specifies whether inliers must be computed and kept.
423 *
424 * @param computeAndKeepInliers true if inliers must be computed and kept,
425 * false if inliers only need to be computed but not kept.
426 * @throws LockedException if calibrator is currently running.
427 */
428 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
429 if (running) {
430 throw new LockedException();
431 }
432 this.computeAndKeepInliers = computeAndKeepInliers;
433 }
434
435 /**
436 * Indicates whether residuals must be computed and kept.
437 *
438 * @return true if residuals must be computed and kept, false if residuals
439 * only need to be computed but not kept.
440 */
441 public boolean isComputeAndKeepResiduals() {
442 return computeAndKeepResiduals;
443 }
444
445 /**
446 * Specifies whether residuals must be computed and kept.
447 *
448 * @param computeAndKeepResiduals true if residuals must be computed and kept,
449 * false if residuals only need to be computed but not kept.
450 * @throws LockedException if calibrator is currently running.
451 */
452 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
453 if (running) {
454 throw new LockedException();
455 }
456 this.computeAndKeepResiduals = computeAndKeepResiduals;
457 }
458
459 /**
460 * Estimates magnetometer calibration parameters containing hard-iron
461 * bias and soft-iron scale factors and cross-coupling errors.
462 *
463 * @throws LockedException if calibrator is currently running.
464 * @throws NotReadyException if calibrator is not ready.
465 * @throws CalibrationException if estimation fails for numerical reasons.
466 */
467 @SuppressWarnings("DuplicatedCode")
468 @Override
469 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
470 if (running) {
471 throw new LockedException();
472 }
473 if (!isReady()) {
474 throw new NotReadyException();
475 }
476
477 final var innerEstimator = new PROSACRobustEstimator<>(new PROSACRobustEstimatorListener<PreliminaryResult>() {
478 @Override
479 public double[] getQualityScores() {
480 return qualityScores;
481 }
482
483 @Override
484 public double getThreshold() {
485 return threshold;
486 }
487
488 @Override
489 public int getTotalSamples() {
490 return measurements.size();
491 }
492
493 @Override
494 public int getSubsetSize() {
495 return preliminarySubsetSize;
496 }
497
498 @Override
499 public void estimatePreliminarSolutions(
500 final int[] samplesIndices, final List<PreliminaryResult> solutions) {
501 computePreliminarySolutions(samplesIndices, solutions);
502 }
503
504 @Override
505 public double computeResidual(final PreliminaryResult currentEstimation, final int i) {
506 return computeError(measurements.get(i), currentEstimation);
507 }
508
509 @Override
510 public boolean isReady() {
511 return PROSACRobustKnownFrameMagnetometerCalibrator.this.isReady();
512 }
513
514 @Override
515 public void onEstimateStart(final RobustEstimator<PreliminaryResult> estimator) {
516 // no action needed
517 }
518
519 @Override
520 public void onEstimateEnd(final RobustEstimator<PreliminaryResult> estimator) {
521 // no action needed
522 }
523
524 @Override
525 public void onEstimateNextIteration(
526 final RobustEstimator<PreliminaryResult> estimator, final int iteration) {
527 if (listener != null) {
528 listener.onCalibrateNextIteration(
529 PROSACRobustKnownFrameMagnetometerCalibrator.this, iteration);
530 }
531 }
532
533 @Override
534 public void onEstimateProgressChange(
535 final RobustEstimator<PreliminaryResult> estimator, final float progress) {
536 if (listener != null) {
537 listener.onCalibrateProgressChange(
538 PROSACRobustKnownFrameMagnetometerCalibrator.this, progress);
539 }
540 }
541 });
542
543 try {
544 running = true;
545
546 if (listener != null) {
547 listener.onCalibrateStart(this);
548 }
549
550 inliersData = null;
551
552 setupWmmEstimator();
553
554 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
555 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
556 innerEstimator.setConfidence(confidence);
557 innerEstimator.setMaxIterations(maxIterations);
558 innerEstimator.setProgressDelta(progressDelta);
559 final var preliminaryResult = innerEstimator.estimate();
560 inliersData = innerEstimator.getInliersData();
561
562 attemptRefine(preliminaryResult);
563
564 if (listener != null) {
565 listener.onCalibrateEnd(this);
566 }
567
568 } catch (final com.irurueta.numerical.LockedException e) {
569 throw new LockedException(e);
570 } catch (final com.irurueta.numerical.NotReadyException e) {
571 throw new NotReadyException(e);
572 } catch (final RobustEstimatorException | IOException e) {
573 throw new CalibrationException(e);
574 } finally {
575 running = false;
576 }
577 }
578
579 /**
580 * Returns method being used for robust estimation.
581 *
582 * @return method being used for robust estimation.
583 */
584 @Override
585 public RobustEstimatorMethod getMethod() {
586 return RobustEstimatorMethod.PROSAC;
587 }
588
589 /**
590 * Indicates whether this calibrator requires quality scores for each
591 * measurement or not.
592 *
593 * @return true if quality scores are required, false otherwise.
594 */
595 @Override
596 public boolean isQualityScoresRequired() {
597 return true;
598 }
599
600 /**
601 * Sets quality scores corresponding to each provided sample.
602 * This method is used internally and does not check whether instance is
603 * locked or not.
604 *
605 * @param qualityScores quality scores to be set.
606 * @throws IllegalArgumentException if provided quality scores length
607 * is smaller than 4 samples.
608 */
609 private void internalSetQualityScores(final double[] qualityScores) {
610 if (qualityScores == null || qualityScores.length < MINIMUM_MEASUREMENTS) {
611 throw new IllegalArgumentException();
612 }
613
614 this.qualityScores = qualityScores;
615 }
616 }