Skip to content
Snippets Groups Projects
Commit d0d71ded authored by Idril-Tadzio Geer Cousté's avatar Idril-Tadzio Geer Cousté
Browse files

work

parent c3a5bbf7
No related branches found
No related tags found
4 merge requests!39release after RAL,!38After 2nd RAL submission,!33Imu2d static initialization,!31Imu2d static initialization
...@@ -25,9 +25,10 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -25,9 +25,10 @@ class ProcessorImuStaticInitTest : public testing::Test
bool second_frame; bool second_frame;
Vector6d data; Vector6d data;
Matrix6d data_cov; Matrix6d data_cov;
VectorComposite x0c; // initial state composite //VectorComposite x0c; // initial state composite
VectorComposite s0c; // initial sigmas composite VectorComposite s0c; // initial sigmas composite
std::shared_ptr<wolf::CaptureImu> C; std::shared_ptr<wolf::CaptureImu> C;
FrameBasePtr KF0;
FrameBasePtr _first_frame; FrameBasePtr _first_frame;
FrameBasePtr _last_frame; FrameBasePtr _last_frame;
SolverCeresPtr _solver; SolverCeresPtr _solver;
...@@ -65,6 +66,13 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -65,6 +66,13 @@ class ProcessorImuStaticInitTest : public testing::Test
// Create the solver // Create the solver
_solver = make_shared<SolverCeres>(_problem_ptr); _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 ...@@ -73,10 +81,6 @@ class ProcessorImuStaticInitTest : public testing::Test
TEST_F(ProcessorImuStaticInitTest, static) TEST_F(ProcessorImuStaticInitTest, static)
{ {
WOLF_INFO("Starting Test"); 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_cov *= 1e-3;
data << 1, 2, 3, 4, 5, 6; data << 1, 2, 3, 4, 5, 6;
...@@ -86,9 +90,6 @@ TEST_F(ProcessorImuStaticInitTest, static) ...@@ -86,9 +90,6 @@ TEST_F(ProcessorImuStaticInitTest, static)
//dt = 0.0001; //dt = 0.0001;
auto KF0 = _problem_ptr->setPriorFix(x0c, t0, dt/2);
KF0->fix();
_processor_motion->setOrigin(KF0);
WOLF_INFO("Data is: \n", data); WOLF_INFO("Data is: \n", data);
int size = 7; int size = 7;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment