ov_msckf::UpdaterZeroVelocity class

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

Constructors, destructors, conversion operators

UpdaterZeroVelocity(UpdaterOptions& options, Propagator::NoiseManager& noises, std::shared_ptr<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.

Public functions

void feed_imu(const ov_core::ImuData& message)
Feed function for inertial data.
auto try_update(std::shared_ptr<State> state, double timestamp) -> bool
Will first detect if the system is zero velocity, then will update.

Protected variables

UpdaterOptions _options
Options used during update (chi2 multiplier)
Propagator::NoiseManager _noises
Container for the imu noise values.
std::shared_ptr<FeatureDatabase> _db
Feature tracker database with all features in it.
std::shared_ptr<Propagator> _prop
Our propagator!
Eigen::Vector3d _gravity
Gravity vector.
double _zupt_max_velocity
Max velocity (m/s) that we should consider a zupt with.
double _zupt_noise_multiplier
Multiplier of our IMU noise matrix (default should be 1.0)
double _zupt_max_disparity
Max disparity (pixels) that we should consider a zupt with.
std::map<int, double> chi_squared_table
Chi squared 95th percentile table (lookup would be size of residual)
std::vector<ov_core::ImuData> imu_data
Our history of IMU messages (time, angular, linear)
double last_prop_time_offset
Estimate for time offset at last propagation time.

Function documentation

ov_msckf::UpdaterZeroVelocity::UpdaterZeroVelocity(UpdaterOptions& options, Propagator::NoiseManager& noises, std::shared_ptr<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

void ov_msckf::UpdaterZeroVelocity::feed_imu(const ov_core::ImuData& message)

Feed function for inertial data.

Parameters
message Contains our timestamp and inertial information

bool ov_msckf::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