From 137dc06fb68f02fbb688c2a78719f4d2367c0af3 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 10 Oct 2022 13:12:55 +0200
Subject: [PATCH] [hotfix] fixed gtests for imu2d
---
test/gtest_factor_imu2d.cpp | 14 +++++++++-----
test/gtest_factor_imu2d_with_gravity.cpp | 12 ++++++++----
2 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 6aafb6847..7437f2df4 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -113,7 +113,8 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f1)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
@@ -153,7 +154,8 @@ TEST_F(FactorImu2d_test, bias_zero_solve_f0)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
@@ -244,7 +246,7 @@ TEST_F(FactorImu2d_test, solve_b0)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
@@ -292,7 +294,8 @@ TEST_F(FactorImu2d_test, solve_f0)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
@@ -340,7 +343,8 @@ TEST_F(FactorImu2d_test, solve_f1)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 51ee79d6e..97eb0687a 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -125,7 +125,8 @@ TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f1)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
// std::cout << report << std::endl;
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
// WOLF_INFO(frm1->getStateVector());
// remove feature (and factor) for the next loop
@@ -172,7 +173,8 @@ TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f0)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
// WOLF_INFO(frm1->getStateVector());
// remove feature (and factor) for the next loop
@@ -331,7 +333,8 @@ TEST_F(FactorImu2dWithGravity_test, solve_f0)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getStateVector("V"), x0.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
@@ -385,7 +388,8 @@ TEST_F(FactorImu2dWithGravity_test, solve_f1)
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
// remove feature (and factor) for the next loop
fea1->remove();
--
GitLab