Enhancing Visual Inertial SLAM with Magnetic Measurements (2025)

III-A Problem Formulation

We consider the visual inertial magnetometer odometry problem where we want to track the state of a sensing system (a handheld underwater sensor suite) equipped with an IMU, a stereo/monocular camera, and a magnetometer. The IMU frame coincides with the body frame ”B” we want to track, and the transformation between the camera and the IMU ($\text{T}_{\text{BC}}$) is known by calibrating the extrinsic parameters and remains constant throughout the experiment; see Fig. 2 . In this work, we use a 9-axis IMU that includes an accelerometer, a gyroscope, and a magnetometer. As such, we assume that the magnetometer and IMU (gyroscope and accelerometer) axes are aligned and that there is no significant axis misalignment. When using stereo cameras, we assume that they are rigidly attached with known extrinsic parameters between the cameras and between the cameras and the IMU.

III-B Fusing visual, inertial and magnetometer measurements

The reprojection error for the landmark ${}_{W}\mathbf{l}_{j}\in\pazocal{J}_{k}$ is calculated as $\mathbf{e_{r}}^{i,j}=h({}_{W}\mathbf{l}_{j})-\mathbf{z}^{j,k}$ where $\pazocal{J}_{k}$ is the set of all the landmarks visible in the keyframe $k$. Here, $h(.)$ denotes the camera projection model and $\mathbf{z}^{j,k}$ the measurements in the image coordinates. For more details, please refer to [okvis]. The inertial residuals $\mathbf{e_{i}}$ are obtained using the IMU preintegration theory proposed in [qin2017vins_mono, quatenrion_sola] which is detailed in Section III-C. We employ a marginalization strategy similar to [okvis] to obtain the marginalization prior error term, $\mathbf{e_{p}}$. Whenever a new frame is inserted in the optimization window, the marginalization operation is classified into two cases. If the oldest frame in the sliding window is not a keyframe, it is marginalized together with the oldest speed and bias states, and all its landmark measurements are dropped to maintain sparsity. In the case where the oldest frame is a keyframe, only the landmarks that are visible in that frame but not in the most recent keyframe are marginalized out.

III-C IMU Preintegration

In this section, we review the IMU preintegration formulation, which in turn will be used to derive the magnetometer residuals in Section III-D. The IMU preintegration formulation is based on [qin2017vins_mono] inspired by continuous-time quaternion kinematics from [quatenrion_sola] and uses IMU bias manipulation according to [forster2016manifold].

