From f80fb4dd01cff7080ae32c7a691478504231127b Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Thu, 9 Jun 2022 13:55:17 +0200
Subject: [PATCH] [skip ci] new asserts for gtest
---
include/core/utils/utils_gtest.h | 47 ++++++++++++++++---
test/gtest_SE3.cpp | 36 +++++++-------
test/gtest_factor_absolute.cpp | 2 +-
test/gtest_factor_diff_drive.cpp | 12 ++---
test/gtest_factor_pose_2d.cpp | 2 +-
test/gtest_factor_pose_3d.cpp | 3 +-
test/gtest_factor_relative_pose_3d.cpp | 6 +--
...actor_relative_pose_3d_with_extrinsics.cpp | 26 +++++-----
test/gtest_frame_base.cpp | 4 +-
test/gtest_local_param.cpp | 4 +-
test/gtest_param_prior.cpp | 4 +-
test/gtest_problem.cpp | 17 +++----
...sor_and_factor_pose_3d_with_extrinsics.cpp | 18 +++----
test/gtest_processor_diff_drive.cpp | 18 +++----
test/gtest_processor_motion.cpp | 12 ++---
test/gtest_processor_odom_3d.cpp | 4 +-
test/gtest_state_composite.cpp | 2 +-
..._tree_manager_sliding_window_dual_rate.cpp | 4 +-
18 files changed, 124 insertions(+), 97 deletions(-)
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index b89b9102a..85aff3e6a 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -144,24 +144,59 @@ TEST(Test, Foo)
}, \
C_expect, C_actual);
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
+ return lhs.angularDistance(rhs) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
+ return lhs.angularDistance(rhs) < precision; \
+ }, \
+ C_expect, C_actual);
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
+#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
+ }, \
+ C_expect, C_actual);
-#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define EXPECT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
- MatrixXd er = lhs - rhs; \
+#define ASSERT_POSE2d_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ if (lhs.size() != 3 or rhs.size() != 3){ return false;} \
+ Eigen::VectorXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
+#define EXPECT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
+#define ASSERT_POSE3d_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ assert(lhs.size() == 7 and rhs.size() == 7); \
+ Eigen::Vector4d er; \
+ er.head(3) = lhs.head(3) - rhs.head(3); \
+ er(3) = Eigen::Quaterniond(Eigen::Vector4d(lhs.tail(4))).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs.tail(4)))); \
+ return er.isMuchSmallerThan(1, precision); \
+ }, \
+ C_expect, C_actual);
+
} // namespace testing
#endif /* WOLF_UTILS_GTEST_H */
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index be07e9804..ff1ddb6d1 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -41,7 +41,7 @@ TEST(SE3, exp_0)
Vector6d tau = Vector6d::Zero();
Vector7d pose; pose << 0,0,0, 0,0,0,1;
- ASSERT_MATRIX_APPROX(pose, exp(tau), 1e-8);
+ ASSERT_POSE3d_APPROX(pose, exp(tau), 1e-8);
}
TEST(SE3, log_I)
@@ -58,7 +58,7 @@ TEST(SE3, exp_log)
Vector7d pose = exp(tau);
ASSERT_MATRIX_APPROX(tau, log(exp(tau)), 1e-8);
- ASSERT_MATRIX_APPROX(pose, exp(log(pose)), 1e-8);
+ ASSERT_POSE3d_APPROX(pose, exp(log(pose)), 1e-8);
}
TEST(SE3, exp_of_multiple)
@@ -73,13 +73,13 @@ TEST(SE3, exp_of_multiple)
pose3 = compose(pose2, pose);
// 1
- ASSERT_MATRIX_APPROX(exp(tau), pose, 1e-8);
+ ASSERT_POSE3d_APPROX(exp(tau), pose, 1e-8);
// 2
- ASSERT_MATRIX_APPROX(exp(tau2), pose2, 1e-8);
+ ASSERT_POSE3d_APPROX(exp(tau2), pose2, 1e-8);
// 3
- ASSERT_MATRIX_APPROX(exp(tau3), pose3, 1e-8);
+ ASSERT_POSE3d_APPROX(exp(tau3), pose3, 1e-8);
}
TEST(SE3, log_of_power)
@@ -151,7 +151,7 @@ TEST(SE3, composeEigenVectors)
compose(x1, x2, xc);
- ASSERT_MATRIX_APPROX(xc, xc_true, 1e-8);
+ ASSERT_POSE3d_APPROX(xc, xc_true, 1e-8);
}
TEST(SE3, composeVectorComposite)
@@ -175,7 +175,7 @@ TEST(SE3, composeVectorComposite)
compose(x1, x2, xc);
ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
- ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+ ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
}
TEST(SE3, composeVectorComposite_Jacobians)
@@ -204,7 +204,7 @@ TEST(SE3, composeVectorComposite_Jacobians)
compose(x1, x2, xc, J_xc_x1, J_xc_x2);
ASSERT_MATRIX_APPROX(xc.at('P'), pc, 1e-8);
- ASSERT_MATRIX_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
+ ASSERT_QUATERNION_VECTOR_APPROX(xc.at('O'), qc.coeffs(), 1e-8);
ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'P'), Matrix3d::Identity() , 1e-8);
ASSERT_MATRIX_APPROX(J_xc_x1.at('P', 'O'), - R1 * skew(p2) , 1e-8);
@@ -232,7 +232,7 @@ TEST(SE3, exp_0_Composite)
VectorComposite x = SE3::exp(tau);
ASSERT_MATRIX_APPROX(x.at('P'), Vector3d::Zero(), 1e-8);
- ASSERT_MATRIX_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
+ ASSERT_QUATERNION_VECTOR_APPROX(x.at('O'), Quaterniond::Identity().coeffs(), 1e-8);
}
@@ -257,7 +257,7 @@ TEST(SE3, plus_0_Composite)
VectorComposite res = plus(x, tau);
ASSERT_MATRIX_APPROX(res.at('P'), p, 1e-8);
- ASSERT_MATRIX_APPROX(res.at('O'), q, 1e-8);
+ ASSERT_QUATERNION_VECTOR_APPROX(res.at('O'), q, 1e-8);
}
TEST(SE3, interpolate_I_xyz)
@@ -272,7 +272,7 @@ TEST(SE3, interpolate_I_xyz)
Vector7d p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_xyz_xyz)
@@ -287,7 +287,7 @@ TEST(SE3, interpolate_xyz_xyz)
Vector7d p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qx)
@@ -302,7 +302,7 @@ TEST(SE3, interpolate_I_qx)
Vector7d p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qy)
@@ -317,7 +317,7 @@ TEST(SE3, interpolate_I_qy)
Vector7d p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qz)
@@ -332,7 +332,7 @@ TEST(SE3, interpolate_I_qz)
Vector7d p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qxyz)
@@ -350,7 +350,7 @@ TEST(SE3, interpolate_I_qxyz)
interpolate(p1, p2, t, p_pre);
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_half)
@@ -371,7 +371,7 @@ TEST(SE3, interpolate_half)
double t = 0.5;
interpolate(p1, p2, t, p_pre);
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_third)
@@ -394,7 +394,7 @@ TEST(SE3, interpolate_third)
double t = 1.0/3.0;
interpolate(p1, p2, t, p_pre);
- ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
+ ASSERT_POSE3d_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, toSE3_I)
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 4d7137a20..7019025dc 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -135,7 +135,7 @@ TEST(FactorQuatAbs, fac_block_abs_o)
std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 0d03e872d..2cca034dd 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -370,7 +370,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
// WOLF_TRACE("\n", report);
problem->print(1,0,1,1);
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), delta, 1e-6);
+ ASSERT_POSE2d_APPROX(F1->getStateVector("PO"), delta, 1e-6);
}
@@ -549,11 +549,10 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
WOLF_TRACE("calib_gt : ", calib_gt.transpose());
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState() , calib_gt , 1e-6 );
- ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
+ ASSERT_POSE2d_APPROX(problem->getState( N*dt).vector("PO") , x1 , 1e-6 );
+ ASSERT_POSE2d_APPROX(problem->getState(2*N*dt).vector("PO") , x2 , 1e-6 );
cout << "\n\n" << endl;
-
}
TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
@@ -687,9 +686,8 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
ASSERT_MATRIX_APPROX(sensor->getIntrinsic()->getState(), calib_gt, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState( N*dt).vector("PO"), x1, 1e-2);
- ASSERT_MATRIX_APPROX(problem->getState(2*N*dt).vector("PO"), x2, 1e-6);
-
+ ASSERT_POSE2d_APPROX(problem->getState( N*dt).vector("PO"), x1, 1e-2);
+ ASSERT_POSE2d_APPROX(problem->getState(2*N*dt).vector("PO"), x2, 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index a352ae12e..f818f97ee 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.cpp
@@ -78,7 +78,7 @@ TEST(FactorPose2d, solve)
// solve for frm0
std::string report = solver.solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), pose, 1e-6);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), pose, 1e-6);
}
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index c3548e02d..718ca9809 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.cpp
@@ -85,8 +85,7 @@ TEST(FactorPose3d, solve)
// solve for frm0
std::string brief_report = solver.solve(SolverManager::ReportVerbosity::FULL);
- ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), pose7, 1e-6);
-
+ ASSERT_POSE3d_APPROX(frm0->getStateVector("PO"), pose7, 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 0588ab5b0..524255e52 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -75,7 +75,7 @@ TEST(FactorRelativePose3d, check_tree)
TEST(FactorRelativePose3d, expectation)
{
- ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
+ ASSERT_POSE3d_APPROX(ctr1->expectation() , delta, 1e-6);
}
TEST(FactorRelativePose3d, fix_0_solve)
@@ -89,7 +89,7 @@ TEST(FactorRelativePose3d, fix_0_solve)
// solve for frm1
std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("PO"), delta, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), delta, 1e-6);
}
@@ -103,7 +103,7 @@ TEST(FactorRelativePose3d, fix_1_solve)
// solve for frm0
std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(frm0->getStateVector("PO"), problem_ptr->stateZero().vector("PO"), 1e-6);
+ ASSERT_POSE3d_APPROX(frm0->getStateVector("PO"), problem_ptr->stateZero().vector("PO"), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index 3c35668b0..e1a328927 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -180,7 +180,7 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
F1->getP()->perturb();
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
}
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
@@ -196,7 +196,7 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
F1->getO()->perturb();
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
}
TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
@@ -207,9 +207,9 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
nullptr,
false);
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
}
@@ -226,8 +226,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
lmk1->getP()->perturb();
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
}
TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
@@ -243,8 +243,8 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
lmk1->getO()->perturb();
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
}
@@ -298,16 +298,16 @@ TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
Vector7d t_w_r, t_w_l;
t_w_r << p_w_r, q_w_r.coeffs();
t_w_l << p_w_l, q_w_l.coeffs();
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO"), t_w_r, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO"), t_w_l, 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), t_w_r, 1e-6);
+ ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), t_w_l, 1e-6);
// unfix LMK, perturbate state
lmk1->unfix();
lmk1->perturb();
std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(F1->getStateVector("PO").transpose(), t_w_r.transpose(), 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getStateVector("PO").transpose(), t_w_l.transpose(), 1e-6);
+ ASSERT_POSE3d_APPROX(F1->getStateVector("PO").transpose(), t_w_r.transpose(), 1e-6);
+ ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO").transpose(), t_w_l.transpose(), 1e-6);
}
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 6cab028bb..7d82e0207 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -416,8 +416,8 @@ TEST(FrameBase, GetSetState)
ASSERT_MATRIX_APPROX(v, F.getV()->getState(), Constants::EPS_SMALL);
// get the state, form 2 by return value
- x2 = F.getStateVector("POV");
- ASSERT_MATRIX_APPROX(x2, x, Constants::EPS_SMALL);
+ ASSERT_POSE3d_APPROX(F.getStateVector("PO"), x.head(7), Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(F.getStateVector("V"), x.tail(3), Constants::EPS_SMALL);
}
TEST(FrameBase, Constructor_composite)
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index a709195a5..fcab27747 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -85,7 +85,7 @@ TEST(TestLocalParametrization, QuaternionLocal)
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
Quaterniond qref = Map<Quaterniond>(q.data()) * wolf::v2q(da);
- ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
+ ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS);
Qpar_loc.computeJacobian(q_m,J);
@@ -130,7 +130,7 @@ TEST(TestLocalParametrization, QuaternionGlobal)
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data());
- ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
+ ASSERT_QUATERNION_VECTOR_APPROX(qo_m,qref.coeffs(),Constants::EPS);
Qpar_glob.computeJacobian(q_m,J);
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index a6b2177e1..9c90d2ce4 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -70,13 +70,13 @@ TEST(ParameterPrior, add_prior_o)
{
ASSERT_TRUE(problem_ptr->check(0));
ASSERT_TRUE(sensor_ptr_->getO());
- ASSERT_MATRIX_APPROX(sensor_ptr_->getO()->getState(),initial_extrinsics_o,Constants::EPS);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(),initial_extrinsics_o,Constants::EPS);
sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Matrix3d::Identity());
std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index a5808a865..874682995 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -147,7 +147,7 @@ TEST(Problem, SetOrigin_PO_2d)
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
- ASSERT_MATRIX_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
+ ASSERT_POSE2d_APPROX(x0.vector("PO"), P->getState().vector("PO"), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
@@ -423,8 +423,7 @@ TEST(Problem, perturb)
for (auto S : problem->getHardware()->getSensorList())
{
- ASSERT_MATRIX_APPROX(S->getP()->getState(), Vector2d(0,0), prec );
- ASSERT_MATRIX_APPROX(S->getO()->getState(), Vector1d(0), prec );
+ ASSERT_POSE2d_APPROX(S->getStateVector("PO"), Vector3d(0,0,0), prec );
ASSERT_FALSE(S->getIntrinsic()->getState().isApprox(Vector3d(1,1,1), prec) );
}
@@ -433,13 +432,11 @@ TEST(Problem, perturb)
auto F = pair_T_F.second;
if (F->isFixed()) // fixed
{
- ASSERT_MATRIX_APPROX (F->getP()->getState(), Vector2d(0,0), prec );
- ASSERT_MATRIX_APPROX (F->getO()->getState(), Vector1d(0), prec );
+ ASSERT_POSE2d_APPROX (F->getStateVector("PO"), Vector3d(0,0,0), prec );
}
else
{
- ASSERT_FALSE(F->getP()->getState().isApprox(Vector2d(0,0), prec) );
- ASSERT_FALSE(F->getO()->getState().isApprox(Vector1d(0), prec) );
+ ASSERT_FALSE(F->getStateVector("PO").isApprox(Vector3d(0,0,0), prec) );
}
for (auto C : F->getCaptureList())
{
@@ -451,13 +448,11 @@ TEST(Problem, perturb)
{
if ( L->isFixed() ) // fixed
{
- ASSERT_MATRIX_APPROX (L->getP()->getState(), Vector2d(1,2), prec );
- ASSERT_MATRIX_APPROX (L->getO()->getState(), Vector1d(3), prec );
+ ASSERT_POSE2d_APPROX (L->getStateVector("PO"), Vector3d(1,2,3), prec );
}
else
{
- ASSERT_FALSE(L->getP()->getState().isApprox(Vector2d(1,2), prec) );
- ASSERT_FALSE(L->getO()->getState().isApprox(Vector1d(3), prec) );
+ ASSERT_FALSE(L->getStateVector("PO").isApprox(Vector3d(1,2,3), prec) );
}
}
}
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index d71d2daac..87eb9db92 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -296,10 +296,10 @@ TEST_F(FactorPose3dWithExtrinsicsMANUAL_Test, solve)
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
- ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
@@ -314,10 +314,10 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test, solve
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
- ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
@@ -333,10 +333,10 @@ TEST_F(FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test, solve)
std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL);
SensorPosePtr sp = std::static_pointer_cast<SensorPose>(sensor_pose_);
- ASSERT_MATRIX_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
- ASSERT_MATRIX_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF1_->getStateVector("PO"), pose1_, 1e-6);
+ ASSERT_POSE3d_APPROX(KF2_->getStateVector("PO"), pose2_, 1e-6);
ASSERT_MATRIX_APPROX(sp->getP()->getState(), b_p_bm_, 1e-6);
- ASSERT_MATRIX_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
+ ASSERT_QUATERNION_VECTOR_APPROX(sp->getO()->getState(), b_q_m_.coeffs(), 1e-6);
}
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 11a0663c1..04fa9ddb4 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -224,21 +224,21 @@ TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3d::Zero(), 1e-8);
+ ASSERT_POSE2d_APPROX(delta, Vector3d::Zero(), 1e-8);
// 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
data(0) = 0.25*params_sen->ticks_per_wheel_revolution;
data(1) = 0.50*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*params_sen->ticks_per_wheel_revolution;
data(1) = 0.25*params_sen->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
@@ -262,7 +262,7 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
delta1 = delta2;
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
- ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*params_sen->ticks_per_wheel_revolution / 4;
@@ -278,7 +278,7 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
delta1 = delta2;
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
- ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, statePlusDelta)
@@ -303,7 +303,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(x2.vector("PO"), Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*params_sen->ticks_per_wheel_revolution / 4;
@@ -319,7 +319,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2.vector("PO"), Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_POSE2d_APPROX(x2.vector("PO"), Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, process)
@@ -381,7 +381,7 @@ TEST_F(ProcessorDiffDriveTest, linear)
// radius is 1.0m, 100 ticks per revolution, so advanced distance is
double distance = 2 * M_PI * 1.0;
- ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,0), 1e-6)
+ ASSERT_POSE2d_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,0), 1e-6)
}
TEST_F(ProcessorDiffDriveTest, angular)
@@ -413,7 +413,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
double angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
- ASSERT_MATRIX_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
+ ASSERT_POSE2d_APPROX(processor->getState().vector("PO"), Vector3d(distance,0,angle), 1e-6)
}
int main(int argc, char **argv)
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 33848e703..e09d93e7c 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -111,7 +111,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightAutoPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, getState_structure)
@@ -201,7 +201,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
@@ -228,7 +228,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
@@ -251,7 +251,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleAutoPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
@@ -278,7 +278,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
@@ -305,7 +305,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
WOLF_DEBUG("t: ", t, " x: ", problem->getState().vector("PO").transpose());
}
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
+ ASSERT_POSE2d_APPROX(problem->getState().vector("PO"), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, mergeCaptures)
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index 23c0afdf5..1a1a7f834 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.cpp
@@ -103,7 +103,7 @@ TEST(ProcessorOdom3d, computeCurrentDelta)
// call the function under test
prc.computeCurrentDelta(data, data_cov, VectorXd::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
- ASSERT_MATRIX_APPROX(delta_ret , delta, Constants::EPS_SMALL);
+ ASSERT_POSE3d_APPROX(delta_ret , delta, Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(delta_cov_ret , delta_cov, Constants::EPS_SMALL);
}
@@ -128,7 +128,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta)
prc.deltaPlusDelta(D, d, dt, D_int);
- ASSERT_MATRIX_APPROX(D_int , D_int_check, 1e-10);
+ ASSERT_POSE3d_APPROX(D_int , D_int_check, 1e-10);
}
TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index 74dfe643a..fbc1dc118 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.cpp
@@ -131,7 +131,7 @@ TEST_F(StateBlockCompositeInit, set_vectors)
states.setVectors("PQ", {p, q});
ASSERT_MATRIX_APPROX(states.at('P')->getState(), p, 1e-20);
- ASSERT_MATRIX_APPROX(states.at('Q')->getState(), q, 1e-20);
+ ASSERT_QUATERNION_VECTOR_APPROX(states.at('Q')->getState(), q, 1e-20);
ASSERT_DEATH(states.setVectors("PX", {p,x}), "");
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 3609485ec..f994993b4 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
@@ -1141,7 +1141,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
WOLF_INFO("state: ", problem->getState().vector("PO").transpose());
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ ASSERT_POSE3d_APPROX(problem->getState().vector("PO").transpose(),
problem_bl->getState().vector("PO").transpose(),
1e-12);
@@ -1149,7 +1149,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowWithProcessor)
solver->solve();
solver_bl->solve();
- ASSERT_MATRIX_APPROX(problem->getState().vector("PO").transpose(),
+ ASSERT_POSE3d_APPROX(problem->getState().vector("PO").transpose(),
problem_bl->getState().vector("PO").transpose(),
1e-12);
--
GitLab