ov_init::DynamicInitializer class

Initializer for a dynamic visual-inertial system.

Contents

This implementation that will try to recover the initial conditions of the system. Additionally, we will try to recover the covariance of the system. To initialize with arbitrary motion:

  1. Preintegrate our system to get the relative rotation change (biases assumed known)
  2. Construct linear system with features to recover velocity (solve with |g| constraint)
  3. Perform a large MLE with all calibration and recover the covariance.

Method is based on this work (see this tech report for a high level walk through):

Dong-Si, Tue-Cuong, and Anastasios I. Mourikis. "Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration." 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012.

Constructors, destructors, conversion operators

DynamicInitializer(const InertialInitializerOptions& params_, std::shared_ptr<ov_core::FeatureDatabase> db, std::shared_ptr<std::vector<ov_core::ImuData>> imu_data_) explicit
Default constructor.

Public functions

auto initialize(double& timestamp, Eigen::MatrixXd& covariance, std::vector<std::shared_ptr<ov_type::Type>>& order, std::shared_ptr<ov_type::IMU>& _imu, std::map<double, std::shared_ptr<ov_type::PoseJPL>>& _clones_IMU, std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>>& _features_SLAM) -> bool
Try to get the initialized system.

Function documentation

ov_init::DynamicInitializer::DynamicInitializer(const InertialInitializerOptions& params_, std::shared_ptr<ov_core::FeatureDatabase> db, std::shared_ptr<std::vector<ov_core::ImuData>> imu_data_) explicit

Default constructor.

Parameters
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

bool ov_init::DynamicInitializer::initialize(double& timestamp, Eigen::MatrixXd& covariance, std::vector<std::shared_ptr<ov_type::Type>>& order, std::shared_ptr<ov_type::IMU>& _imu, std::map<double, std::shared_ptr<ov_type::PoseJPL>>& _clones_IMU, std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>>& _features_SLAM)

Try to get the initialized system.

Parameters
timestamp out Timestamp we have initialized the state at (last imu state)
covariance out Calculated covariance of the returned state
order out Order of the covariance matrix
_imu Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba)
_clones_IMU Map between imaging times and clone poses (q_GtoIi, p_IiinG)
_features_SLAM Our current set of SLAM features (3d positions)
Returns True if we have successfully initialized our system