The accelerometer and gyroscope noise is modeled as additive Gaussian noise with $\boldsymbol{\eta}_{a}\sim\pazocal{N}(\mathbf{0},\sigma_{a}^{2}\cdot\mathbf{I})$ and $\boldsymbol{\eta}_{w}\sim\pazocal{N}(\mathbf{0},\sigma_{w}^{2}\cdot\mathbf{I})$, where $\mathbf{I}$ being the identity matrix. IMU biases are modeled as a slowly varying random walk with $\dot{\mathbf{b}}_{w_{t}}=\boldsymbol{\eta}_{b_{w}}$ and $\dot{\mathbf{b}}_{a_{t}}=\boldsymbol{\eta}_{b_{a}}$ where $\boldsymbol{\eta}_{b_{w}}\sim\pazocal{N}(\mathbf{0},\sigma_{b_{w}}^{2}\cdot$ and $\boldsymbol{\eta}_{b_{a}}\sim\pazocal{N}(\mathbf{0},\sigma_{b_{a}}^{2}\cdot$. Most modern 9-axis IMUs provide high-frequency synchronized accelerometer, gyroscope, and magnetometer measurements, as shown in Fig.3. As such, ${}_{\text{W}}\mathbf{p}^{i},\mathbf{q}_{\text{WB}}^{i}$ and ${}_{\text{W}}\mathbf{v}^{i}$ can be propagated in the time interval $[t_{k},t_{k+1}]$ using accelerometer and gyroscope measurements. Although in practice IMU measurements may not perfectly synchronize with image timestamps, the sensor setup is calibrated using Kalibr[kalibr] beforehand. Propagation in the world frame requires knowledge of the initial state of the system at time $t_{k}$. Whenever the system state changes during optimization, repropagation is needed. Thus, the IMU preintegration is performed in the body frame to avoid repropagation whenever system state is updated during optimization for computational efficiency.

where $\boldsymbol{\alpha}_{k}^{k+1}$, $\boldsymbol{\beta}_{k}^{k+1}$ and $\boldsymbol{\gamma}_{k}^{k+1}$ are the preintegration terms which only depend on the inertial measurements and biases. These preintegration terms along with their covariance ${}_{\text{B}}\mathbf{P}_{k}^{k+1}$ can be updated iteratively using discrete accelerometer and gyroscope measurements; see [qin2017vins_mono, forster2016manifold] for details. The preintegrated terms $\boldsymbol{\alpha}_{k}^{k+1}$, $\boldsymbol{\beta}_{k}^{k+1}$ and $\boldsymbol{\gamma}_{k}^{k+1}$ are updated using the first-order approximation with respect to their biases if the bias estimates are relatively unchanged as proposed in [forster2016manifold]. Otherwise, we redo the propagation using the updated bias estimates. The inertial error $\mathbf{e_{i}}^{k}$ is obtained from Eq. 5 as

The preintegration term allows us to estimate the body orientation at time $j\in(k,k+1]$ during recursive updates which we will use to obtain the magnetometer error in the next section.

III-D Magnetometer Residuals

However, the magnetometer measures the superposition of Earth’s magnetic field and local magnetic field because of the presence of magnetic materials in the sensor’s vicinity. Thus, the magnetometer is calibrated before experiments to estimate the soft and hard iron effects. The calibration procedure is explained in the next section. During VIO initialization, we align the orientation of the body frame in the ENU direction. When properly calibrated, the Earth’s magnetic field points in the north-down direction. First, the upward-pointing gravity direction is obtained from the accelerometer measurements. The cross product of the magnetic field and the gravity direction provides the east direction. The remaining axis can be obtained from the cross product of the estimated axes. This is a common procedure used in AHRS systems such as the Madgwick filter[madwick_filter] and the complementary filter[complimentary_filter]. Formulating the magnetometer residual directly in terms of Eq. 4 has one significant drawback that any error during the initial alignment is also included in the estimation process. Thus, we formulate the magnetometer residuals as relative orientation constraint between two consecutive frames.

For the magnetometer residual formulation, we assume that the magnetometer samples are calibrated following the procedure in Section III-E. Using the preintegrated orientation term allows us to use all magnetometer measurements between consecutive frames. Given the current consecutive frame state estimates $\mathbf{x}_{k}$ and $\mathbf{x}_{k+1}$, we define the magnetometer residual for measurement at time $t_{j}\in(t_{k},t_{k+1}]$ as

where $\Delta\text{R}_{k}^{j}$ is the propagated orientation between the time interval $[t_{k},t_{j}]$ and transforms the magnetometer measurement from time $j$ to $k$. This propagated orientation is the rotation matrix representation of the intermediate preintegration result $\boldsymbol{\gamma}_{k}^{j}$. Rotation $\text{R}_{\text{BW}}^{k}\ \text{R}_{\text{WB}}^{k+1}$ represents relative orientation during the time interval $[t_{k},t_{k+1}]$ in the body frame $\text{B}_{k}$ and transforms the magnetometer measurement from time $t_{k+1}$ to $t_{k}$. By expressing the error term in the body frame, we can recursively calculate $\boldsymbol{\gamma}_{k}^{j}$ efficiently as $t_{j}<t_{k+1}$. Thus, the magnetometer error using preintegrated orientation allows us to use all the magnetometer measurements in the time interval $(k,k+1]$ to optimize the relative orientation between consecutive frames. The magnetometer residual does not depend on the position estimate; as such, it does not directly affect the state’s position estimate. However, a better orientation estimate will certainly yield a better position accuracy in the long run.

The magnetometer measurement estimated at $t_{k}$ using quaternion preintegration depends on the gyroscope noise. However, the gyroscope noise is already considered while computing $\boldsymbol{\gamma}_{k}^{j}$ and is significantly smaller than the magnetometer noise. Also, we found that the propagated orientation variance was very small compared to the magnetometer variance. Thus, for simplicity and computational efficiency, we assume that the magnetometer residual weights depend only on the magnetometer sensor noise. Moreover, when the state connected to the magnetometer residuals is marginalized, the magnetometer residuals are added as the marginalization prior along with the inertial and visual residuals.

III-E Soft and Hard Iron Effect Calibration

where S represents the soft iron matrix and $\mathbf{h}$ represents the hard iron effect. Hard iron effects arise because of the permanent magnetization of the material and also depend on the fixed sensor recording mechanism. In particular, we can think of the hard iron effect as a constant bias $\mathbf{h}$. Soft iron effects are due to the magnetization of ferromagnetic materials due to the local magnetic field and depend on the orientation with respect to the local magnetic field[magnetometer_inertial_calibration]. The soft iron effect can be represented as a 3$\times$3 symmetric matrix S. We do not consider other sources of magnetometer errors that could arise from non-orthogonality of the magnetometer axes or differences in sensitivity along the three magnetometer axes[mag_extra_calib].

IV-A Datasets

All the datasets are collected utilizing the sensor suite described in Rahman et al.[RahmanOceans2018]. The first dataset was collected using IDS UI-3251LE cameras in a stereo configuration and Microstrain 3DM-GX5-25 AHRS in the Devil’s cave system, Florida. We call this dataset cave1_short and images are captured at 15Hz. The cave1_short dataset spans approximately 220m. The second dataset was collected with the same sensor suite in Dos Ojos Cenote, QR, Mexico. This trajectory is very long ($\sim$680m) with a duration of more than 50 minutes. COLMAP was unable to finish the complete trajectory due to computational and memory constraints. Thus, this data set is used only for qualitative evaluation.

The third dataset was collected using a similar sensor suite in monocular setup with Flir BFS-U3-16S7C-C camera and microstrain 3DM-GV7 AHRS. We call this dataset cave1_long with total trajectory length of approximately 506m. Both datasets are very challenging in terms of low brightness, illumination changes due to artificial lighting, and low visibility.

IV-B Results

Due to the absence of GPS or motion capture systems underwater, COLMAP[colmap_sfm] is used to produce baseline trajectories. COLMAP is a Structure-from-Motion (SfM) framework that performs joint optimization of camera poses and map structure using global bundle adjustment. Since one of the datasets only contains monocular images, we consider the COLMAP trajectory up to scale for uniformity. Thus, the trajectories are aligned with the COLMAP baseline trajectories using sim3 alignment[umeyama].

We compare the performance of our magnetometer formulation with the VIO formulation from OKVIS[okvis] in terms of absolute trajectory error (ATE) [absolute_traj_error]. We include the results of the VIO-only case without fusing magnetometer measurements and compare them with the proposed method after sim(3) alignment. Each method is run three times and the absolute trajectory error in terms of degree/meters is reported in Table I. As seen in the table, there is a significant reduction in both translation and rotation RMSE. In particular in the cave1_long dataset, both the rotation and translation error decreased significantly from $\sim$15°to $\sim$6°and from $\sim$14m to $\sim$3m.

We also computed the relative trajectory error (RPE) that computes the relative error between states at different times with the focus on the yaw error [Zhang18iros]. RPE shows the local drift over different sections of the trajectory and is unaffected by previously accumulated error. The relative yaw error calculated for cave1_long trajectory after aligning the initial pose is shown in Fig. 6. We can see that when using the magnetometer the yaw error remains constant over large distances, whereas the VIO accumulates error when distances keep increasing.

Two different deployments from the Devil’s Eye cave system are presented in Fig. 5. The first deployment, a short foray of 100 meters penetration inside the cave, can be seen in the left image, and the drift in the trajectory (blue) without magnetometer data fusion is clear. The visual inertial magnetometer fusion (green) follows much more closely the ground truth trajectory from COLMAP. More dramatic improvements are presented in the middle figure with trajectory of more than 500 meters. Compared to the globally optimized (COLMAP) trajectory there was reduction of positional error from 2.8% to 0.6%. The right image presents a partial map of the cave system with the COLMAP trajectory superimposed. The map is not up to scale, thus some passages are not scaled uniformly. Nonetheless, the trajectory follows the main passages of the cave system.

Figure 7 presents the trajectory resulting from the deployment of the old sensor suite at the Cenote Dos Ojos. During the 50-minute deployment, the sensor covered approximately 680 meters inside a highly decorated cave. The large volume of data made the production of a COLMAP\hypbased ground truth trajectory infeasible due to computation and memory constraints. As such, this trajectory is used only as a qualitative comparison. As can be seen, the VIO diverged almost 100 meters. The trajectory obtained from magnetometer fusion follows the path out much more closely to the path taken on the way in. It is worth noting that the sensor’s trajectory does not follow exactly the same path; however, the confined structure of the cave passage constrains the inbound and the outbound trajectories fairly close.