Main Content

# estimateGravityRotation

Estimate gravity rotation using IMU measurements and factor graph optimization

Since R2023a

## Syntax

``[gRot,info] = estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings,Name=Value)``

## Description

The `estimateGravityRotation` function estimates the gravity rotation that helps transform input poses to the local navigation reference frame of IMU using IMU measurements and factor graph optimization. The gravity rotation transforms the gravity vector from the local navigation reference frame of IMU to the pose reference frame.

The accelerometer measurements contain constant gravity acceleration that does not contribute to motion. You must remove this from the measurements for accurate fusion with other sensor data. The input pose reference frame may not always match the local navigation reference frame of IMU, North-East-Down (NED) or East-North-Up (ENU) in which the gravity direction is known. So, it is necessary to transform the input poses to the local navigation frame to remove the known gravity effect. The estimated rotation helps align the input pose reference frame and local navigation reference frame of IMU.

example

````[gRot,info] = estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings,Name=Value)` estimates the rotation required to transform the gravity vector from the local navigation reference frame of IMU (NED or ENU) to the input pose reference frame. NoteInput poses must be in the initial IMU reference frame unless you specify the `SensorTransform` name-value argument, then the poses can be in a different frame. ```

## Examples

collapse all

Specify input poses in the first camera pose reference frame.

```poses = [0.1 0 0 0.7071 0 0 0.7071; ... 0.1 0.4755 -0.1545 0.7071 0 0 0.7071];```

Specify 10 gyroscope and accelerometer readings between consecutive camera frames.

```accelReadings = repmat([97.9887 -3.0315 -22.0285],10,1); gyroReadings = zeros(10,3);```

Specify IMU parameters.

```params = factorIMUParameters(SampleRate=100, ... ReferenceFrame="NED");```

Specify a transformation consisting of 3-D translation and rotation to transform input poses from the initial camera pose reference frame to the initial IMU pose reference frame. Initial sensor reference frame has first sensor pose at the origin of the sensor reference frame.

`sensorTransform = se3(eul2rotm([-pi/2 0 0]),[0 0.1 0]);`

Specify factor graph solver options.

`opts = factorGraphSolverOptions(MaxIterations=50);`

Estimate gravity rotation using IMU measurements between camera pose estimates.

```% [gRot,solutionInfo] = estimateGravityRotation(poses, ... % {gyroReadings},{accelReadings}, ... % IMUParameters=params, ... % SensorTransform=sensorTransform, ... % SolverOptions=opts)```

Use gravity rotation to transform gravity vector from local navigation frame to initial camera pose reference frame.

```% % gravity direction in NED frame is along Z-Axis. % gravityDirectionNED = [0; 0; 1]; % % gravity direction in pose reference frame. % gravityDirection = gRot*gravityDirectionNED % % gravity vector in pose reference frame. % gravityMagnitude = 9.81; % gravity = gravityDirection*gravityMagnitude```

## Input Arguments

collapse all

Camera or lidar poses, with similar metric units as IMU measurements estimated by stereo-visual-inertial or lidar-inertial systems, respectively, specified as one of these pose types:

Gyroscope readings between consecutive camera views or poses, specified as an (N-1)-element cell array of M-by-3 matrices, in radians per second. N is the total number of camera views or poses specified in `poses`. M is the number of gyroscope readings between consecutive camera views and the three columns of `gyroscopeReadings` represent the [x y z] measurements between the camera views or poses.

Note that there can be a different number of readings between consecutive camera views.

Data Types: `cell`

Accelerometer readings between consecutive camera views or poses, specified as an (N-1)-element cell array of M-by-3 matrices, in meters per second squared. N is the total number of camera views or poses specified in `poses`. M is the number of accelerometer readings between consecutive camera views and the three columns of `accelerometerReadings` represent the [x y z] measurements between the camera views or poses.

Note that there can be a different number of readings between consecutive camera views.

Data Types: `cell`

### Name-Value Arguments

Specify optional pairs of arguments as `Name1=Value1,...,NameN=ValueN`, where `Name` is the argument name and `Value` is the corresponding value. Name-value arguments must appear after other arguments, but the order of the pairs does not matter.

Example: `estimateGravityRotation(poses,gyroscopeReadings,accelerometerReadings,IMUParameters=factorIMUParameters(SampleRate=100))` estimates the gravity rotation based on an IMU.

IMU parameters, specified as a `factorIMUParameters` object.

Example: `IMUParameters=factorIMUParameters(SampleRate=100)`

Solver options, specified as a `factorGraphSolverOptions` object.

Example: `SolverOptions=factorGraphSolverOptions(MaxIterations=50)`

Transformation consisting of 3-D translation and rotation to transform a quantity like a pose or a point in the input pose reference frame to the initial IMU sensor reference frame, specified as a `se3` object.

For example, if the input poses are camera poses in the initial camera sensor reference frame, then the sensor transform rotates and translates a pose or a point in the initial camera sensor reference frame to the initial IMU sensor reference frame. The initial sensor reference frame has the very first sensor pose at its origin.

Example: `SensorTransform=se3(eul2rotm([-pi/2,0,0]),[0,0.1,0])`

## Output Arguments

collapse all

Gravity rotation, returned as a 3-by-3 matrix, `se3` object, or `rigidtform3d` (Image Processing Toolbox) object similar to input pose type. It contains the rotation required to transform the gravity vector from the local navigation reference frame of IMU (NED or ENU) to the input pose reference frame.

Factor graph optimization solution information, returned as a structure. The fields of the structure are:

FieldDescription
`InitialCost`

Initial cost of the non-linear least squares problem formulated by the factor graph before the optimization.

`FinalCost`

Final cost of the non-linear least squares problem formulated by the factor graph after the optimization.

Note

Cost is the sum of error terms, known as residuals, where each residual is a function of a subset of factor measurements.

`NumSuccessfulSteps`

Number of iterations in which the solver decreases the cost. This value includes the initialization iteration at 0 in addition to the minimizer iterations.

`NumUnsuccessfulSteps`

Number of iterations in which the iteration is numerically invalid or the solver does not decrease the cost.

`TotalTime`

Total solver optimization time in seconds.

`TerminationType`

Termination type as an integer in the range [0, 2]:

• `0` — Solver found a solution that meets convergence criterion and decreases in cost after optimization.

• `1` — Solver could not find a solution that meets convergence criterion after running for the maximum number of iterations.

• `2` — Solver terminated due to an error.

`IsSolutionUsable`

Solution is usable if `1` (`true`), not usable if `0` (`false`).

Use this information to check whether the optimization required for alignment has converged or not.

Data Types: `struct`

## References

[1] Campos, Carlos, Richard Elvira, Juan J. Gomez Rodriguez, Jose M. M. Montiel, and Juan D. Tardos. “ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM.” IEEE Transactions on Robotics 37, no. 6 (December 2021): 1874–90. https://doi.org/10.1109/TRO.2021.3075644.

## Version History

Introduced in R2023a