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(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_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 get_good_features_MSCKF() -> std::vector<Eigen::Vector3d>
- Returns 3d features used in the last update in 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(const ov_core::
CameraData& message) -> 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<ov_core::
TrackBase> trackFEATS - Our sparse feature tracker (klt or descriptor)
-
std::shared_ptr<ov_core::
TrackBase> trackARUCO - Our aruoc tracker.
-
std::shared_ptr<ov_init::
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::vector<double> camera_queue_init
Function documentation
ov_msckf::
Default constructor, will load all configuration variables.
| Parameters | |
|---|---|
| params_ | Parameters loaded from either ROS or CMDLINE |
void ov_msckf::
Feed function for inertial data.
| Parameters | |
|---|---|
| message | Contains our timestamp and inertial information |
void ov_msckf::
Feed function for camera measurements.
| Parameters | |
|---|---|
| message | Contains our timestamp, images, and camera ids |
void ov_msckf::
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::
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::
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::
This will do the propagation and feature updates to the state.
| Parameters | |
|---|---|
| message | Contains our timestamp, images, and camera ids |
bool ov_msckf::
This function will try to initialize the state.
| Parameters | |
|---|---|
| message | Contains our timestamp, images, and camera ids |
| 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::
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::vector<double> ov_msckf::
This is the queue of measurement times that have come in since we starting doing initialization After we initialize, we will want to prop & update to the latest timestamp quickly