Core class that manages the entire system. More...

#include <VioManager.h>

Public Member Functions
void	feed_measurement_camera (const ov_core::CameraData &message)
	Feed function for camera measurements. More...

void	feed_measurement_imu (const ov_core::ImuData &message)
	Feed function for inertial data. More...

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. More...

void	get_active_image (double &timestamp, cv::Mat &image)
	Return the image used when projecting the active tracks. More...

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. More...

std::vector< Eigen::Vector3d >	get_features_ARUCO ()
	Returns 3d ARUCO features in the global frame. More...

std::vector< Eigen::Vector3d >	get_features_SLAM ()
	Returns 3d SLAM features in the global frame. More...

std::vector< Eigen::Vector3d >	get_good_features_MSCKF ()
	Returns 3d features used in the last update in global frame. More...

cv::Mat	get_historical_viz_image ()
	Get a nice visualization image of what tracks we have. More...

VioManagerOptions	get_params ()
	Accessor for current system parameters. More...

std::shared_ptr< Propagator >	get_propagator ()
	Accessor to get the current propagator. More...

std::shared_ptr< State >	get_state ()
	Accessor to get the current state. More...

void	initialize_with_gt (Eigen::Matrix< double, 17, 1 > imustate)
	Given a state, this will initialize our IMU state. More...

bool	initialized ()
	If we are initialized or not. More...

double	initialized_time ()
	Timestamp that the system was initialized at. More...

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

Protected Member Functions
void	do_feature_propagate_update (const ov_core::CameraData &message)
	This will do the propagation and feature updates to the state. More...

void	retriangulate_active_tracks (const ov_core::CameraData &message)
	This function will will re-triangulate all features in the current frame. More...

void	track_image_and_update (const ov_core::CameraData &message)
	Given a new set of camera images, this will track them. More...

bool	try_to_initialize (const ov_core::CameraData &message)
	This function will try to initialize the state. More...

Protected Attributes
std::map< size_t, Eigen::Matrix3d >	active_feat_linsys_A

std::map< size_t, Eigen::Vector3d >	active_feat_linsys_b

std::map< size_t, int >	active_feat_linsys_count

cv::Mat	active_image

std::unordered_map< size_t, Eigen::Vector3d >	active_tracks_posinG

double	active_tracks_time = -1

std::unordered_map< size_t, Eigen::Vector3d >	active_tracks_uvd

std::vector< double >	camera_queue_init

std::mutex	camera_queue_init_mtx

bool	did_zupt_update = false

double	distance = 0

std::vector< Eigen::Vector3d >	good_features_MSCKF

bool	has_moved_since_zupt = false

std::shared_ptr< ov_init::InertialInitializer >	initializer
	State initializer. More...

bool	is_initialized_vio = false
	Boolean if we are initialized or not. More...

std::ofstream	of_statistics

VioManagerOptions	params
	Manager parameters. More...

std::shared_ptr< Propagator >	propagator
	Propagator of our state. More...

boost::posix_time::ptime	rT1

boost::posix_time::ptime	rT2

boost::posix_time::ptime	rT3

boost::posix_time::ptime	rT4

boost::posix_time::ptime	rT5

boost::posix_time::ptime	rT6

boost::posix_time::ptime	rT7

double	startup_time = -1

std::shared_ptr< State >	state
	Our master state object :D. More...

std::atomic< bool >	thread_init_running

std::atomic< bool >	thread_init_success

double	timelastupdate = -1

std::shared_ptr< ov_core::TrackBase >	trackARUCO
	Our aruoc tracker. More...

std::shared_ptr< ov_core::TrackBase >	trackFEATS
	Our sparse feature tracker (klt or descriptor) More...

std::shared_ptr< UpdaterMSCKF >	updaterMSCKF
	Our MSCKF feature updater. More...

std::shared_ptr< UpdaterSLAM >	updaterSLAM
	Our SLAM/ARUCO feature updater. More...

