Class sym::ImuWithGravityDirectionFactor

template<typename Scalar>
class ImuWithGravityDirectionFactor

A factor for using on-manifold IMU preintegration in a SymForce optimization problem, with the ability to optimize the gravity vector direction.

For full documentation, see ImuFactor.

Public Types

using Pose3 = sym::Pose3<Scalar>
using Vector3 = sym::Vector3<Scalar>
using Preintegrator = sym::ImuPreintegrator<Scalar>
using Measurement = sym::PreintegratedImuMeasurements<Scalar>
using SqrtInformation = sym::Matrix99<Scalar>

Public Functions

explicit ImuWithGravityDirectionFactor(const Preintegrator &preintegrator)

Construct an ImuFactor from a preintegrator.

ImuWithGravityDirectionFactor(const Measurement &measurement, const SqrtInformation &sqrt_information)

Construct an ImuFactor from a (preintegrated) measurement and its corresponding sqrt information.

sym::Factor<Scalar> Factor(const std::vector<Key> &keys_to_func) const

Construct a Factor object that can be passed to an Optimizer object given the keys to be passed to the function.

Convenience method to avoid manually specifying which arguments are optimized.

void operator()(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const Vector3 &accel_bias_i, const Vector3 &gyro_bias_i, const Unit3<Scalar> &gravity_direction, Scalar gravity_norm, Scalar epsilon, Eigen::Matrix<Scalar, 9, 1> *residual = nullptr, Eigen::Matrix<Scalar, 9, 26> *jacobian = nullptr, Eigen::Matrix<Scalar, 26, 26> *hessian = nullptr, Eigen::Matrix<Scalar, 26, 1> *rhs = nullptr) const

Calculate a between factor on the product manifold of the pose and velocity where the prior is calculated from the preintegrated IMU measurements.

Time step i is the time of the first IMU measurement of the interval. Time step j is the time after the last IMU measurement of the interval.

Parameters:

  • pose_i – Pose at time step i (world_T_body)

  • vel_i – Velocity at time step i (world frame)

  • pose_j – Pose at time step j (world_T_body)

  • vel_j – Velocity at time step j (world frame)

  • accel_bias_i – Bias of the accelerometer measurements between timesteps i and j

  • gyro_bias_i – Bias of the gyroscope measurements between timesteps i and j

  • gravity_direction – When multiplied by gravity_norm, the acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU.

  • gravity_norm – The norm of the gravity vector

  • epsilon – epsilon used for numerical stability

  • residual – [out] The 9dof whitened local coordinate difference between predicted and estimated state

  • jacobian – [out] (9x26) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2)

  • hessian – [out] (26x26) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2)

  • rhs – [out] (26x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2)