Will try to detect and then update using zero velocity assumption. More...

#include <UpdaterZeroVelocity.h>

Public Member Functions
void	clean_old_imu_measurements (double oldest_time)
	This will remove any IMU measurements that are older then the given measurement time. More...

void	feed_imu (const ov_core::ImuData &message, double oldest_time=-1)
	Feed function for inertial data. More...

bool	try_update (std::shared_ptr< State > state, double timestamp)
	Will first detect if the system is zero velocity, then will update. More...

	UpdaterZeroVelocity (UpdaterOptions &options, NoiseManager &noises, std::shared_ptr< ov_core::FeatureDatabase > db, std::shared_ptr< Propagator > prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, double zupt_max_disparity)
	Default constructor for our zero velocity detector and updater. More...

Protected Attributes
std::shared_ptr< ov_core::FeatureDatabase >	_db
	Feature tracker database with all features in it. More...

Eigen::Vector3d	_gravity
	Gravity vector. More...

NoiseManager	_noises
	Container for the imu noise values. More...

UpdaterOptions	_options
	Options used during update (chi2 multiplier) More...

std::shared_ptr< Propagator >	_prop
	Our propagator! More...

double	_zupt_max_disparity = 1.0
	Max disparity (pixels) that we should consider a zupt with. More...

double	_zupt_max_velocity = 1.0
	Max velocity (m/s) that we should consider a zupt with. More...

double	_zupt_noise_multiplier = 1.0
	Multiplier of our IMU noise matrix (default should be 1.0) More...

std::map< int, double >	chi_squared_table
	Chi squared 95th percentile table (lookup would be size of residual) More...

bool	have_last_prop_time_offset = false

std::vector< ov_core::ImuData >	imu_data
	Our history of IMU messages (time, angular, linear) More...

double	last_prop_time_offset = 0.0
	Estimate for time offset at last propagation time. More...

int	last_zupt_count = 0
	Number of times we have called update. More...

double	last_zupt_state_timestamp = 0.0
	Last timestamp we did zero velocity update with. More...

Detailed Description

Will try to detect and then update using zero velocity assumption.

Consider the case that a VIO unit remains stationary for a period time. Typically this can cause issues in a monocular system without SLAM features since no features can be triangulated. Additional, if features could be triangulated (e.g. stereo) the quality can be poor and hurt performance. If we can detect the cases where we are stationary then we can leverage this to prevent the need to do feature update during this period. The main application would be using this on a wheeled vehicle which needs to stop (e.g. stop lights or parking).

Definition at line 54 of file UpdaterZeroVelocity.h.

Constructor & Destructor Documentation

◆ UpdaterZeroVelocity()

UpdaterZeroVelocity::UpdaterZeroVelocity	(	UpdaterOptions &	options,
		NoiseManager &	noises,
		std::shared_ptr< ov_core::FeatureDatabase >	db,
		std::shared_ptr< Propagator >	prop,
		double	gravity_mag,
		double	zupt_max_velocity,
		double	zupt_noise_multiplier,
		double	zupt_max_disparity
	)

Default constructor for our zero velocity detector and updater.

Parameters

options	Updater options (chi2 multiplier)
noises	imu noise characteristics (continuous time)
db	Feature tracker database with all features in it
prop	Propagator class object which can predict the state forward in time
gravity_mag	Global gravity magnitude of the system (normally 9.81)
zupt_max_velocity	Max velocity we should consider to do a update with
zupt_noise_multiplier	Multiplier of our IMU noise matrix (default should be 1.0)
zupt_max_disparity	Max disparity we should consider to do a update with

Definition at line 42 of file UpdaterZeroVelocity.cpp.

Member Function Documentation

◆ clean_old_imu_measurements()

void ov_msckf::UpdaterZeroVelocity::clean_old_imu_measurements ( double oldest_time )

inline

This will remove any IMU measurements that are older then the given measurement time.

Parameters

oldest_time Time that we can discard measurements before (in IMU clock)

Definition at line 96 of file UpdaterZeroVelocity.h.

◆ feed_imu()

void ov_msckf::UpdaterZeroVelocity::feed_imu	(	const ov_core::ImuData &	message,
		double	oldest_time = `-1`
	)

inline

Feed function for inertial data.

Parameters

message	Contains our timestamp and inertial information
oldest_time	Time that we can discard measurements before

