File internal_imu_with_gravity_factor.h¶
-
namespace sym
Functions
-
template<typename Scalar>
void InternalImuWithGravityFactor(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Pose3<Scalar> &pose_j, const Eigen::Matrix<Scalar, 3, 1> &vel_j, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_i, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &sqrt_info, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gravity, const Scalar dt, const Scalar epsilon, Eigen::Matrix<Scalar, 9, 1> *const res = nullptr, Eigen::Matrix<Scalar, 9, 27> *const jacobian = nullptr, Eigen::Matrix<Scalar, 27, 27> *const hessian = nullptr, Eigen::Matrix<Scalar, 27, 1> *const rhs = nullptr)¶ An internal helper function to linearize the difference between the orientation, velocity, and position at one time step and those of another time step. The expected difference is calculated from the preintegrated IMU measurements between those time steps.
NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use the one found in
symforce/slam/imu_preintegration/imu_factor.h.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.
Assumes sqrt_info is lower triangular and only reads the lower triangle.
Args: 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: The accelerometer bias between timesteps i and j gyro_bias_i: The gyroscope bias between timesteps i and j DR: Preintegrated estimate for pose_i.R.inverse() * pose_j.R Dv: Preintegrated estimate for vel_j - vel_i in the body frame at timestep i Dp: Preintegrated estimate for position change (before velocity and gravity corrections) in the body frame at timestep i::
sqrt_info: sqrt info matrix of DR(‘s tangent space), Dv, Dp DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat accel_bias_hat: The accelerometer bias used during preintegration gyro_bias_hat: The gyroscope bias used during preintegration gravity: 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. dt: The time between timestep i and j: t_j - t_i epsilon: epsilon used for numerical stabilityR_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2), where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i
Outputs: res: 9dof product residual between the orientations, then velocities, then positions. jacobian: (9x27) 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 (3) hessian: (27x27) 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 (3) rhs: (27x1) 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 (3)
-
template<typename Scalar>