From 484516bee3d0111a47fd9adc2054d25200add9b5 Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Thu, 23 Sep 2021 13:28:22 +0200
Subject: [PATCH] added TODOs to try and fix the problem
---
test/gtest_imu_static_init.cpp | 3 +++
1 file changed, 3 insertions(+)
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 6c68bc59b..cfb7a446d 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -81,6 +81,7 @@ TEST_F(ProcessorImuStaticInitTest, static)
data_cov *= 1e-3;
data << 1, 2, 3, 4, 5, 6;
Vector6d bias_groundtruth;
+ //TODO: Fix this, it works because initial position and extrinsics are 0, but gravity has to be preceded by the quaternion that takes it to the IMU reference
bias_groundtruth.head(3) = data.head(3) -wolf::gravity();
bias_groundtruth.tail(3) = data.tail(3);
@@ -123,6 +124,8 @@ TEST_F(ProcessorImuStaticInitTest, static)
n_frames++;
last_frame_ = processor_motion_->getOrigin()->getFrame();
+ // TODO: We have to impose that the position is the same as for the first frame and fix it, and do the same for the orientation
+
// impose zero velocity
last_frame_->getV()->setZero();
last_frame_->getV()->fix();
--
GitLab