diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp index 2102973688a41db82a0f3ba0b816108e047b58ef..4dbac962133c7d8da9f76e152a09e0749c1f2e71 100644 --- a/test/gtest_processor_motion_intrinsics_update.cpp +++ b/test/gtest_processor_motion_intrinsics_update.cpp @@ -1,6 +1,8 @@ #include <fstream> +#include "imu/internal/config.h" #include "core/utils/utils_gtest.h" +#include "core/utils/logging.h" #include "core/common/wolf.h" #include "core/problem/problem.h" @@ -227,6 +229,28 @@ TEST_F(ProcessorImuTest, getState) #if WRITE_CSV_FILE file_est.close(); #endif + + /** assert final results + * + * t 0 .9 1 1.9 2 3 + * BIAS .4 .4 0 0 0 0 + * BIAS PREINT .4 .4 .4 .4 0 0 + * X 0 !0 0 0 0 0 + * V 0 !0 0 0 0 0 + * + * though we only test at KFs so t = { 0, 1, 2, 3 } + */ + + for (auto F : *problem_->getTrajectory() ) + { + auto ts = F->getTimeStamp(); + + double bias = (t == 0 ? 0.42 : 0.0); + ASSERT_NEAR(F->getCaptureOf(sensor_)->getSensorIntrinsic()->getState()(0), bias, 1e-6); + ASSERT_NEAR(F->getP()->getState()(0),0.0,1e-6); + ASSERT_NEAR(F->getV()->getState()(0),0.0,1e-6); + } + }