PreintegratedRotation

Overview¶

PreintegratedRotation is a class within GTSAM used primarily for pre-integrating rotation rate measurements in factor graph-based estimation problems. It facilitates efficient handling of rotation rate measurements, typically from gyroscopes in inertial measurement units (IMU). By preintegrating rotations between two time instances, it enables effective and accurate state estimation without repeatedly recalculating rotations from individual measurements.

This is the base class for PreintegratedAhrsMeasurements defined in AHRSFactor.h and used in the AHRSFactor. The IMUFactor has its own integration classes, derived from PreintegrationBase.

Mathematical Background¶

The class employs exponential maps and logarithms in the Lie group $SO(3)$ . The rotation integration leverages incremental updates via rotation matrices and their tangent spaces, utilizing efficient methods from Lie group theory:

Exponential map: Converts a rotation vector ω into a rotation matrix using:
$R = \exp([\omega]_\times)$
(1)
Logarithmic map: Converts rotation matrices back into rotation vectors:
$\omega = \log(R)$
(2)

Here, $[\omega]_\times$ denotes the skew-symmetric matrix associated with vector ω.

The preintegration process updates rotations incrementally according to:

R_{k+1} = R_k \exp\left(\left[\omega_{\text{bias-corrected}}\Delta t\right]_\times\right)

(3)

Bias correction is performed as:

\omega_{\text{bias-corrected}} = \omega_{\text{measured}} - b_\omega

(4)

PreintegratedRotation Parameters¶

The PreintegratedRotationParams class configures the gyroscope preintegration:

Parameter	Description
`gyroscopeCovariance`	3×3 continuous-time noise covariance matrix of gyroscope measurements (units: rad²/s²/Hz)
`omegaCoriolis`	Optional Coriolis acceleration compensation vector (for earth rotation effects)
`body_P_sensor`	Optional pose transformation between the IMU sensor frame and body frame

The covariance matrix represents the uncertainty in angular velocity measurements, which propagates into orientation uncertainty during integration. When the IMU is not located at the center of the body frame, the optional body-to-sensor transformation allows for proper handling of lever-arm effects.

In C++ you typically create a shared_ptr to a single object and pass it to all PreintegratedRotation objects. In python this is automatic, as all objects are encapsulated in shared pointers:

from gtsam import PreintegratedRotationParams
params = PreintegratedRotationParams()
print(params)


gyroscopeCovariance:
1 0 0
0 1 0
0 0 1

You can (and should) configure using your gyroscope’s noise covariance matrix and the optional Coriolis acceleration compensation vector:

Setters¶

setGyroscopeCovariance: Sets the 3×3 continuous-time noise covariance matrix for gyroscope measurements.
setOmegaCoriolis: Sets an optional Coriolis acceleration compensation vector.
setBodyPSensor: Sets an optional pose transformation between the body frame and the sensor frame.

Getters¶

getGyroscopeCovariance: Returns the gyroscope covariance matrix.
getOmegaCoriolis: Returns the optional Coriolis acceleration vector.
getBodyPSensor: Returns the optional body-to-sensor pose transformation.

Core Methods¶

Typically you use these methods between timesteps $t_i$ and $t_j$ :

PreintegratedRotation: Constructs the object, needs a shared pointer to parameters.
resetIntegration: Clears the integrated rotation, typically used after updating state estimates or correcting biases.
integrateGyroMeasurement: Integrates incremental rotation measurements, given angular velocities and integration intervals.

You can then query:

deltaTij: Accumulated time interval.
deltaRij: The integrated rotation between the two timestamps as element of $SO(3)$ .
biascorrectedDeltaRij: A bias corrected version of the integrated rotation, given an increment on the estimated bias.

Internals¶

delRdelBiasOmega: Returns Jacobian of integrated rotation with respect to gyroscope bias. This is useful for updating bias estimates during optimization.

Example¶

from gtsam import PreintegratedRotation, Point3
import numpy as np

# Create a PreintegratedRotation object using the params
preintegrated_rotation = PreintegratedRotation(params)

# Add random omega measurements
biasHat = np.zeros(3)  # Assuming no bias for simplicity
np.random.seed(42)  # For reproducibility
rotations = []  # List to record integrated rotations
for _ in range(15):  # Add 15 random measurements, biased to move around z-axis
    omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3)  # Random angular velocity vector
    preintegrated_rotation.integrateGyroMeasurement(omega, biasHat, deltaT=0.1)
    rotations.append(preintegrated_rotation.deltaRij())

print(preintegrated_rotation)

    deltaTij [1.5]
    deltaRij.ypr = (  -0.82321 -0.0142842  0.0228577)

We can show the evolution of the integrated rotation by showing how it transforms the point (1,0,0) on the sphere.

import plotly.graph_objects as go
phi, theta = np.meshgrid(np.linspace(0, np.pi, 50), np.linspace(0, 2 * np.pi, 50))
fig = go.Figure(go.Surface(
    x=np.sin(phi) * np.cos(theta), y=np.sin(phi) * np.sin(theta), z=np.cos(phi),
    opacity=0.3, colorscale='Viridis', showscale=False
))
etas_np = np.array([R.matrix()[:,0] for R in rotations])
time_steps = np.linspace(0, 1, len(etas_np))  # Normalize time steps between 0 and 1
colors = time_steps  # Use time steps as color values

fig.add_trace(go.Scatter3d(
    x=etas_np[:, 0], y=etas_np[:, 1], z=etas_np[:, 2],
    mode='markers+lines',
    marker=dict(size=5, color=colors, colorscale='Hot', colorbar=dict(title='Time')),
    line=dict(color='grey')
))
fig.update_layout(scene_camera=dict(eye=dict(x=1.1, y=-1.1, z=0.2)), margin=dict(l=0, r=0, t=0, b=0))
fig.show()

Source¶

GTSAM Docs

PreintegratedImuMeasurements

GTSAM Docs

PreintegrationCombinedParams