From d0d71dedc56fc73a87177b5eef2f1292af8e5f2d Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Thu, 9 Sep 2021 13:13:53 +0200
Subject: [PATCH] work
---
test/gtest_imu_static_init.cpp | 17 +++++++++--------
1 file changed, 9 insertions(+), 8 deletions(-)
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 32566857f..7145d9b23 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;
--
GitLab