ov_msckf::State class

State of our filter.

Reference

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.

Constructors, destructors, conversion operators

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

Public functions

void initialize_variables(): Initializes pointers and covariance.
void update(const Eigen::MatrixXd dx): Given an update vector for the entire covariance, updates each variable.
void set_timestamp(double timestamp): Set the current timestamp of the filter.
auto margtimestep() -> double: Will return the timestep that we will marginalize next.
auto timestamp() -> double: Access current timestamp.
auto options() -> StateOptions&: Access options struct.
auto imu() -> IMU*: Access to IMU type.
auto Cov() -> Eigen::MatrixXd&: Access covariance matrix.
auto n_vars() -> size_t: Get size of covariance.
auto calib_dt_CAMtoIMU() -> Vec*: Access imu-camera time offset type.
auto get_clone(double timestamp) -> PoseJPL*: Access to a given clone.
auto get_clones() -> std::map<double, PoseJPL*>: Access to all current clones in the state.
auto n_clones() -> size_t: Get current number of clones.
auto get_calib_IMUtoCAM(size_t id) -> PoseJPL*: Access to a given camera extrinsics.
auto get_calib_IMUtoCAMs() -> std::unordered_map<size_t, PoseJPL*>: Access to all current extrinsic calibration between IMU and each camera.
auto get_intrinsics_CAM(size_t id) -> Vec*: Access to a given camera intrinsics.
auto get_model_CAM(size_t id) -> bool&: Access to a given camera intrinsics.
void insert_SLAM_feature(size_t featid, Landmark* landmark): Add a SLAM feature.
auto features_SLAM() -> std::unordered_map<size_t, Landmark*>&: Access SLAM features.

Protected variables

double _timestamp: Current timestamp (should be the last update time!)
StateOptions _options: Struct containing filter options.
IMU* _imu: Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba)
std::map<double, PoseJPL*> _clones_IMU: Map between imaging times and clone poses (q_GtoIi, p_IiinG)
std::unordered_map<size_t, Landmark*> _features_SLAM: Our current set of SLAM features (3d positions)
Vec* _calib_dt_CAMtoIMU: Time offset base IMU to camera (t_imu = t_cam + t_off)
std::unordered_map<size_t, PoseJPL*> _calib_IMUtoCAM: Calibration poses for each camera (R_ItoC, p_IinC)
std::unordered_map<size_t, Vec*> _cam_intrinsics: Camera intrinsics.
std::unordered_map<size_t, bool> _cam_intrinsics_model: What distortion model we are using (false=radtan, true=fisheye)
Eigen::MatrixXd _Cov: Covariance of all active variables.
std::vector<Type*> _variables: Vector of variables.

Function documentation

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

Default Constructor (will initialize variables to defaults)

Parameters
options_	Options structure containing filter options

void ov_msckf::State::initialize_variables()

Initializes pointers and covariance.

TODO: Read initial values and covariance from options

void ov_msckf::State::update(const Eigen::MatrixXd dx)

Given an update vector for the entire covariance, updates each variable.

Parameters
dx	Correction vector for the entire filter state

void ov_msckf::State::set_timestamp(double timestamp)

Set the current timestamp of the filter.

Parameters
timestamp	New timestamp

double ov_msckf::State::margtimestep()

Will return the timestep that we will marginalize next.

Returns	timestep of clone we will marginalize

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.