Skip to content

GTSAM magFactor3

MAGYC - Benchmark Methods - magfactor3

This module contains the magfactor3 method. This method is a factor graph implementation of the full-magnetometer calibration least-squares problems.

Functions:

Name Description
magfactor3

Factor graph based approach to full-magnetometer calibration.

magfactor3(magnetic_field, rph, magnetic_declination, reference_magnetic_field, optimizer='dogleg', relative_error_tol=1e-12, absolute_error_tol=1e-12, max_iter=1000)

The full-magnetometer calibration least-squares problems can also be modeled as a factor graph. This can be implemented using the GTSAM python wrapper with the magFactor3 factor. This approach allows the user to get the soft-iron (SI) as the identity scaled by a constant and the hard-iron (HI) from the magnetometer bias.

This method assumes that the rotation from the body frame with respect to the world frame and the local magnetic field are known.

Parameters:

Name Type Description Default
magnetic_field Union[ndarray, list]

Magnetic field raw data

 required
rph Union[ndarray, list]

Roll, pitch and heading data

 required
magnetic_declination float

Magnetic declination in degrees

 required
reference_magnetic_field Union[ndarray, list]

Reference magnetic field

 required
optimizer str

Optimization algorithm to use. Options are "dogleg" or "lm" for the Dogleg and Levenberg-Marquardt optimizers respectively.

 'dogleg'
relative_error_tol float

Relative error tolerance for the optimizer. Default is 1.00e-12

 1e-12
absolute_error_tol float

Absolute error tolerance for the optimizer. Default is 1.00e-12

 1e-12
max_iter int

Maximum number of iterations for the optimizer. Default is 1000

 1000

Returns:

Name Type Description
hard_iron ndarray

Hard-iron offset in G
soft_iron ndarray

Soft-iron scaling matrix
corrected_magnetic_field ndarray

Corrected magnetic field data
optimization_errors list

List of optimization errors in each iteration

Raises:

Type Description
TypeError

If the magnetic field input is not a numpy array or a list
TypeError

If the reference magnetic field input is not a numpy array or a list
TypeError

If the rph input is not a numpy array or a list
ValueError

If the magnetic field input is not a 3xN or Nx3 numpy array
ValueError

If the reference magnetic field input is not a 3, numpy array
ValueError

If the rph input is not a 3xN or Nx3 numpy array
TypeError

If the magnetic declination is not a float
ValueError

If the optimizer is not a string or not "dogleg" or "lm"
TypeError

If the relative error tolerance is not a float
TypeError

If the absolute error tolerance is not a float
ValueError

