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(const ov_core::ImuData& message)
Feed function for inertial data.
void feed_measurement_camera(const ov_core::CameraData& message)
Feed function for camera measurements.
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_params() -> VioManagerOptions
Accessor for current system parameters.
auto get_state() -> std::shared_ptr<State>
Accessor to get the current state.
auto get_propagator() -> std::shared_ptr<Propagator>
Accessor to get the current propagator.
auto get_historical_viz_image() -> cv::Mat
Get a nice visualization image of what tracks we have.
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.
void get_active_image(double& timestamp, cv::Mat& image)
Return the image used when projecting the active tracks.
void get_active_tracks(double& timestamp, std::unordered_map<size_t, Eigen::Vector3d>& feat_posinG, std::unordered_map<size_t, Eigen::Vector3d>& feat_tracks_uvd)
Returns active tracked features in the current frame.

Protected functions

void track_image_and_update(const ov_core::CameraData& message)
Given a new set of camera images, this will track them.
void do_feature_propagate_update(const ov_core::CameraData& message)
This will do the propagation and feature updates to the state.
auto try_to_initialize() -> bool
This function will try to initialize the state.
void retriangulate_active_tracks(const ov_core::CameraData& message)
This function will will re-triangulate all features in the current frame.

Protected variables

VioManagerOptions params
Manager parameters.
std::shared_ptr<State> state
Our master state object :D.
std::shared_ptr<Propagator> propagator
Propagator of our state.
std::shared_ptr<FeatureDatabase> trackDATABASE
Complete history of our feature tracks.
std::shared_ptr<TrackBase> trackFEATS
Our sparse feature tracker (klt or descriptor)
std::shared_ptr<TrackBase> trackARUCO
Our aruoc tracker.
std::shared_ptr<InertialInitializer> initializer
State initializer.
bool is_initialized_vio
Boolean if we are initialized or not.
std::shared_ptr<UpdaterMSCKF> updaterMSCKF
Our MSCKF feature updater.
std::shared_ptr<UpdaterSLAM> updaterSLAM
Our SLAM/ARUCO feature updater.
std::shared_ptr<UpdaterZeroVelocity> updaterZUPT
Our zero velocity tracker.
std::deque<ov_core::CameraData> camera_queue
std::shared_ptr<FeatureInitializer> active_tracks_initializer
Feature initializer used to triangulate all active tracks.

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(const ov_core::ImuData& message)

Feed function for inertial data.

Parameters
message Contains our timestamp and inertial information

void ov_msckf::VioManager::feed_measurement_camera(const ov_core::CameraData& message)

Feed function for camera measurements.

Parameters
message Contains our timestamp, images, and camera ids

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]

void ov_msckf::VioManager::track_image_and_update(const ov_core::CameraData& message) protected

Given a new set of camera images, this will track them.

Parameters
message Contains our timestamp, images, and camera ids

If we are having stereo tracking, we should call stereo tracking functions. Otherwise we will try to track on each of the images passed.

void ov_msckf::VioManager::do_feature_propagate_update(const ov_core::CameraData& message) protected

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

Parameters
message Contains our timestamp, images, and camera ids

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::retriangulate_active_tracks(const ov_core::CameraData& message) protected

This function will will re-triangulate all features in the current frame.

Parameters
message Contains our timestamp, images, and camera ids

For all features that are currently being tracked by the system, this will re-triangulate them. This is useful for downstream applications which need the current pointcloud of points (e.g. loop closure). This will try to triangulate all points, not just ones that have been used in the update.

Variable documentation

std::deque<ov_core::CameraData> ov_msckf::VioManager::camera_queue protected

Queue up camera measurements sorted by time and trigger once we have exactly one IMU measurement with timestamp newer than the camera measurement This also handles out-of-order camera measurements, which is rare, but a nice feature to have for general robustness to bad camera drivers.