ov_msckf::VioManager class

Core class that manages the entire system.

This class contains the state and other algorithms needed for the MSCKF to work. We feed in measurements into this class and send them to their respective algorithms. If we have measurements to propagate or update with, this class will call on our state to do that.

Constructors, destructors, conversion operators

VioManager(VioManagerOptions& params_)
Default constructor, will load all configuration variables.

Public functions

void feed_measurement_imu(double timestamp, Eigen::Vector3d wm, Eigen::Vector3d am)
Feed function for inertial data.
void feed_measurement_monocular(double timestamp, cv::Mat& img0, size_t cam_id)
Feed function for a single camera.
void feed_measurement_stereo(double timestamp, cv::Mat& img0, cv::Mat& img1, size_t cam_id0, size_t cam_id1)
Feed function for stereo camera pair.
void feed_measurement_simulation(double timestamp, const std::vector<int>& camids, const std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>>& feats)
Feed function for a synchronized simulated cameras.
void initialize_with_gt(Eigen::Matrix<double, 17, 1> imustate)
Given a state, this will initialize our IMU state.
auto initialized() -> bool
If we are initialized or not.
auto initialized_time() -> double
Timestamp that the system was initialized at.
auto get_state() -> State*
Accessor to get the current state.
auto get_propagator() -> Propagator*
Accessor to get the current propagator.
auto get_track_feat() -> TrackBase*
Get feature tracker.
auto get_track_aruco() -> TrackBase*
Get aruco feature tracker.
auto get_good_features_MSCKF() -> std::vector<Eigen::Vector3d>
Returns 3d features used in the last update in global frame.
auto get_features_SLAM() -> std::vector<Eigen::Vector3d>
Returns 3d SLAM features in the global frame.
auto get_features_ARUCO() -> std::vector<Eigen::Vector3d>
Returns 3d ARUCO features in the global frame.
auto did_zero_velocity_update() -> bool
Return true if we did a zero velocity update.
auto get_zero_velocity_update_image() -> cv::Mat
Return the zero velocity update image.
auto hist_last_marg_state(double& timestamp, Eigen::Matrix<double, 7, 1>& stateinG) -> bool
Returns the last timestamp we have marginalized (true if we have a state)
void hist_get_features(std::unordered_map<size_t, Eigen::Vector3d>& feat_posinG, std::unordered_map<size_t, std::unordered_map<size_t, std::vector<Eigen::VectorXf>>>& feat_uvs, std::unordered_map<size_t, std::unordered_map<size_t, std::vector<Eigen::VectorXf>>>& feat_uvs_norm, std::unordered_map<size_t, std::unordered_map<size_t, std::vector<double>>>& feat_timestamps)
Returns historical feature positions, and measurements times and uvs used to get its estimate.

Protected functions

auto try_to_initialize() -> bool
This function will try to initialize the state.
void do_feature_propagate_update(double timestamp)
This will do the propagation and feature updates to the state.
void update_keyframe_historical_information(const std::vector<Feature*>& features)
This function will update our historical tracking information. This historical information includes the best estimate of a feature in the global frame. For all features it also has the normalized and raw pixel coordinates at each timestep. The state is also recorded after it is marginalized out of the state.

Protected variables

VioManagerOptions params
Manager parameters.
State* state
Our master state object :D.
Propagator* propagator
Propagator of our state.
TrackBase* trackFEATS
Our sparse feature tracker (klt or descriptor)
TrackBase* trackARUCO
Our aruoc tracker.
InertialInitializer* initializer
State initializer.
bool is_initialized_vio
Boolean if we are initialized or not.
UpdaterMSCKF* updaterMSCKF
Our MSCKF feature updater.
UpdaterSLAM* updaterSLAM
Our MSCKF feature updater.
UpdaterZeroVelocity* updaterZUPT
Our aruoc tracker.
std::vector<Eigen::Vector3d> good_features_MSCKF
Good features that where used in the last update.

Function documentation

ov_msckf::VioManager::VioManager(VioManagerOptions& params_)

Default constructor, will load all configuration variables.

Parameters
params_ Parameters loaded from either ROS or CMDLINE

void ov_msckf::VioManager::feed_measurement_imu(double timestamp, Eigen::Vector3d wm, Eigen::Vector3d am)

Feed function for inertial data.

Parameters
timestamp Time of the inertial measurement
wm Angular velocity
am Linear acceleration

void ov_msckf::VioManager::feed_measurement_monocular(double timestamp, cv::Mat& img0, size_t cam_id)

Feed function for a single camera.

Parameters
timestamp Time that this image was collected
img0 Grayscale image
cam_id Unique id of what camera the image is from

void ov_msckf::VioManager::feed_measurement_stereo(double timestamp, cv::Mat& img0, cv::Mat& img1, size_t cam_id0, size_t cam_id1)

Feed function for stereo camera pair.

Parameters
timestamp Time that this image was collected
img0 Grayscale image
img1 Grayscale image
cam_id0 Unique id of what camera the image is from
cam_id1 Unique id of what camera the image is from

void ov_msckf::VioManager::feed_measurement_simulation(double timestamp, const std::vector<int>& camids, const std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>>& feats)

Feed function for a synchronized simulated cameras.

Parameters
timestamp Time that this image was collected
camids Camera ids that we have simulated measurements for
feats Raw uv simulated measurements

void ov_msckf::VioManager::initialize_with_gt(Eigen::Matrix<double, 17, 1> imustate)

Given a state, this will initialize our IMU state.

Parameters
imustate State in the MSCKF ordering: [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel]

bool ov_msckf::VioManager::try_to_initialize() protected

This function will try to initialize the state.

Returns True if we have successfully initialized

This should call on our initializer and try to init the state. In the future we should call the structure-from-motion code from here. This function could also be repurposed to re-initialize the system after failure. *

void ov_msckf::VioManager::do_feature_propagate_update(double timestamp) protected

This will do the propagation and feature updates to the state.

Parameters
timestamp The most recent timestamp we have tracked to

void ov_msckf::VioManager::update_keyframe_historical_information(const std::vector<Feature*>& features) protected

This function will update our historical tracking information. This historical information includes the best estimate of a feature in the global frame. For all features it also has the normalized and raw pixel coordinates at each timestep. The state is also recorded after it is marginalized out of the state.

Parameters
features Features using in the last update phase