Skip to content
Snippets Groups Projects
Commit 137dc06f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[hotfix] fixed gtests for imu2d

parent 6fa76f2b
No related branches found
No related tags found
1 merge request!54devel->main
Hide whitespace changes
Inline Side-by-side
test/gtest_factor_imu2d.cpp
+ 9
− 5
View file @ 137dc06f
......@@ -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();
......
test/gtest_factor_imu2d_with_gravity.cpp
+ 8
− 4
View file @ 137dc06f
......@@ -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();
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment