diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp index 6aafb68474cf2c95f12544b18f50bc0e809b284d..7437f2df480b4834adc33c6942db41e5e5d54ddc 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 51ee79d6ee34e460d02c6022a016828d1bdfd627..97eb0687a1b946a74876a285b4bb63425c582068 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();