function [gRot, scale, info] = estimateGravityRotationAndPoseScale(poses, gyroscopeReadings, accelerometerReadings, varargin)
%ESTIMATEGRAVITYROTATIONANDPOSESCALE Estimate gravity rotation and 
%   pose scale using IMU measurements and factor graph optimization
%
%   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 that 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.
%
%   Inputs:
%
%       poses                  -  Camera poses, at an unknown scale 
%                                 estimated by monocular camera-based
%                                 structure from motion (Sfm), specified as
%                                 one of these pose types:
%                                 1) 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].
%                                 2) Array of se3 objects.
%                                 3) Camera pose table returned by the
%                                 poses function of the imageviewset
%                                 object.
%                                 4) Array of rigidtform3d objects.
%
%       gyroscopeReadings      -  Gyroscope readings between consecutive
%                                 camera views or poses, specified as a
%                                 cell array of N-by-3 matrix in rad/s.
%
%       accelerometerReadings  -  Accelerometer readings between
%                                 consecutive camera views or poses,
%                                 specified as a cell array of N-by-3
%                                 matrix in m/s^2.
%
%   Outputs:
%
%      GROT    - Gravity rotation, returned as a 3-by-3 matrix, se3 object,
%                or rigidtform3d 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.
% 
%      SCALE   - Scalar multiplier 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.
% 
%      INFO    - Factor graph optimization solution information, returned
%                as a structure. Use this information to check whether the
%                optimization required for alignment has converged or not.
%
%
%   estimateGravityRotationAndPoseScale Name-Value arguments:
%
%      IMUParameters   - IMU parameters, specified as a
%                          factorIMUParameters object.
%
%        Default: factorIMUParameters()
%
%      SolverOptions   - Solver options, specified as a
%                          factorGraphSolverOptions object.
%
%        Default: factorGraphSolverOptions()
%
%      SensorTransform - Transformation consisting of 3-D translation and
%                        rotation to transform a quantity like a pose or a
%                        point from input pose reference frame to 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.
%                        Initial sensor reference frame has the very first
%                        sensor pose at its origin.
%
%        Default: se3()
%
%
%   Example:
%      % Specify input poses in the first camera pose reference frame.
%      poses = [0.1,0,0,0.7071,0,0,0.7071; ...
%               0.1,2.3776,-0.7725,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 rigid transformation to convert poses from the first
%      % camera pose reference frame to the first 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 gravity rotation and pose scale.
%      [gRot, scale, solutionInfo] = ...
%                 estimateGravityRotationAndPoseScale(...
%                 poses, {gyroReadings}, {accelReadings}, ...
%                 IMUParameters = params, ...
%                 SensorTransform = sensorTransform,...
%                 SolverOptions = opts);
%
%   References:
%
%   [1] C. Campos, R. Elvira, J. J. G. Rodriguez, J. M. M. Montiel and J.
%   D. Tardos, "ORB-SLAM3: An Accurate Open-Source Library for Visual,
%   Visual-Inertial, and Multimap SLAM," in IEEE Transactions on Robotics,
%   vol. 37, no. 6, pp. 1874-1890, Dec. 2021, doi:
%   10.1109/TRO.2021.3075644.
%
%
%   See also factorIMU, factorIMUParameters, estimateGravityRotation   

%   Copyright 2022-2023 The MathWorks, Inc.

%#codegen

    narginchk(3,9);
    
    [gRot, scale, info] = nav.algs.internal.estimateGS(...
        true, 'estimateGravityRotationAndPoseScale',...
        poses, gyroscopeReadings, accelerometerReadings, varargin{:});

end