diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index daa57316c6d4fd986f15c5351957a3c9a407cd08..0bda304c2123fbb95f72e6572b7b9a7e5bc80e06 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -211,7 +211,7 @@ FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion) auto feature = FeatureBase::emplace<FeatureIMU>(_capture_motion, _capture_motion->getBuffer().get().back().delta_integr_, _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_, - _capture_motion->getBuffer().getCalibrationPreint(), + _capture_motion->getCalibrationPreint(), _capture_motion->getBuffer().get().back().jacobian_calib_); return feature; } diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp index 424fee3e2315d1513b3fd7be381a16b45f0dca60..f6044cf0037b1d0fa3644fab899cda2aea354f2d 100644 --- a/test/gtest_IMU.cpp +++ b/test/gtest_IMU.cpp @@ -237,7 +237,7 @@ class Process_Factor_IMU : public testing::Test */ static MotionBuffer integrateDeltaTrajectory(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b) { - MotionBuffer trajectory(6, 10, 9, 6); + MotionBuffer trajectory(6, 10, 9); VectorXs Delta(10); MatrixXs M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1); Quaternions q; diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp index 1c6b38afc03f49a6880f6a58e0d788278047ad96..f5e9a0c2b4b987cad4373bbb6e48de0c4149b284 100644 --- a/test/gtest_feature_IMU.cpp +++ b/test/gtest_feature_IMU.cpp @@ -87,7 +87,7 @@ class FeatureIMU_test : public testing::Test //emplace a feature delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_; delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08; - VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint(); + VectorXs calib_preint = problem->getProcessorMotion()->getLast()->getCalibrationPreint(); dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_; feat_imu = std::static_pointer_cast<FeatureIMU>( FeatureBase::emplace<FeatureIMU>(imu_ptr,