ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added skipping IMU measurements before the first image.

This commit is contained in:
Vladyslav Usenko 2019-08-09 19:34:11 +02:00
parent 5e3c21922b
commit f6a33469f5
1 changed files with 8 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/vi_estimator/keypoint_vio.cpp
View File

@ -138,6 +138,14 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
// curr_frame->t_ns += calib.cam_time_offset_ns; // curr_frame->t_ns += calib.cam_time_offset_ns;
if (!initialized) { if (!initialized) {
while (data->t_ns < curr_frame->t_ns) {
imu_data_queue.pop(data);
if (!data.get()) break;
data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
std::cout << "Skipping IMU data.." << std::endl;
}
Eigen::Vector3d vel_w_i_init; Eigen::Vector3d vel_w_i_init;
vel_w_i_init.setZero(); vel_w_i_init.setZero();