From 843942bf4436137b4e32756e2d07e46df5040e5d Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Fri, 19 Jul 2019 23:22:38 +0200 Subject: [PATCH] small fix --- src/vi_estimator/keypoint_vio.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 5848ac4..cba51a4 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -112,9 +112,10 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, OpticalFlowResult::Ptr prev_frame, curr_frame; IntegratedImuMeasurement::Ptr meas; - Eigen::Vector3d accel_cov, gyro_cov; - accel_cov = calib.dicreete_time_accel_noise_std().array().square(); - gyro_cov = calib.dicreete_time_gyro_noise_std().array().square(); + const Eigen::Vector3d accel_cov = + calib.dicreete_time_accel_noise_std().array().square(); + const Eigen::Vector3d gyro_cov = + calib.dicreete_time_gyro_noise_std().array().square(); ImuData::Ptr data; imu_data_queue.pop(data); @@ -167,8 +168,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, } while (data->t_ns <= curr_frame->t_ns) { - meas->integrate(*data, calib.dicreete_time_accel_noise_std(), - calib.dicreete_time_gyro_noise_std()); + meas->integrate(*data, accel_cov, gyro_cov); imu_data_queue.pop(data); if (!data.get()) break; }