If the maximum number of iterations is not a positive integer
Source code in magyc/benchmark_methods/magfactor.py 
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
def magfactor3(magnetic_field: Union[np.ndarray, list], rph: Union[np.ndarray, list], magnetic_declination: float,
               reference_magnetic_field: Union[np.ndarray, list], optimizer: str = "dogleg",
               relative_error_tol: float = 1.00e-12, absolute_error_tol: float = 1.00e-12,
               max_iter: int = 1000) -> Tuple[np.ndarray, np.ndarray, np.ndarray, list]:
    """
    The full-magnetometer calibration least-squares problems can also be modeled
    as a factor graph. This can be implemented using the [GTSAM](https://github.com/borglab/gtsam)
    python wrapper with the magFactor3 factor. This approach allows the user to get the soft-iron
    (SI) as the identity scaled by a constant and the hard-iron (HI) from the
    magnetometer bias.

    This method assumes that the rotation from the body frame with respect to
    the world frame and the local magnetic field are known.

    Args:
        magnetic_field (Union[np.ndarray, list]): Magnetic field raw data
        rph (Union[np.ndarray, list]): Roll, pitch and heading data
        magnetic_declination (float): Magnetic declination in degrees
        reference_magnetic_field (Union[np.ndarray, list]): Reference magnetic field
        optimizer (str): Optimization algorithm to use. Options are "dogleg" or "lm"
            for the Dogleg and Levenberg-Marquardt optimizers respectively.
        relative_error_tol (float): Relative error tolerance for the optimizer. Default is 1.00e-12
        absolute_error_tol (float): Absolute error tolerance for the optimizer. Default is 1.00e-12
        max_iter (int): Maximum number of iterations for the optimizer. Default is 1000

    Returns:
        hard_iron (np.ndarray): Hard-iron offset in G
        soft_iron (np.ndarray): Soft-iron scaling matrix
        corrected_magnetic_field (np.ndarray): Corrected magnetic field data
        optimization_errors (list): List of optimization errors in each iteration

    Raises:
        TypeError: If the magnetic field input is not a numpy array or a list
        TypeError: If the reference magnetic field input is not a numpy array or a list
        TypeError: If the rph input is not a numpy array or a list
        ValueError: If the magnetic field input is not a 3xN or Nx3 numpy array
        ValueError: If the reference magnetic field input is not a 3, numpy array
        ValueError: If the rph input is not a 3xN or Nx3 numpy array
        TypeError: If the magnetic declination is not a float
        ValueError: If the optimizer is not a string or not "dogleg" or "lm"
        TypeError: If the relative error tolerance is not a float
        TypeError: If the absolute error tolerance is not a float
        ValueError: If the maximum number of iterations is not a positive integer
    """
    # Check if the magnetic field input is a list and convert it to a numpy array
    if isinstance(magnetic_field, list):
        magnetic_field = np.array(magnetic_field)

    # Check if the reference magnetic field input is a list and convert it to a numpy array
    if isinstance(reference_magnetic_field, list):
        reference_magnetic_field = np.array(reference_magnetic_field).flatten()

    # Check if the rph input is a list and convert it to a numpy array
    if isinstance(rph, list):
        rph = np.array(rph)

    # Check if the magnetic field input is a numpy array
    if not isinstance(magnetic_field, np.ndarray):
        raise TypeError("The magnetic field input must be a numpy array or a list.")

    # Check if the reference magnetic field input is a numpy array
    if not isinstance(reference_magnetic_field, np.ndarray):
        raise TypeError("The reference magnetic field input must be a numpy array or a list.")

    # Check if the rph input is a numpy array
    if not isinstance(rph, np.ndarray):
        raise TypeError("The rph input must be a numpy array or a list.")

    # Check if the magnetic field input is a 3xN or Nx3 numpy array
    if magnetic_field.ndim != 2 or (magnetic_field.shape[0] != 3 and magnetic_field.shape[1] != 3):
        raise ValueError("The magnetic field input must be a 3xN or Nx3 numpy array.")

    # Check if the reference magnetic field input is a 3, numpy array
    reference_magnetic_field = reference_magnetic_field.flatten()
    if reference_magnetic_field.shape[0] != 3:
        raise ValueError("The reference magnetic field input must be a 3, or 1x3, or 3x1 numpy array.")

    # Check if the rph input is is a 3xN or Nx3 numpy array
    if rph.ndim != 2 or (rph.shape[0] != 3 and rph.shape[1] != 3):
        raise ValueError("The rph input must be a 3xN or Nx3 numpy array.")

    # Force the magnetic field array to be a Nx3 numpy array
    if magnetic_field.shape[0] == 3:
        magnetic_field = magnetic_field.T

    # Force the rph array to be a Nx3 numpy array
    if rph.shape[0] == 3:
        rph = rph.T

    # Check that the magnetic declination is a float
    if not isinstance(magnetic_declination, float):
        raise TypeError("The magnetic declination must be a float.")

    # Check that the optimizer is a string and is either "dogleg" or "lm"
    if not isinstance(optimizer, str) or optimizer not in ["dogleg", "lm"]:
        raise ValueError("The optimizer must be a string and either 'dogleg' or 'lm'.")

    # Check that the relative error tolerance is a float
    if not isinstance(relative_error_tol, float) or relative_error_tol <= 0:
        raise TypeError("The relative error tolerance must be a float.")

    # Check that the absolute error tolerance is a float
    if not isinstance(absolute_error_tol, float) or absolute_error_tol <= 0:
        raise TypeError("The absolute error tolerance must be a float.")

    # Check that the maximum number of iterations is a positive integer
    if not isinstance(max_iter, int) or max_iter <= 0:
        raise ValueError("The maximum number of iterations must be a positive integer.")

    # Compute attitude based on magnetic heading
    magnetic_hdg = _ahrs_raw_hdg(magnetic_field, rph) - np.deg2rad(magnetic_declination)
    magnetic_rph = np.concatenate([rph[:, :2], magnetic_hdg.reshape(-1, 1)], axis=1)

    # Compute calibration
    # Smoothing and Mapping Factor Graph
    # 1. Create the non-linear graph
    graph = gtsam.NonlinearFactorGraph()

    # 2. noise model for each factor.
    residual_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)

    # 3. Creates values structure with initial values: S -> Scale, D -> Direction, B -> Bias
    initial = gtsam.Values()
    initial.insert(S(0), 1.0)
    initial.insert(B(0), gtsam.Point3(0, 0, 0))
    keys = [S(0), B(0)]

    # 4. Add factor for each measurement into a single node
    h0 = gtsam.Point3(reference_magnetic_field.flatten())

    for i in range(magnetic_field.shape[0]):
        mi = gtsam.Point3(magnetic_field[i, :])
        bRw = gtsam.Rot3(nm.rph2rot(magnetic_rph[i, :]).T)

        # 5.1 magFactor3
        rf = gtsam.CustomFactor(residual_noise, keys, partial(_residual_factor, mi, h0, bRw))
        graph.add(rf)

    # 5. If not online optimize the full batch
    # 5.1 Create optimizer parameters
    params = gtsam.DoglegParams() if optimizer == "dogleg" else gtsam.LevenbergMarquardtParams()
    params.setRelativeErrorTol(relative_error_tol)
    params.setAbsoluteErrorTol(absolute_error_tol)
    params.setMaxIterations(max_iter)
    params.setLinearSolverType("MULTIFRONTAL_CHOLESKY")

    # 5.2 Create optimizer
    if optimizer == "dogleg":
        optimizer = gtsam.DoglegOptimizer(graph, initial, params)
    else:
        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)

    # 5.3 Optimize
    result, optimization_errors = _gtsam_optimize(optimizer, params)

    # 7. Process Results
    hard_iron = np.vstack(result.atPoint3(B(0)))
    soft_iron = result.atDouble(S(0)) * np.eye(3)

    # Correct the magnetic field
    corrected_magnetic_field = (np.linalg.inv(soft_iron) @ (magnetic_field.T - hard_iron.reshape(3, -1))).T

    return hard_iron.flatten(), soft_iron, corrected_magnetic_field, optimization_errors