ov_msckf::State class

State of our filter.

Contents

This state has all the current estimates for the filter. This system is modeled after the MSCKF filter, thus we have a sliding window of clones. We additionally have more parameters for online estimation of calibration and SLAM features. We also have the covariance of the system, which should be managed using the StateHelper class.

Public static functions

static auto Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd& vec) -> Eigen::Matrix3d
Gyroscope and accelerometer intrinsic matrix (scale imperfection and axis misalignment)
static auto Tg(const Eigen::MatrixXd& vec) -> Eigen::Matrix3d
Gyroscope gravity sensitivity.

Constructors, destructors, conversion operators

State(StateOptions& options_)
Default Constructor (will initialize variables to defaults)

Public functions

auto margtimestep() -> double
Will return the timestep that we will marginalize next. As of right now, since we are using a sliding window, this is the oldest clone. But if you wanted to do a keyframe system, you could selectively marginalize clones.
auto max_covariance_size() -> int
Calculates the current max size of the covariance.
auto imu_intrinsic_size() const -> int
Calculates the error state size for imu intrinsics.

Public variables

std::mutex _mutex_state
Mutex for locking access to the state.
double _timestamp
Current timestamp (should be the last update time in camera clock frame!)
StateOptions _options
Struct containing filter options.
std::shared_ptr<ov_type::IMU> _imu
Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba)
std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU
Map between imaging times and clone poses (q_GtoIi, p_IiinG)
std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM
Our current set of SLAM features (3d positions)
std::shared_ptr<ov_type::Vec> _calib_dt_CAMtoIMU
Time offset base IMU to camera (t_imu = t_cam + t_off)
std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> _calib_IMUtoCAM
Calibration poses for each camera (R_ItoC, p_IinC)
std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> _cam_intrinsics
Camera intrinsics.
std::unordered_map<size_t, std::shared_ptr<ov_core::CamBase>> _cam_intrinsics_cameras
Camera intrinsics camera objects.
std::shared_ptr<ov_type::Vec> _calib_imu_dw
Gyroscope IMU intrinsics (scale imperfection and axis misalignment)
std::shared_ptr<ov_type::Vec> _calib_imu_da
Accelerometer IMU intrinsics (scale imperfection and axis misalignment)
std::shared_ptr<ov_type::Vec> _calib_imu_tg
Gyroscope gravity sensitivity.
std::shared_ptr<ov_type::JPLQuat> _calib_imu_GYROtoIMU
Rotation from gyroscope frame to the "IMU" accelerometer frame (kalibr model)
std::shared_ptr<ov_type::JPLQuat> _calib_imu_ACCtoIMU
Rotation from accelerometer to the "IMU" gyroscope frame frame (rpng model)

Function documentation

static Eigen::Matrix3d ov_msckf::State::Dm(StateOptions::ImuModel imu_model, const Eigen::MatrixXd& vec)

Gyroscope and accelerometer intrinsic matrix (scale imperfection and axis misalignment)

Returns 3x3 matrix of current imu gyroscope / accelerometer intrinsics

If kalibr model, lower triangular of the matrix is used If rpng model, upper triangular of the matrix is used

static Eigen::Matrix3d ov_msckf::State::Tg(const Eigen::MatrixXd& vec)

Gyroscope gravity sensitivity.

Returns 3x3 matrix of current gravity sensitivity

For both kalibr and rpng models, this a 3x3 that is column-wise filled.

ov_msckf::State::State(StateOptions& options_)

Default Constructor (will initialize variables to defaults)

Parameters
options_ Options structure containing filter options

double ov_msckf::State::margtimestep()

Will return the timestep that we will marginalize next. As of right now, since we are using a sliding window, this is the oldest clone. But if you wanted to do a keyframe system, you could selectively marginalize clones.

Returns timestep of clone we will marginalize

int ov_msckf::State::max_covariance_size()

Calculates the current max size of the covariance.

Returns Size of the current covariance matrix

int ov_msckf::State::imu_intrinsic_size() const

Calculates the error state size for imu intrinsics.

Returns size of error state

This is used to construct our state transition which depends on if we are estimating calibration. 15 if doing intrinsics, another +9 if doing grav sensitivity