diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp index 6c68bc59b1882c966273bed07e57fd0b2ad3f9c0..cfb7a446d358ed9668b5433d27625306da4c083a 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();