diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h index b89b9102ab64073b729744ec5d6c7d4d8098b143..85aff3e6a4a533a2303f5e1a6c45dd484cffc8d1 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 be07e98047c8074979d036074f2df6ee490e9a90..ff1ddb6d1ab166f9ee11c2dbc5821669c5114e78 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 4d7137a2028c90a15ca8817f09a11962e6d22764..7019025dc74d3db5d2c668d5d7ea7f55f234e31f 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 0d03e872da2f6aaa9905e576b85385f8242b08e9..2cca034dd35daa2b368b9a0e1bc1c347fb0a0ec7 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 a352ae12ebf13b2d312a6b4a7333f47b644a37f3..f818f97ee35b288943e3c989bb12933c96074d61 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 c3548e02dc394eb80b90f0e7a04d7afd3bda3ed8..718ca9809fb860ae4b1b431d66b798e3f37210f6 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 0588ab5b0b63c15e09d6137ccc52d06d4bf89b16..524255e52010730fad55fecdd5851a1317faa98f 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 3c35668b0aaa1bd4a96a359e21158834338f58c8..e1a328927679631ea3bb63357b08d009908a5cf2 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 6cab028bb08b6eb4b70f7056684aca44ba4bced3..7d82e020710bae62b9f7bb6950b338cfa1fa39f2 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 a709195a5e179f6c64bfa37cabf9fa8e2eb7e375..fcab277470397448464745ff1031d60fa335bf55 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 a6b2177e18235a8948c0cfa4379b2c7da2023146..9c90d2ce427328577eb57ac41d221ecd05f680d8 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 a5808a865a0fb02bb523df1a2e60e2c86208852a..874682995a2b49dcf8eed3314c064e4f709d8aca 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 d71d2daaca4676ef2a4bc9a9e565ca94aa1fc104..87eb9db92716ea4ad4ddefec788bb60499780d33 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 11a0663c11bb2730faa62cd7921861b26cd79259..04fa9ddb439181ed6013e02546c7e2a72838366e 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 33848e703660bddc6d1993f0d77a749f01b9cfc9..e09d93e7c737cdc6be91297ce07a68d2b6fe4280 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 23c0afdf5a4bcf4ef2586d7e63b692ae4c953906..1a1a7f834b1a4c2ff074c170b7c25620e14e3708 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 74dfe643ab18b1658a774fa782fe22e0a9284a66..fbc1dc1186a5095fce16d8e4804c1d6352498b33 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 3609485ec09f2ac328b1f2f693eca7adffe96b16..f994993b47580e4b7b32aa8218896d5f2eba19a8 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);