std::shared_ptr< UpdaterZeroVelocity >	updaterZUPT
	Our zero velocity tracker. More...

Detailed Description

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.

Definition at line 62 of file VioManager.h.

Constructor & Destructor Documentation

◆ VioManager()

VioManager::VioManager ( VioManagerOptions & params_ )

Default constructor, will load all configuration variables.

Parameters

params_ Parameters loaded from either ROS or CMDLINE

Definition at line 50 of file VioManager.cpp.

Member Function Documentation

◆ do_feature_propagate_update()

void 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

Definition at line 323 of file VioManager.cpp.

◆ feed_measurement_camera()

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

inline

Feed function for camera measurements.

Parameters

message Contains our timestamp, images, and camera ids

Definition at line 81 of file VioManager.h.

◆ feed_measurement_imu()

void VioManager::feed_measurement_imu ( const ov_core::ImuData & message )

Feed function for inertial data.

Parameters

message Contains our timestamp and inertial information

Definition at line 166 of file VioManager.cpp.

◆ feed_measurement_simulation()

void 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

Definition at line 191 of file VioManager.cpp.

◆ get_active_image()

void ov_msckf::VioManager::get_active_image	(	double &	timestamp,
		cv::Mat &	image
	)

inline

Return the image used when projecting the active tracks.

Definition at line 126 of file VioManager.h.

◆ get_active_tracks()

void ov_msckf::VioManager::get_active_tracks	(	double &	timestamp,
		std::unordered_map< size_t, Eigen::Vector3d > &	feat_posinG,
		std::unordered_map< size_t, Eigen::Vector3d > &	feat_tracks_uvd
	)

inline

Returns active tracked features in the current frame.

Definition at line 132 of file VioManager.h.

◆ get_features_ARUCO()

std::vector< Eigen::Vector3d > VioManager::get_features_ARUCO ( )

Returns 3d ARUCO features in the global frame.

Definition at line 440 of file VioManagerHelper.cpp.

◆ get_features_SLAM()

std::vector< Eigen::Vector3d > VioManager::get_features_SLAM ( )

Returns 3d SLAM features in the global frame.

Definition at line 417 of file VioManagerHelper.cpp.

◆ get_good_features_MSCKF()

std::vector<Eigen::Vector3d> ov_msckf::VioManager::get_good_features_MSCKF ( )

inline

Returns 3d features used in the last update in global frame.

Definition at line 123 of file VioManager.h.

◆ get_historical_viz_image()

cv::Mat VioManager::get_historical_viz_image ( )

Get a nice visualization image of what tracks we have.

Definition at line 389 of file VioManagerHelper.cpp.

◆ get_params()

VioManagerOptions ov_msckf::VioManager::get_params ( )

inline

Accessor for current system parameters.

Definition at line 105 of file VioManager.h.

◆ get_propagator()

std::shared_ptr<Propagator> ov_msckf::VioManager::get_propagator ( )

inline

Accessor to get the current propagator.

Definition at line 111 of file VioManager.h.

◆ get_state()

std::shared_ptr<State> ov_msckf::VioManager::get_state ( )

inline

Accessor to get the current state.

Definition at line 108 of file VioManager.h.

◆ initialize_with_gt()

void 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]

Definition at line 40 of file VioManagerHelper.cpp.

◆ initialized()

bool ov_msckf::VioManager::initialized ( )

inline

If we are initialized or not.

Definition at line 99 of file VioManager.h.

◆ initialized_time()

double ov_msckf::VioManager::initialized_time ( )

inline

Timestamp that the system was initialized at.

Definition at line 102 of file VioManager.h.

◆ retriangulate_active_tracks()

void VioManager::retriangulate_active_tracks ( const ov_core::CameraData & message )

protected

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

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.

Parameters

message Contains our timestamp, images, and camera ids

