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