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.accelerometer;
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.StandardDeviationFrameBodyKinematics;
23 import com.irurueta.numerical.robust.RANSACRobustEstimator;
24 import com.irurueta.numerical.robust.RANSACRobustEstimatorListener;
25 import com.irurueta.numerical.robust.RobustEstimator;
26 import com.irurueta.numerical.robust.RobustEstimatorException;
27 import com.irurueta.numerical.robust.RobustEstimatorMethod;
28 import com.irurueta.units.Acceleration;
29
30 import java.util.List;
31
32 /**
33 * Robustly estimates accelerometer cross couplings and scaling factors using
34 * a RANSAC algorithm to discard outliers.
35 * This estimator assumes that biases are known.
36 * <p>
37 * To use this calibrator at least 4 measurements at different known frames must
38 * be provided. In other words, accelerometer samples must be obtained at 4
39 * different positions, orientations and velocities (although typically velocities are
40 * always zero).
41 * <p>
42 * Measured specific force is assumed to follow the model shown below:
43 * <pre>
44 * fmeas = ba + (I + Ma) * ftrue + w
45 * </pre>
46 * Where:
47 * - fmeas is the measured specific force. This is a 3x1 vector.
48 * - ba is accelerometer bias. Ideally, on a perfect accelerometer, this should be a
49 * 3x1 zero vector.
50 * - I is the 3x3 identity matrix.
51 * - Ma is the 3x3 matrix containing cross-couplings and scaling factors. Ideally, on
52 * a perfect accelerometer, this should be a 3x3 zero matrix.
53 * - ftrue is ground-truth specific force.
54 * - w is measurement noise.
55 */
56 public class RANSACRobustKnownBiasAndFrameAccelerometerCalibrator extends
57 RobustKnownBiasAndFrameAccelerometerCalibrator {
58
59 /**
60 * Constant defining default threshold to determine whether samples are inliers or not.
61 */
62 public static final double DEFAULT_THRESHOLD = 1e-2;
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 * Constructor.
99 */
100 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator() {
101 super();
102 }
103
104 /**
105 * Constructor.
106 *
107 * @param listener listener to be notified of events such as when estimation
108 * starts, ends or its progress significantly changes.
109 */
110 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
111 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
112 super(listener);
113 }
114
115 /**
116 * Constructor.
117 *
118 * @param measurements list of body kinematics measurements with standard
119 * deviations taken at different frames (positions, orientations
120 * and velocities).
121 */
122 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
123 final List<StandardDeviationFrameBodyKinematics> measurements) {
124 super(measurements);
125 }
126
127 /**
128 * Constructor.
129 *
130 * @param measurements list of body kinematics measurements with standard
131 * deviations taken at different frames (positions, orientations
132 * and velocities).
133 * @param listener listener to handle events raised by this calibrator.
134 */
135 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
136 final List<StandardDeviationFrameBodyKinematics> measurements,
137 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
138 super(measurements, listener);
139 }
140
141 /**
142 * Constructor.
143 *
144 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
145 * accelerometer and gyroscope.
146 */
147 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(final boolean commonAxisUsed) {
148 super(commonAxisUsed);
149 }
150
151 /**
152 * Constructor.
153 *
154 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
155 * accelerometer and gyroscope.
156 * @param listener listener to handle events raised by this calibrator.
157 */
158 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
159 final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
160 super(commonAxisUsed, listener);
161 }
162
163 /**
164 * Constructor.
165 *
166 * @param measurements list of body kinematics measurements with standard
167 * deviations taken at different frames (positions, orientations
168 * and velocities).
169 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
170 * accelerometer and gyroscope.
171 */
172 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
173 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed) {
174 super(measurements, commonAxisUsed);
175 }
176
177 /**
178 * Constructor.
179 *
180 * @param measurements list of body kinematics measurements with standard
181 * deviations taken at different frames (positions, orientations
182 * and velocities).
183 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
184 * accelerometer and gyroscope.
185 * @param listener listener to handle events raised by this calibrator.
186 */
187 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
188 final List<StandardDeviationFrameBodyKinematics> measurements, final boolean commonAxisUsed,
189 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
190 super(measurements, commonAxisUsed, listener);
191 }
192
193 /**
194 * Constructor.
195 *
196 * @param biasX known x coordinate of accelerometer bias expressed in meters per
197 * squared second (m/s^2).
198 * @param biasY known y coordinate of accelerometer bias expressed in meters per
199 * squared second (m/s^2).
200 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
201 * squared second (m/s^2).
202 */
203 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
204 final double biasX, final double biasY, final double biasZ) {
205 super(biasX, biasY, biasZ);
206 }
207
208 /**
209 * Constructor.
210 *
211 * @param biasX known x coordinate of accelerometer bias expressed in meters per
212 * squared second (m/s^2).
213 * @param biasY known y coordinate of accelerometer bias expressed in meters per
214 * squared second (m/s^2).
215 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
216 * squared second (m/s^2).
217 * @param listener listener to be notified of events such as when estimation
218 * starts, ends or its progress significantly changes.
219 */
220 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
221 final double biasX, final double biasY, final double biasZ,
222 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
223 super(biasX, biasY, biasZ, listener);
224 }
225
226 /**
227 * Constructor.
228 *
229 * @param measurements list of body kinematics measurements with standard
230 * deviations taken at different frames (positions, orientations
231 * and velocities).
232 * @param biasX known x coordinate of accelerometer bias expressed in meters per
233 * squared second (m/s^2).
234 * @param biasY known y coordinate of accelerometer bias expressed in meters per
235 * squared second (m/s^2).
236 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
237 * squared second (m/s^2).
238 */
239 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
240 final List<StandardDeviationFrameBodyKinematics> measurements,
241 final double biasX, final double biasY, final double biasZ) {
242 super(measurements, biasX, biasY, biasZ);
243 }
244
245 /**
246 * Constructor.
247 *
248 * @param measurements list of body kinematics measurements with standard
249 * deviations taken at different frames (positions, orientations
250 * and velocities).
251 * @param biasX known x coordinate of accelerometer bias expressed in meters per
252 * squared second (m/s^2).
253 * @param biasY known y coordinate of accelerometer bias expressed in meters per
254 * squared second (m/s^2).
255 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
256 * squared second (m/s^2).
257 * @param listener listener to handle events raised by this calibrator.
258 */
259 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
260 final List<StandardDeviationFrameBodyKinematics> measurements,
261 final double biasX, final double biasY, final double biasZ,
262 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
263 super(measurements, biasX, biasY, biasZ, listener);
264 }
265
266 /**
267 * Constructor.
268 *
269 * @param biasX known x coordinate of accelerometer bias expressed in meters per
270 * squared second (m/s^2).
271 * @param biasY known y coordinate of accelerometer bias expressed in meters per
272 * squared second (m/s^2).
273 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
274 * squared second (m/s^2).
275 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
276 * accelerometer and gyroscope.
277 */
278 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
279 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
280 super(biasX, biasY, biasZ, commonAxisUsed);
281 }
282
283 /**
284 * Constructor.
285 *
286 * @param biasX known x coordinate of accelerometer bias expressed in meters per
287 * squared second (m/s^2).
288 * @param biasY known y coordinate of accelerometer bias expressed in meters per
289 * squared second (m/s^2).
290 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
291 * squared second (m/s^2).
292 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
293 * accelerometer and gyroscope.
294 * @param listener listener to handle events raised by this calibrator.
295 */
296 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
297 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
298 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
299 super(biasX, biasY, biasZ, commonAxisUsed, listener);
300 }
301
302 /**
303 * Constructor.
304 *
305 * @param measurements list of body kinematics measurements with standard
306 * deviations taken at different frames (positions, orientations
307 * and velocities).
308 * @param biasX known x coordinate of accelerometer bias expressed in meters per
309 * squared second (m/s^2).
310 * @param biasY known y coordinate of accelerometer bias expressed in meters per
311 * squared second (m/s^2).
312 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
313 * squared second (m/s^2).
314 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
315 * accelerometer and gyroscope.
316 */
317 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
318 final List<StandardDeviationFrameBodyKinematics> measurements,
319 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed) {
320 super(measurements, biasX, biasY, biasZ, commonAxisUsed);
321 }
322
323 /**
324 * Constructor.
325 *
326 * @param measurements list of body kinematics measurements with standard
327 * deviations taken at different frames (positions, orientations
328 * and velocities).
329 * @param biasX known x coordinate of accelerometer bias expressed in meters per
330 * squared second (m/s^2).
331 * @param biasY known y coordinate of accelerometer bias expressed in meters per
332 * squared second (m/s^2).
333 * @param biasZ known z coordinate of accelerometer bias expressed in meters per
334 * squared second (m/s^2).
335 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
336 * accelerometer and gyroscope.
337 * @param listener listener to handle events raised by this calibrator.
338 */
339 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
340 final List<StandardDeviationFrameBodyKinematics> measurements,
341 final double biasX, final double biasY, final double biasZ, final boolean commonAxisUsed,
342 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
343 super(measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
344 }
345
346 /**
347 * Constructor.
348 *
349 * @param biasX known x coordinate of accelerometer bias.
350 * @param biasY known y coordinate of accelerometer bias.
351 * @param biasZ known z coordinate of accelerometer bias.
352 */
353 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
354 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
355 super(biasX, biasY, biasZ);
356 }
357
358 /**
359 * Constructor.
360 *
361 * @param biasX known x coordinate of accelerometer bias.
362 * @param biasY known y coordinate of accelerometer bias.
363 * @param biasZ known z coordinate of accelerometer bias.
364 * @param listener listener to be notified of events such as when estimation
365 * starts, ends or its progress significantly changes.
366 */
367 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
368 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
369 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
370 super(biasX, biasY, biasZ, listener);
371 }
372
373 /**
374 * Constructor.
375 *
376 * @param measurements list of body kinematics measurements with standard
377 * deviations taken at different frames (positions, orientations
378 * and velocities).
379 * @param biasX known x coordinate of accelerometer bias.
380 * @param biasY known y coordinate of accelerometer bias.
381 * @param biasZ known z coordinate of accelerometer bias.
382 */
383 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
384 final List<StandardDeviationFrameBodyKinematics> measurements,
385 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ) {
386 super(measurements, biasX, biasY, biasZ);
387 }
388
389 /**
390 * Constructor.
391 *
392 * @param measurements list of body kinematics measurements with standard
393 * deviations taken at different frames (positions, orientations
394 * and velocities).
395 * @param biasX known x coordinate of accelerometer bias.
396 * @param biasY known y coordinate of accelerometer bias.
397 * @param biasZ known z coordinate of accelerometer bias.
398 * @param listener listener to handle events raised by this calibrator.
399 */
400 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
401 final List<StandardDeviationFrameBodyKinematics> measurements,
402 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
403 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
404 super(measurements, biasX, biasY, biasZ, listener);
405 }
406
407 /**
408 * Constructor.
409 *
410 * @param biasX known x coordinate of accelerometer bias.
411 * @param biasY known y coordinate of accelerometer bias.
412 * @param biasZ known z coordinate of accelerometer bias.
413 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
414 * accelerometer and gyroscope.
415 */
416 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
417 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
418 final boolean commonAxisUsed) {
419 super(biasX, biasY, biasZ, commonAxisUsed);
420 }
421
422 /**
423 * Constructor.
424 *
425 * @param biasX known x coordinate of accelerometer bias.
426 * @param biasY known y coordinate of accelerometer bias.
427 * @param biasZ known z coordinate of accelerometer bias.
428 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
429 * accelerometer and gyroscope.
430 * @param listener listener to handle events raised by this calibrator.
431 */
432 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
433 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final boolean commonAxisUsed,
434 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
435 super(biasX, biasY, biasZ, commonAxisUsed, listener);
436 }
437
438 /**
439 * Constructor.
440 *
441 * @param measurements list of body kinematics measurements with standard
442 * deviations taken at different frames (positions, orientations
443 * and velocities).
444 * @param biasX known x coordinate of accelerometer bias.
445 * @param biasY known y coordinate of accelerometer bias.
446 * @param biasZ known z coordinate of accelerometer bias.
447 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
448 * accelerometer and gyroscope.
449 */
450 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
451 final List<StandardDeviationFrameBodyKinematics> measurements,
452 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ,
453 final boolean commonAxisUsed) {
454 super(measurements, biasX, biasY, biasZ, commonAxisUsed);
455 }
456
457 /**
458 * Constructor.
459 *
460 * @param measurements list of body kinematics measurements with standard
461 * deviations taken at different frames (positions, orientations
462 * and velocities).
463 * @param biasX known x coordinate of accelerometer bias.
464 * @param biasY known y coordinate of accelerometer bias.
465 * @param biasZ known z coordinate of accelerometer bias.
466 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
467 * accelerometer and gyroscope.
468 * @param listener listener to handle events raised by this calibrator.
469 */
470 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
471 final List<StandardDeviationFrameBodyKinematics> measurements,
472 final Acceleration biasX, final Acceleration biasY, final Acceleration biasZ, final boolean commonAxisUsed,
473 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
474 super(measurements, biasX, biasY, biasZ, commonAxisUsed, listener);
475 }
476
477 /**
478 * Constructor.
479 *
480 * @param bias known accelerometer bias.
481 * @throws IllegalArgumentException if provided array does not have length 3.
482 */
483 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(final double[] bias) {
484 super(bias);
485 }
486
487 /**
488 * Constructor.
489 *
490 * @param bias known accelerometer bias.
491 * @param listener listener to be notified of events such as when estimation
492 * starts, ends or its progress significantly changes.
493 * @throws IllegalArgumentException if provided array does not have length 3.
494 */
495 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
496 final double[] bias, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
497 super(bias, listener);
498 }
499
500 /**
501 * Constructor.
502 *
503 * @param measurements list of body kinematics measurements with standard
504 * deviations taken at different frames (positions, orientations
505 * and velocities).
506 * @param bias known accelerometer bias.
507 * @throws IllegalArgumentException if provided array does not have length 3.
508 */
509 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
510 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias) {
511 super(measurements, bias);
512 }
513
514 /**
515 * Constructor.
516 *
517 * @param measurements list of body kinematics measurements with standard
518 * deviations taken at different frames (positions, orientations
519 * and velocities).
520 * @param bias known accelerometer bias.
521 * @param listener listener to handle events raised by this calibrator.
522 * @throws IllegalArgumentException if provided array does not have length 3.
523 */
524 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
525 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
526 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
527 super(measurements, bias, listener);
528 }
529
530 /**
531 * Constructor.
532 *
533 * @param bias known accelerometer bias.
534 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
535 * accelerometer and gyroscope.
536 * @throws IllegalArgumentException if provided array does not have length 3.
537 */
538 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(final double[] bias, final boolean commonAxisUsed) {
539 super(bias, commonAxisUsed);
540 }
541
542 /**
543 * Constructor.
544 *
545 * @param bias known accelerometer bias.
546 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
547 * accelerometer and gyroscope.
548 * @param listener listener to handle events raised by this calibrator.
549 * @throws IllegalArgumentException if provided array does not have length 3.
550 */
551 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
552 final double[] bias, final boolean commonAxisUsed,
553 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
554 super(bias, commonAxisUsed, listener);
555 }
556
557 /**
558 * Constructor.
559 *
560 * @param measurements list of body kinematics measurements with standard
561 * deviations taken at different frames (positions, orientations
562 * and velocities).
563 * @param bias known accelerometer bias.
564 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
565 * accelerometer and gyroscope.
566 * @throws IllegalArgumentException if provided array does not have length 3.
567 */
568 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
569 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
570 final boolean commonAxisUsed) {
571 super(measurements, bias, commonAxisUsed);
572 }
573
574 /**
575 * Constructor.
576 *
577 * @param measurements list of body kinematics measurements with standard
578 * deviations taken at different frames (positions, orientations
579 * and velocities).
580 * @param bias known accelerometer bias.
581 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
582 * accelerometer and gyroscope.
583 * @param listener listener to handle events raised by this calibrator.
584 * @throws IllegalArgumentException if provided array does not have length 3.
585 */
586 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
587 final List<StandardDeviationFrameBodyKinematics> measurements, final double[] bias,
588 final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
589 super(measurements, bias, commonAxisUsed, listener);
590 }
591
592 /**
593 * Constructor.
594 *
595 * @param bias known accelerometer bias.
596 * @throws IllegalArgumentException if provided matrix is not 3x1.
597 */
598 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(final Matrix bias) {
599 super(bias);
600 }
601
602 /**
603 * Constructor.
604 *
605 * @param bias known accelerometer bias.
606 * @param listener listener to be notified of events such as when estimation
607 * starts, ends or its progress significantly changes.
608 * @throws IllegalArgumentException if provided matrix is not 3x1.
609 */
610 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
611 final Matrix bias, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
612 super(bias, listener);
613 }
614
615 /**
616 * Constructor.
617 *
618 * @param measurements list of body kinematics measurements with standard
619 * deviations taken at different frames (positions, orientations
620 * and velocities).
621 * @param bias known accelerometer bias.
622 * @throws IllegalArgumentException if provided matrix is not 3x1.
623 */
624 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
625 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias) {
626 super(measurements, bias);
627 }
628
629 /**
630 * Constructor.
631 *
632 * @param measurements list of body kinematics measurements with standard
633 * deviations taken at different frames (positions, orientations
634 * and velocities).
635 * @param bias known accelerometer bias.
636 * @param listener listener to handle events raised by this calibrator.
637 * @throws IllegalArgumentException if provided matrix is not 3x1.
638 */
639 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
640 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias,
641 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
642 super(measurements, bias, listener);
643 }
644
645 /**
646 * Constructor.
647 *
648 * @param bias known accelerometer bias.
649 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
650 * accelerometer and gyroscope.
651 * @throws IllegalArgumentException if provided matrix is not 3x1.
652 */
653 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(final Matrix bias, final boolean commonAxisUsed) {
654 super(bias, commonAxisUsed);
655 }
656
657 /**
658 * Constructor.
659 *
660 * @param bias known accelerometer bias.
661 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
662 * accelerometer and gyroscope.
663 * @param listener listener to handle events raised by this calibrator.
664 * @throws IllegalArgumentException if provided matrix is not 3x1.
665 */
666 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
667 final Matrix bias, final boolean commonAxisUsed,
668 final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
669 super(bias, commonAxisUsed, listener);
670 }
671
672 /**
673 * Constructor.
674 *
675 * @param measurements list of body kinematics measurements with standard
676 * deviations taken at different frames (positions, orientations
677 * and velocities).
678 * @param bias known accelerometer bias.
679 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
680 * accelerometer and gyroscope.
681 * @throws IllegalArgumentException if provided matrix is not 3x1.
682 */
683 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
684 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias,
685 final boolean commonAxisUsed) {
686 super(measurements, bias, commonAxisUsed);
687 }
688
689 /**
690 * Constructor.
691 *
692 * @param measurements list of body kinematics measurements with standard
693 * deviations taken at different frames (positions, orientations
694 * and velocities).
695 * @param bias known accelerometer bias.
696 * @param commonAxisUsed indicates whether z-axis is assumed to be common for
697 * accelerometer and gyroscope.
698 * @param listener listener to handle events raised by this calibrator.
699 * @throws IllegalArgumentException if provided matrix is not 3x1.
700 */
701 public RANSACRobustKnownBiasAndFrameAccelerometerCalibrator(
702 final List<StandardDeviationFrameBodyKinematics> measurements, final Matrix bias,
703 final boolean commonAxisUsed, final RobustKnownBiasAndFrameAccelerometerCalibratorListener listener) {
704 super(measurements, bias, commonAxisUsed, listener);
705 }
706
707 /**
708 * Gets threshold to determine whether samples are inliers or not when testing possible solutions.
709 * The threshold refers to the amount of error on norm between measured specific forces and the
710 * ones generated with estimated calibration parameters provided for each sample.
711 *
712 * @return threshold to determine whether samples are inliers or not.
713 */
714 public double getThreshold() {
715 return threshold;
716 }
717
718 /**
719 * Sets threshold to determine whether samples are inliers or not when testing possible solutions.
720 * The threshold refers to the amount of error on norm between measured specific forces and the
721 * ones generated with estimated calibration parameters provided for each sample.
722 *
723 * @param threshold threshold to determine whether samples are inliers or not.
724 * @throws IllegalArgumentException if provided value is equal or less than zero.
725 * @throws LockedException if calibrator is currently running.
726 */
727 public void setThreshold(final double threshold) throws LockedException {
728 if (running) {
729 throw new LockedException();
730 }
731 if (threshold <= MIN_THRESHOLD) {
732 throw new IllegalArgumentException();
733 }
734 this.threshold = threshold;
735 }
736
737 /**
738 * Indicates whether inliers must be computed and kept.
739 *
740 * @return true if inliers must be computed and kept, false if inliers
741 * only need to be computed but not kept.
742 */
743 public boolean isComputeAndKeepInliersEnabled() {
744 return computeAndKeepInliers;
745 }
746
747 /**
748 * Specifies whether inliers must be computed and kept.
749 *
750 * @param computeAndKeepInliers true if inliers must be computed and kept,
751 * false if inliers only need to be computed but not kept.
752 * @throws LockedException if calibrator is currently running.
753 */
754 public void setComputeAndKeepInliersEnabled(final boolean computeAndKeepInliers) throws LockedException {
755 if (running) {
756 throw new LockedException();
757 }
758 this.computeAndKeepInliers = computeAndKeepInliers;
759 }
760
761 /**
762 * Indicates whether residuals must be computed and kept.
763 *
764 * @return true if residuals must be computed and kept, false if residuals
765 * only need to be computed but not kept.
766 */
767 public boolean isComputeAndKeepResiduals() {
768 return computeAndKeepResiduals;
769 }
770
771 /**
772 * Specifies whether residuals must be computed and kept.
773 *
774 * @param computeAndKeepResiduals true if residuals must be computed and kept,
775 * false if residuals only need to be computed but not kept.
776 * @throws LockedException if calibrator is currently running.
777 */
778 public void setComputeAndKeepResidualsEnabled(final boolean computeAndKeepResiduals) throws LockedException {
779 if (running) {
780 throw new LockedException();
781 }
782 this.computeAndKeepResiduals = computeAndKeepResiduals;
783 }
784
785 /**
786 * Estimates accelerometer calibration parameters containing scale factors
787 * and cross-coupling errors.
788 *
789 * @throws LockedException if calibrator is currently running.
790 * @throws NotReadyException if calibrator is not ready.
791 * @throws CalibrationException if estimation fails for numerical reasons.
792 */
793 @SuppressWarnings("DuplicatedCode")
794 @Override
795 public void calibrate() throws LockedException, NotReadyException, CalibrationException {
796 if (running) {
797 throw new LockedException();
798 }
799 if (!isReady()) {
800 throw new NotReadyException();
801 }
802
803 final var innerEstimator = new RANSACRobustEstimator<>(new RANSACRobustEstimatorListener<Matrix>() {
804 @Override
805 public double getThreshold() {
806 return threshold;
807 }
808
809 @Override
810 public int getTotalSamples() {
811 return measurements.size();
812 }
813
814 @Override
815 public int getSubsetSize() {
816 return preliminarySubsetSize;
817 }
818
819 @Override
820 public void estimatePreliminarSolutions(final int[] samplesIndices, final List<Matrix> solutions) {
821 computePreliminarySolutions(samplesIndices, solutions);
822 }
823
824 @Override
825 public double computeResidual(final Matrix currentEstimation, final int i) {
826 return computeError(measurements.get(i), currentEstimation);
827 }
828
829 @Override
830 public boolean isReady() {
831 return RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.super.isReady();
832 }
833
834 @Override
835 public void onEstimateStart(final RobustEstimator<Matrix> estimator) {
836 // no action needed
837 }
838
839 @Override
840 public void onEstimateEnd(final RobustEstimator<Matrix> estimator) {
841 // no action needed
842 }
843
844 @Override
845 public void onEstimateNextIteration(final RobustEstimator<Matrix> estimator, final int iteration) {
846 if (listener != null) {
847 listener.onCalibrateNextIteration(
848 RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, iteration);
849 }
850 }
851
852 @Override
853 public void onEstimateProgressChange(
854 final RobustEstimator<Matrix> estimator, final float progress) {
855 if (listener != null) {
856 listener.onCalibrateProgressChange(
857 RANSACRobustKnownBiasAndFrameAccelerometerCalibrator.this, progress);
858 }
859 }
860 });
861
862 try {
863 running = true;
864
865 if (listener != null) {
866 listener.onCalibrateStart(this);
867 }
868
869 inliersData = null;
870 innerEstimator.setComputeAndKeepInliersEnabled(computeAndKeepInliers || refineResult);
871 innerEstimator.setComputeAndKeepResidualsEnabled(computeAndKeepResiduals || refineResult);
872 innerEstimator.setConfidence(confidence);
873 innerEstimator.setMaxIterations(maxIterations);
874 innerEstimator.setProgressDelta(progressDelta);
875 final var preliminaryResult = innerEstimator.estimate();
876 inliersData = innerEstimator.getInliersData();
877
878 attemptRefine(preliminaryResult);
879
880 if (listener != null) {
881 listener.onCalibrateEnd(this);
882 }
883
884 } catch (final com.irurueta.numerical.LockedException e) {
885 throw new LockedException(e);
886 } catch (final com.irurueta.numerical.NotReadyException e) {
887 throw new NotReadyException(e);
888 } catch (final RobustEstimatorException e) {
889 throw new CalibrationException(e);
890 } finally {
891 running = false;
892 }
893 }
894
895 /**
896 * Returns method being used for robust estimation.
897 *
898 * @return method being used for robust estimation.
899 */
900 @Override
901 public RobustEstimatorMethod getMethod() {
902 return RobustEstimatorMethod.RANSAC;
903 }
904
905 /**
906 * Indicates whether this calibrator requires quality scores for each
907 * measurement or not.
908 *
909 * @return true if quality scores are required, false otherwise.
910 */
911 @Override
912 public boolean isQualityScoresRequired() {
913 return false;
914 }
915 }