Definition at line 190 of file VioManagerHelper.cpp.

◆ track_image_and_update()

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

protected

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

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

Parameters

message Contains our timestamp, images, and camera ids

Definition at line 256 of file VioManager.cpp.

◆ try_to_initialize()

bool VioManager::try_to_initialize ( const ov_core::CameraData & message )

protected

This function will try to initialize the state.

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.

Parameters

message Contains our timestamp, images, and camera ids

Returns: True if we have successfully initialized

Definition at line 78 of file VioManagerHelper.cpp.

Member Data Documentation

◆ active_feat_linsys_A

std::map<size_t, Eigen::Matrix3d> ov_msckf::VioManager::active_feat_linsys_A

protected

Definition at line 241 of file VioManager.h.

◆ active_feat_linsys_b

std::map<size_t, Eigen::Vector3d> ov_msckf::VioManager::active_feat_linsys_b

protected

Definition at line 242 of file VioManager.h.

◆ active_feat_linsys_count

std::map<size_t, int> ov_msckf::VioManager::active_feat_linsys_count

protected

Definition at line 243 of file VioManager.h.

◆ active_image

cv::Mat ov_msckf::VioManager::active_image

protected

Definition at line 240 of file VioManager.h.

◆ active_tracks_posinG

std::unordered_map<size_t, Eigen::Vector3d> ov_msckf::VioManager::active_tracks_posinG

protected

Definition at line 238 of file VioManager.h.

◆ active_tracks_time

double ov_msckf::VioManager::active_tracks_time = -1

protected

Definition at line 237 of file VioManager.h.

◆ active_tracks_uvd

std::unordered_map<size_t, Eigen::Vector3d> ov_msckf::VioManager::active_tracks_uvd

protected

Definition at line 239 of file VioManager.h.

◆ camera_queue_init

std::vector<double> ov_msckf::VioManager::camera_queue_init

protected

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

Definition at line 211 of file VioManager.h.

◆ camera_queue_init_mtx

std::mutex ov_msckf::VioManager::camera_queue_init_mtx

protected

Definition at line 212 of file VioManager.h.

◆ did_zupt_update

bool ov_msckf::VioManager::did_zupt_update = false

protected

Definition at line 229 of file VioManager.h.

◆ distance

double ov_msckf::VioManager::distance = 0

protected

Definition at line 220 of file VioManager.h.

◆ good_features_MSCKF

std::vector<Eigen::Vector3d> ov_msckf::VioManager::good_features_MSCKF

protected

Definition at line 233 of file VioManager.h.

◆ has_moved_since_zupt

bool ov_msckf::VioManager::has_moved_since_zupt = false

protected

Definition at line 230 of file VioManager.h.

◆ initializer

std::shared_ptr<ov_init::InertialInitializer> ov_msckf::VioManager::initializer

protected

State initializer.

Definition at line 195 of file VioManager.h.

◆ is_initialized_vio

bool ov_msckf::VioManager::is_initialized_vio = false

protected

Boolean if we are initialized or not.

Definition at line 198 of file VioManager.h.

◆ of_statistics

std::ofstream ov_msckf::VioManager::of_statistics

protected

Definition at line 215 of file VioManager.h.

◆ params

VioManagerOptions ov_msckf::VioManager::params

protected

Manager parameters.

Definition at line 180 of file VioManager.h.

◆ propagator

std::shared_ptr<Propagator> ov_msckf::VioManager::propagator

protected

Propagator of our state.

Definition at line 186 of file VioManager.h.

◆ rT1

boost::posix_time::ptime ov_msckf::VioManager::rT1

protected

Definition at line 216 of file VioManager.h.

◆ rT2

boost::posix_time::ptime ov_msckf::VioManager::rT2

protected

Definition at line 216 of file VioManager.h.

◆ rT3

boost::posix_time::ptime ov_msckf::VioManager::rT3

protected

Definition at line 216 of file VioManager.h.

