File roll_forward_state.h#
-
namespace sym
Functions
-
template<typename Scalar>
void RollForwardState(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 3, 1> &gravity, const Scalar dt, sym::Pose3<Scalar> *const res0 = nullptr, Eigen::Matrix<Scalar, 3, 1> *const res1 = nullptr)# Roll forward the given state by the given preintegrated measurements
This returns the pose_j and vel_j that give 0 residual
Args: pose_i: Pose at time step i (world_T_body) vel_i: Velocity at time step i (world frame) 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 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
Returns: T.Tuple[sf.Pose3, sf.V3]: pose_j, vel_j
-
template<typename Scalar>