From 4d6b05ec2bbfbddd16c555a2202d33544e8db10f Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Fri, 17 May 2024 12:05:30 +0200
Subject: [PATCH] reorder gtest bootstrap
---
test/gtest_imu.cpp | 70 ++++++++++++++++++++++++----------------------
1 file changed, 36 insertions(+), 34 deletions(-)
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index a7eb2f0c7..5172314dc 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);
--
GitLab