◆ rT4

boost::posix_time::ptime ov_msckf::VioManager::rT4

protected

Definition at line 216 of file VioManager.h.

◆ rT5

boost::posix_time::ptime ov_msckf::VioManager::rT5

protected

Definition at line 216 of file VioManager.h.

◆ rT6

boost::posix_time::ptime ov_msckf::VioManager::rT6

protected

Definition at line 216 of file VioManager.h.

◆ rT7

boost::posix_time::ptime ov_msckf::VioManager::rT7

protected

Definition at line 216 of file VioManager.h.

◆ startup_time

double ov_msckf::VioManager::startup_time = -1

protected

Definition at line 223 of file VioManager.h.

◆ state

std::shared_ptr<State> ov_msckf::VioManager::state

protected

Our master state object :D.

Definition at line 183 of file VioManager.h.

◆ thread_init_running

std::atomic<bool> ov_msckf::VioManager::thread_init_running

protected

Definition at line 226 of file VioManager.h.

◆ thread_init_success

std::atomic<bool> ov_msckf::VioManager::thread_init_success

protected

Definition at line 226 of file VioManager.h.

◆ timelastupdate

double ov_msckf::VioManager::timelastupdate = -1

protected

Definition at line 219 of file VioManager.h.

◆ trackARUCO

std::shared_ptr<ov_core::TrackBase> ov_msckf::VioManager::trackARUCO

protected

Our aruoc tracker.

Definition at line 192 of file VioManager.h.

◆ trackFEATS

std::shared_ptr<ov_core::TrackBase> ov_msckf::VioManager::trackFEATS

protected

Our sparse feature tracker (klt or descriptor)

Definition at line 189 of file VioManager.h.

◆ updaterMSCKF

std::shared_ptr<UpdaterMSCKF> ov_msckf::VioManager::updaterMSCKF

protected

Our MSCKF feature updater.

Definition at line 201 of file VioManager.h.

◆ updaterSLAM

std::shared_ptr<UpdaterSLAM> ov_msckf::VioManager::updaterSLAM

protected

Our SLAM/ARUCO feature updater.

Definition at line 204 of file VioManager.h.

◆ updaterZUPT

std::shared_ptr<UpdaterZeroVelocity> ov_msckf::VioManager::updaterZUPT

protected

Our zero velocity tracker.

Definition at line 207 of file VioManager.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ VioManager()

Member Function Documentation

◆ do_feature_propagate_update()

◆ feed_measurement_camera()

◆ feed_measurement_imu()

◆ feed_measurement_simulation()

◆ get_active_image()

◆ get_active_tracks()

◆ get_features_ARUCO()

◆ get_features_SLAM()

◆ get_good_features_MSCKF()

◆ get_historical_viz_image()

◆ get_params()

◆ get_propagator()

◆ get_state()

◆ initialize_with_gt()

◆ initialized()

◆ initialized_time()

◆ retriangulate_active_tracks()

◆ track_image_and_update()

◆ try_to_initialize()

Member Data Documentation

◆ active_feat_linsys_A

◆ active_feat_linsys_b

◆ active_feat_linsys_count

◆ active_image

◆ active_tracks_posinG

◆ active_tracks_time

◆ active_tracks_uvd

◆ camera_queue_init

◆ camera_queue_init_mtx

◆ did_zupt_update

◆ distance

◆ good_features_MSCKF

◆ has_moved_since_zupt

◆ initializer

◆ is_initialized_vio

◆ of_statistics

◆ params

◆ propagator

◆ rT1

◆ rT2

◆ rT3

◆ rT4

◆ rT5

◆ rT6

◆ rT7

◆ startup_time

◆ state

◆ thread_init_running

◆ thread_init_success

◆ timelastupdate

◆ trackARUCO

◆ trackFEATS

◆ updaterMSCKF

◆ updaterSLAM

◆ updaterZUPT