ov_init::InertialInitializer class

Initializer for visual-inertial system.

Contents

This will try to do both dynamic and state initialization of the state. The user can request to wait for a jump in our IMU readings (i.e. device is picked up) or to initialize as soon as possible. For state initialization, the user needs to specify the calibration beforehand, otherwise dynamic is always used. The logic is as follows:

  1. Try to perform dynamic initialization of state elements.
  2. If this fails and we have calibration then we can try to do static initialization
  3. If the unit is stationary and we are waiting for a jerk, just return, otherwise initialize the state!

The dynamic system is based on an implementation and extension of the work Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration [11] which solves the initialization problem by first creating a linear system for recovering the camera to IMU rotation, then for velocity, gravity, and feature positions, and finally a full optimization to allow for covariance recovery. Another paper which might be of interest to the reader is An Analytical Solution to the IMU Initialization Problem for Visual-Inertial Systems which has some detailed experiments on scale recovery and the accelerometer bias.

Constructors, destructors, conversion operators

InertialInitializer(InertialInitializerOptions& params_, std::shared_ptr<ov_core::FeatureDatabase> db) explicit
Default constructor.

Public functions

void feed_imu(const ov_core::ImuData& message, double oldest_time = -1)
Feed function for inertial data.
auto initialize(double& timestamp, Eigen::MatrixXd& covariance, std::vector<std::shared_ptr<ov_type::Type>>& order, std::shared_ptr<ov_type::IMU> t_imu, bool wait_for_jerk = true) -> bool
Try to get the initialized system.

Protected variables

InertialInitializerOptions params
Initialization parameters.
std::shared_ptr<ov_core::FeatureDatabase> _db
Feature tracker database with all features in it.
std::shared_ptr<std::vector<ov_core::ImuData>> imu_data
Our history of IMU messages (time, angular, linear)
std::shared_ptr<StaticInitializer> init_static
Static initialization helper class.
std::shared_ptr<DynamicInitializer> init_dynamic
Dynamic initialization helper class.

Function documentation

ov_init::InertialInitializer::InertialInitializer(InertialInitializerOptions& params_, std::shared_ptr<ov_core::FeatureDatabase> db) explicit

Default constructor.

Parameters
params_ Parameters loaded from either ROS or CMDLINE
db Feature tracker database with all features in it

void ov_init::InertialInitializer::feed_imu(const ov_core::ImuData& message, double oldest_time = -1)

Feed function for inertial data.

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

bool ov_init::InertialInitializer::initialize(double& timestamp, Eigen::MatrixXd& covariance, std::vector<std::shared_ptr<ov_type::Type>>& order, std::shared_ptr<ov_type::IMU> t_imu, bool wait_for_jerk = true)

Try to get the initialized system.

Parameters
timestamp out Timestamp we have initialized the state at
covariance out Calculated covariance of the returned state
order out Order of the covariance matrix
t_imu out Our imu type (need to have correct ids)
wait_for_jerk If true we will wait for a "jerk"
Returns True if we have successfully initialized our system