ov_core::InertialInitializer class

Initializer for visual-inertial system.

This class has a series of functions that can be used to initialize your system. Right now we have our implementation that assumes that the imu starts from standing still. In the future we plan to add support for structure-from-motion dynamic initialization.

To initialize from standstill:

  1. Collect all inertial measurements
  2. See if within the last window there was a jump in acceleration
  3. If the jump is past our threshold we should init (i.e. we have started moving)
  4. Use the previous window, which should have been stationary to initialize orientation
  5. Return a roll and pitch aligned with gravity and biases.

Public types

struct IMUDATA
Struct for a single imu measurement (time, wm, am)

Constructors, destructors, conversion operators

InertialInitializer(Eigen::Matrix<double, 3, 1> gravity, double window_length, double imu_excite_threshold)
Default constructor.

Public functions

void feed_imu(double timestamp, Eigen::Matrix<double, 3, 1> wm, Eigen::Matrix<double, 3, 1> am)
Stores incoming inertial readings.
auto initialize_with_imu(double& time0, Eigen::Matrix<double, 4, 1>& q_GtoI0, Eigen::Matrix<double, 3, 1>& b_w0, Eigen::Matrix<double, 3, 1>& v_I0inG, Eigen::Matrix<double, 3, 1>& b_a0, Eigen::Matrix<double, 3, 1>& p_I0inG, bool wait_for_jerk = true) -> bool
Try to initialize the system using just the imu.

Protected variables

Eigen::Matrix<double, 3, 1> _gravity
Gravity vector.
double _window_length
Amount of time we will initialize over (seconds)
double _imu_excite_threshold
Variance threshold on our acceleration to be classified as moving.
std::vector<IMUDATA> imu_data
Our history of IMU messages (time, angular, linear)

Function documentation

ov_core::InertialInitializer::InertialInitializer(Eigen::Matrix<double, 3, 1> gravity, double window_length, double imu_excite_threshold)

Default constructor.

Parameters
gravity Gravity in the global frame of reference
window_length Amount of time we will initialize over (seconds)
imu_excite_threshold Variance threshold on our acceleration to be classified as moving

void ov_core::InertialInitializer::feed_imu(double timestamp, Eigen::Matrix<double, 3, 1> wm, Eigen::Matrix<double, 3, 1> am)

Stores incoming inertial readings.

Parameters
timestamp Timestamp of imu reading
wm Gyro angular velocity reading
am Accelerometer linear acceleration reading

bool ov_core::InertialInitializer::initialize_with_imu(double& time0, Eigen::Matrix<double, 4, 1>& q_GtoI0, Eigen::Matrix<double, 3, 1>& b_w0, Eigen::Matrix<double, 3, 1>& v_I0inG, Eigen::Matrix<double, 3, 1>& b_a0, Eigen::Matrix<double, 3, 1>& p_I0inG, bool wait_for_jerk = true)

Try to initialize the system using just the imu.

Parameters
time0 out Timestamp that the returned state is at
q_GtoI0 out Orientation at initialization
b_w0 out Gyro bias at initialization
v_I0inG out Velocity at initialization
b_a0 out Acceleration bias at initialization
p_I0inG out Position at initialization
wait_for_jerk If true we will wait for a "jerk"
Returns True if we have successfully initialized our system

This will check if we have had a large enough jump in our acceleration. If we have then we will use the period of time before this jump to initialize the state. This assumes that our imu is sitting still and is not moving (so this would fail if we are experiencing constant acceleration).

In the case that we do not wait for a jump (i.e. wait_for_jerk is false), then the system will try to initialize as soon as possible. This is only recommended if you have zero velocity update enabled to handle the stationary cases. To initialize in this case, we need to have the average angular variance be below the set threshold (i.e. we need to be stationary).