Initializer for visual-inertial system.
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:
- Try to perform dynamic initialization of state elements.
- If this fails and we have calibration then we can try to do static initialization
- 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  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.
void feed_imu(const ov_core::
ImuData& message, double oldest_time = -1)
- Feed function for inertial data.
auto initialize(double& timestamp,
Type>>& order, std::shared_ptr<ov_type:: IMU> t_imu, bool wait_for_jerk = true) -> bool
- Try to get the initialized system.
- InertialInitializerOptions params
- Initialization parameters.
- Feature tracker database with all features in it.
- 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.
InertialInitializer:: initialize(double& timestamp,
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.
|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|