Definition at line 77 of file UpdaterZeroVelocity.h.

◆ try_update()

bool UpdaterZeroVelocity::try_update	(	std::shared_ptr< State >	state,
		double	timestamp
	)

Will first detect if the system is zero velocity, then will update.

Parameters

state	State of the filter
timestamp	Next camera timestamp we want to see if we should propagate to.

Returns: True if the system is currently at zero velocity

Definition at line 65 of file UpdaterZeroVelocity.cpp.

Member Data Documentation

◆ _db

std::shared_ptr<ov_core::FeatureDatabase> ov_msckf::UpdaterZeroVelocity::_db

protected

Feature tracker database with all features in it.

Definition at line 125 of file UpdaterZeroVelocity.h.

◆ _gravity

Eigen::Vector3d ov_msckf::UpdaterZeroVelocity::_gravity

protected

Gravity vector.

Definition at line 131 of file UpdaterZeroVelocity.h.

◆ _noises

NoiseManager ov_msckf::UpdaterZeroVelocity::_noises

protected

Container for the imu noise values.

Definition at line 122 of file UpdaterZeroVelocity.h.

◆ _options

UpdaterOptions ov_msckf::UpdaterZeroVelocity::_options

protected

Options used during update (chi2 multiplier)

Definition at line 119 of file UpdaterZeroVelocity.h.

◆ _prop

std::shared_ptr<Propagator> ov_msckf::UpdaterZeroVelocity::_prop

protected

Our propagator!

Definition at line 128 of file UpdaterZeroVelocity.h.

◆ _zupt_max_disparity

double ov_msckf::UpdaterZeroVelocity::_zupt_max_disparity = 1.0

protected

Max disparity (pixels) that we should consider a zupt with.

Definition at line 140 of file UpdaterZeroVelocity.h.

◆ _zupt_max_velocity

double ov_msckf::UpdaterZeroVelocity::_zupt_max_velocity = 1.0

protected

Max velocity (m/s) that we should consider a zupt with.

Definition at line 134 of file UpdaterZeroVelocity.h.

◆ _zupt_noise_multiplier

double ov_msckf::UpdaterZeroVelocity::_zupt_noise_multiplier = 1.0

protected

Multiplier of our IMU noise matrix (default should be 1.0)

Definition at line 137 of file UpdaterZeroVelocity.h.

◆ chi_squared_table

std::map<int, double> ov_msckf::UpdaterZeroVelocity::chi_squared_table

protected

Chi squared 95th percentile table (lookup would be size of residual)

Definition at line 143 of file UpdaterZeroVelocity.h.

◆ have_last_prop_time_offset

bool ov_msckf::UpdaterZeroVelocity::have_last_prop_time_offset = false

protected

Definition at line 150 of file UpdaterZeroVelocity.h.

◆ imu_data

std::vector<ov_core::ImuData> ov_msckf::UpdaterZeroVelocity::imu_data

protected

Our history of IMU messages (time, angular, linear)

Definition at line 146 of file UpdaterZeroVelocity.h.

◆ last_prop_time_offset

double ov_msckf::UpdaterZeroVelocity::last_prop_time_offset = 0.0

protected

Estimate for time offset at last propagation time.

Definition at line 149 of file UpdaterZeroVelocity.h.

◆ last_zupt_count

int ov_msckf::UpdaterZeroVelocity::last_zupt_count = 0

protected

Number of times we have called update.

Definition at line 156 of file UpdaterZeroVelocity.h.

◆ last_zupt_state_timestamp

double ov_msckf::UpdaterZeroVelocity::last_zupt_state_timestamp = 0.0

protected

Last timestamp we did zero velocity update with.

Definition at line 153 of file UpdaterZeroVelocity.h.

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

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ UpdaterZeroVelocity()

Member Function Documentation

◆ clean_old_imu_measurements()

◆ feed_imu()

◆ try_update()

Member Data Documentation

◆ _db

◆ _gravity

◆ _noises

◆ _options

◆ _prop

◆ _zupt_max_disparity

◆ _zupt_max_velocity

◆ _zupt_noise_multiplier

◆ chi_squared_table

◆ have_last_prop_time_offset

◆ imu_data

◆ last_prop_time_offset

◆ last_zupt_count

◆ last_zupt_state_timestamp