ov_init::InitializerHelper class

Has a bunch of helper functions for dynamic initialization (should all be static)

Contents

Public static functions

static auto interpolate_data(const ov_core::ImuData& imu_1, const ov_core::ImuData& imu_2, double timestamp) -> ov_core::ImuData
Nice helper function that will linearly interpolate between two imu messages.
static auto select_imu_readings(const std::vector<ov_core::ImuData>& imu_data_tmp, double time0, double time1) -> std::vector<ov_core::ImuData>
Helper function that given current imu data, will select imu readings between the two times.
static void gram_schmidt(const Eigen::Vector3d& gravity_inI, Eigen::Matrix3d& R_GtoI)
Given a gravity vector, compute the rotation from the inertial reference frame to this vector.
static auto compute_dongsi_coeff(Eigen::MatrixXd& D, const Eigen::MatrixXd& d, double gravity_mag) -> Eigen::VectorXd
Compute coefficients for the constrained optimization quadratic problem.

Function documentation

static ov_core::ImuData ov_init::InitializerHelper::interpolate_data(const ov_core::ImuData& imu_1, const ov_core::ImuData& imu_2, double timestamp)

Nice helper function that will linearly interpolate between two imu messages.

Parameters
imu_1 imu at begining of interpolation interval
imu_2 imu at end of interpolation interval
timestamp Timestamp being interpolated to

This should be used instead of just "cutting" imu messages that bound the camera times Give better time offset if we use this function, could try other orders/splines if the imu is slow.

static std::vector<ov_core::ImuData> ov_init::InitializerHelper::select_imu_readings(const std::vector<ov_core::ImuData>& imu_data_tmp, double time0, double time1)

Helper function that given current imu data, will select imu readings between the two times.

Parameters
imu_data_tmp IMU data we will select measurements from
time0 Start timestamp
time1 End timestamp
Returns Vector of measurements (if we could compute them)

This will create measurements that we will integrate with, and an extra measurement at the end. We use the interpolate_data() function to "cut" the imu readings at the beginning and end of the integration. The timestamps passed should already take into account the time offset values.

static void ov_init::InitializerHelper::gram_schmidt(const Eigen::Vector3d& gravity_inI, Eigen::Matrix3d& R_GtoI)

Given a gravity vector, compute the rotation from the inertial reference frame to this vector.

Parameters
gravity_inI Gravity in our sensor frame
R_GtoI Rotation from the arbitrary inertial reference frame to this gravity vector

The key assumption here is that our gravity is along the vertical direction in the inertial frame. We can take this vector (z_in_G=0,0,1) and find two arbitrary tangent directions to it. https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process

static Eigen::VectorXd ov_init::InitializerHelper::compute_dongsi_coeff(Eigen::MatrixXd& D, const Eigen::MatrixXd& d, double gravity_mag)

Compute coefficients for the constrained optimization quadratic problem.

Parameters
D Gravity in our sensor frame
d Rotation from the arbitrary inertial reference frame to this gravity vector
gravity_mag Scalar size of gravity (normally is 9.81)
Returns Coefficents from highest to the constant

https://gist.github.com/goldbattle/3791cbb11bbf4f5feb3f049dad72bfdd