diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp index 32566857f6f38058ac09d70c1d6e4b353fcfe090..7145d9b23c3478e0ee085f2c9db29c2e3a1bc172 100644 --- a/test/gtest_imu_static_init.cpp +++ b/test/gtest_imu_static_init.cpp @@ -25,9 +25,10 @@ class ProcessorImuStaticInitTest : public testing::Test bool second_frame; Vector6d data; Matrix6d data_cov; - VectorComposite x0c; // initial state composite + //VectorComposite x0c; // initial state composite VectorComposite s0c; // initial sigmas composite std::shared_ptr<wolf::CaptureImu> C; + FrameBasePtr KF0; FrameBasePtr _first_frame; FrameBasePtr _last_frame; SolverCeresPtr _solver; @@ -65,6 +66,13 @@ class ProcessorImuStaticInitTest : public testing::Test // Create the solver _solver = make_shared<SolverCeres>(_problem_ptr); + + // Set the origin + VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()}); + WOLF_INFO("x0c is: \n", x0c); + KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2); + KF0->fix(); + _processor_motion->setOrigin(KF0); } }; @@ -73,10 +81,6 @@ class ProcessorImuStaticInitTest : public testing::Test TEST_F(ProcessorImuStaticInitTest, static) { WOLF_INFO("Starting Test"); - // Set the origin - x0c['P'] = Vector3d(0, 0, 0); - x0c['O'] = Vector4d(0, 0, 0, 1); - x0c['V'] = Vector3d(0, 0, 0); data_cov *= 1e-3; data << 1, 2, 3, 4, 5, 6; @@ -86,9 +90,6 @@ TEST_F(ProcessorImuStaticInitTest, static) //dt = 0.0001; - auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2); - KF0->fix(); - _processor_motion->setOrigin(KF0); WOLF_INFO("Data is: \n", data); int size = 7;