diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 6e63ec0bba182b5b0e163ed3c7ff094896f4a296..91f648ac1dffac1ea777fce3be71ab98eec7a7d4 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -1539,10 +1539,32 @@ TEST_F(Process_Factor_Imu_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F } +TEST_F(Process_Factor_Imu, bootstrap) +{ + processor_imu->setVotingActive(true); + processor_imu->setMaxBuffLength(20); + + auto KF0 = problem->emplaceFrame(0.0); + problem->keyFrameCallback(KF0,nullptr); + problem->print(4, 0, 1, 0); + + // Vector6d data(0,0,9.806, 0,0,0); // gravity on Z + Vector6d data(0,9.806,0, 0,0,0); // gravity on Y + + capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity()); + + for (t = 0.0; t < 1.0; t += dt) + { + capture_imu->setTimeStamp(t); + capture_imu->process(); + problem->print(4, 0, 1, 0); + } +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.*"; + // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu.bootstrap"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.*"; // ::testing::GTEST_FLAG(filter) = "Process_Factor_Imu_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";