ov_init::DynamicInitializer class

Initializer for a dynamic visual-inertial system.

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:

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, std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>>& _calib_IMUtoCAM, std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>>& _cam_intrinsics) -> 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, std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>>& _calib_IMUtoCAM, std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>>& _cam_intrinsics)

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)
_calib_IMUtoCAM Calibration poses for each camera (R_ItoC, p_IinC)
_cam_intrinsics Camera intrinsics
Returns True if we have successfully initialized our system