ov_msckf::VioManager class

Core class that manages the entire system.

Contents

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(ros::NodeHandle& nh)
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 intialized() -> bool
If we are initialized or not.
auto get_state() -> State*
Accessor to get the current state.
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.

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.

Protected variables

State* state
Our master state object :D.
Propagator* propagator
Propagator of our state.
bool use_stereo
Boolean if we should do stereo tracking or if false do binocular.
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.
std::vector<Eigen::Vector3d> good_features_MSCKF
Good features that where used in the last update.

Function documentation

ov_msckf::VioManager::VioManager(ros::NodeHandle& nh)

Default constructor, will load all configuration variables.

Parameters
nh ROS node handler which we will load parameters from

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 initalizer 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