diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index a7eb2f0c7def414fbf273eb4b44e56cfa1a99e77..5172314dcccf59169cad1e2714cb1ed2b2884d0c 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -1051,6 +1051,42 @@ TEST_F(Process_Factor_Imu, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed__ } +TEST_F(Process_Factor_Imu, bootstrap) +{ + processor_imu->setVotingActive(true); + processor_imu->setMaxTimeSpan(0.04); + processor_imu->bootstrapEnable(true); + + 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.0, 9.806, 0.0, 0.0, 0.0, 0.0); // gravity on Y + Vector6d data; + data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0 ; // gravity on Y + + capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity()); + + for (t = 0; t < 0.14; t += dt) + { + capture_imu->setTimeStamp(t); + capture_imu->process(); + } + + problem->print(4, 0, 1, 1); + + Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X + for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap()) + { + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10); + ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10); + } +} + + + TEST_F(Process_Factor_Imu_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated { @@ -1538,40 +1574,6 @@ 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->setMaxTimeSpan(0.04); - processor_imu->bootstrapEnable(true); - - 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.0, 9.806, 0.0, 0.0, 0.0, 0.0); // gravity on Y - Vector6d data; - data << 0.0, 9.806, 0.0, 0.0, 0.0, 0.0 ; // gravity on Y - - capture_imu = std::make_shared<CaptureImu>(0.0, sensor_imu, data, Matrix6d::Identity()); - - for (t = 0; t < 0.14; t += dt) - { - capture_imu->setTimeStamp(t); - capture_imu->process(); - } - - problem->print(4, 0, 1, 1); - - Quaterniond qref(AngleAxisd(M_PI / 2, Vector3d::UnitX())); // turn of +90deg over X - for (auto pair_ts_frame : problem->getTrajectory()->getFrameMap()) - { - ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("P"), Vector3d::Zero(), 1e-10); - ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("O"), qref.coeffs(), 1e-10); - ASSERT_MATRIX_APPROX(pair_ts_frame.second->getStateVector("V"), Vector3d::Zero(), 1e-10); - } -} - int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);