Initializer for a static visual-inertial system.
This implementation that assumes that the imu starts from standing still.
To initialize from standstill:
- Collect all inertial measurements
- See if within the last window there was a jump in acceleration
- If the jump is past our threshold we should init (i.e. we have started moving)
- Use the previous window, which should have been stationary to initialize orientation
- Return a roll and pitch aligned with gravity and biases.
StaticInitializer:: StaticInitializer(InertialInitializerOptions& params_,
std::shared_ptr<ov_core:: FeatureDatabase> db,
std::shared_ptr<std::vector<ov_core:: ImuData>> imu_data_) explicit
|params_||Parameters loaded from either ROS or CMDLINE|
|db||Feature tracker database with all features in it|
|imu_data_||Shared pointer to our IMU vector of historical information|
StaticInitializer:: 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 using just the imu.
|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 element|
|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).