Main Content

estimateGravityRotationAndPoseScale

Estimate gravity rotation and pose scale using IMU measurements and factor graph optimization

Since R2023a

collapse all in page

    Syntax

    [gRot,scale,info] = estimateGravityRotationAndPoseScale(poses,gyroscopeReadings,accelerometerReadings,Name=Value)

    Description

    The estimateGravityRotationAndPoseScale function estimates the gravity rotation and pose scale that helps in transforming 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 pose scale brings input poses to the metric scale, similar to IMU measurements.

    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 in transforming the input pose reference frame to the local navigation reference frame of IMU.

    Monocular camera sensor-based structure from motion (SfM) estimates poses at an unknown scale different from metric measurements obtained by an IMU. The accelerometer readings help estimate scale factor to bring input poses to metric scale similar to IMU measurements.

    [gRot,scale,info] = estimateGravityRotationAndPoseScale(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. The function also estimates scale, so input poses at an unknown scale can be converted to metric units similar to those in the IMU measurements.

    Note

    Input 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.

    example

    Examples

    collapse all

    Open Live Script

    Specify input poses in the initial 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 poses from the initial camera pose reference frame to the initial IMU pose reference frame.

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

    Specify factor graph solver options.

    opts = factorGraphSolverOptions(MaxIterations=50);

    Estimate the gravity rotation and pose scale using IMU measurements between camera frames.

    [gRot,scale,solutionInfo] = estimateGravityRotationAndPoseScale(poses, ...
                            {gyroReadings},{accelReadings}, ...
                            IMUParameters=params, ...
                            SensorTransform=sensorTransform, ...
                            SolverOptions=opts)
    gRot = 3×3

    0.9804   -0.0656   -0.1856
   -0.1765    0.1251   -0.9763
    0.0873    0.9900    0.1111


    scale = 
0.7357

    solutionInfo = struct with fields:
             InitialCost: 2.3123e+03
               FinalCost: 27.9811
      NumSuccessfulSteps: 30
    NumUnsuccessfulSteps: 21
               TotalTime: 0.0059
         TerminationType: 1
        IsSolutionUsable: 1
        OptimizedNodeIDs: [0 1 3 4 6 7]
            FixedNodeIDs: [2 5]

    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
    gravityDirection = 3×1

   -0.1856
   -0.9763
    0.1111


    % gravity vector in pose reference frame.
gravityMagnitude = 9.81;
gravity = gravityDirection*gravityMagnitude
    gravity = 3×1

   -1.8211
   -9.5777
    1.0900

    Input Arguments

    collapse all

    Camera poses, at an unknown scale estimated by monocular camera-based structure from motion (SfM), specified as one of these pose types:

    • N-by-7 matrix, where each row is of the form [x y z qw qx qy qz]. Each row defines the xyz-position, in meters, and quaternion orientation, [qw qx qy qz].

    • Array of se3 objects.

    • Camera pose table returned by the poses (Computer Vision Toolbox) function of the imageviewset (Computer Vision Toolbox) object.

    • Array of rigidtform3d (Image Processing Toolbox) objects.

    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: estimateGravityRotationAndPoseScale(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.

    Multiplier by which to scale the input poses, returned as a numeric scalar. Use this value to scale the input poses to the same metric units as the IMU measurements.

    Data Types: double

    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.

    Extended Capabilities

    C/C++ Code Generation
    Generate C and C++ code using MATLAB® Coder™.

    Version History

    Introduced in R2023a

    See Also

    | |