diff --git a/gtsam/base/serializationTestHelpers.h b/gtsam/base/serializationTestHelpers.h index 5994a5e51..bb8574245 100644 --- a/gtsam/base/serializationTestHelpers.h +++ b/gtsam/base/serializationTestHelpers.h @@ -42,7 +42,7 @@ T create() { } // Creates or empties a folder in the build folder and returns the relative path -boost::filesystem::path resetFilesystem( +inline boost::filesystem::path resetFilesystem( boost::filesystem::path folder = "actual") { boost::filesystem::remove_all(folder); boost::filesystem::create_directory(folder); diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index cd576f900..020cab2f9 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) { } /* ************************************************************************* */ -TEST(Cal3_S2, Print) { +TEST(Cal3Bundler, Print) { Cal3Bundler cal(1, 2, 3, 4, 5); std::stringstream os; os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2() diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index d17dc7689..c199a7af0 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -796,44 +796,45 @@ TEST(Pose2, align_4) { } //****************************************************************************** +namespace pose2_example { +Pose2 id; Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); +} // namespace pose2_example //****************************************************************************** -TEST(Pose2 , Invariants) { - Pose2 id; +TEST(Pose2, Invariants) { + using namespace pose2_example; - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); } //****************************************************************************** -TEST(Pose2 , LieGroupDerivatives) { - Pose2 id; - - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); +TEST(Pose2, LieGroupDerivatives) { + using namespace pose2_example; + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Pose2 , ChartDerivatives) { - Pose2 id; +TEST(Pose2, ChartDerivatives) { + using namespace pose2_example; - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,T2); - CHECK_CHART_DERIVATIVES(T2,id); - CHECK_CHART_DERIVATIVES(T2,T1); + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T2, T1); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index e862b94ad..88548d15e 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -80,12 +80,6 @@ TEST(Quaternion , Compose) { EXPECT(traits::Equals(expected, actual)); } -//****************************************************************************** -Vector3 Q_z_axis(0, 0, 1); -Q id(Eigen::AngleAxisd(0, Q_z_axis)); -Q R1(Eigen::AngleAxisd(1, Q_z_axis)); -Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - //****************************************************************************** TEST(Quaternion , Between) { Vector3 z_axis(0, 0, 1); @@ -108,7 +102,17 @@ TEST(Quaternion , Inverse) { } //****************************************************************************** -TEST(Quaternion , Invariants) { +namespace q_example { +Vector3 Q_z_axis(0, 0, 1); +Q id(Eigen::AngleAxisd(0, Q_z_axis)); +Q R1(Eigen::AngleAxisd(1, Q_z_axis)); +Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); +} // namespace q_example + +//****************************************************************************** +TEST(Quaternion, Invariants) { + using namespace q_example; + EXPECT(check_group_invariants(id, id)); EXPECT(check_group_invariants(id, R1)); EXPECT(check_group_invariants(R2, id)); @@ -121,7 +125,9 @@ TEST(Quaternion , Invariants) { } //****************************************************************************** -TEST(Quaternion , LieGroupDerivatives) { +TEST(Quaternion, LieGroupDerivatives) { + using namespace q_example; + CHECK_LIE_GROUP_DERIVATIVES(id, id); CHECK_LIE_GROUP_DERIVATIVES(id, R2); CHECK_LIE_GROUP_DERIVATIVES(R2, id); @@ -129,7 +135,9 @@ TEST(Quaternion , LieGroupDerivatives) { } //****************************************************************************** -TEST(Quaternion , ChartDerivatives) { +TEST(Quaternion, ChartDerivatives) { + using namespace q_example; + CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, R2); CHECK_CHART_DERIVATIVES(R2, id); diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 7cd27a9da..f08f3e2a5 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -156,44 +156,42 @@ TEST( Rot2, relativeBearing ) } //****************************************************************************** +namespace rot2_example { +Rot2 id; Rot2 T1(0.1); Rot2 T2(0.2); +} // namespace rot2_example //****************************************************************************** -TEST(Rot2 , Invariants) { - Rot2 id; - - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); +TEST(Rot2, Invariants) { + using namespace rot2_example; + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); } //****************************************************************************** -TEST(Rot2 , LieGroupDerivatives) { - Rot2 id; - - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); - +TEST(Rot2, LieGroupDerivatives) { + using namespace rot2_example; + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Rot2 , ChartDerivatives) { - Rot2 id; - - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,T2); - CHECK_CHART_DERIVATIVES(T2,id); - CHECK_CHART_DERIVATIVES(T2,T1); +TEST(Rot2, ChartDerivatives) { + using namespace rot2_example; + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T2, T1); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index dc4b888b3..537ec8f4f 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -640,46 +640,50 @@ TEST( Rot3, slerp) } //****************************************************************************** +namespace rot3_example { +Rot3 id; Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1)); Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2)); +} // namespace rot3_example //****************************************************************************** -TEST(Rot3 , Invariants) { - Rot3 id; +TEST(Rot3, Invariants) { + using namespace rot3_example; - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,T1)); - EXPECT(check_group_invariants(T2,id)); - EXPECT(check_group_invariants(T2,T1)); - EXPECT(check_group_invariants(T1,T2)); + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, T1)); + EXPECT(check_group_invariants(T2, id)); + EXPECT(check_group_invariants(T2, T1)); + EXPECT(check_group_invariants(T1, T2)); - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,T1)); - EXPECT(check_manifold_invariants(T2,id)); - EXPECT(check_manifold_invariants(T2,T1)); - EXPECT(check_manifold_invariants(T1,T2)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, T1)); + EXPECT(check_manifold_invariants(T2, id)); + EXPECT(check_manifold_invariants(T2, T1)); + EXPECT(check_manifold_invariants(T1, T2)); } //****************************************************************************** -TEST(Rot3 , LieGroupDerivatives) { - Rot3 id; +TEST(Rot3, LieGroupDerivatives) { + using namespace rot3_example; - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,id); - CHECK_LIE_GROUP_DERIVATIVES(T1,T2); - CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, id); + CHECK_LIE_GROUP_DERIVATIVES(T1, T2); + CHECK_LIE_GROUP_DERIVATIVES(T2, T1); } //****************************************************************************** -TEST(Rot3 , ChartDerivatives) { - Rot3 id; +TEST(Rot3, ChartDerivatives) { + using namespace rot3_example; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,T2); - CHECK_CHART_DERIVATIVES(T2,id); - CHECK_CHART_DERIVATIVES(T1,T2); - CHECK_CHART_DERIVATIVES(T2,T1); + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, T2); + CHECK_CHART_DERIVATIVES(T2, id); + CHECK_CHART_DERIVATIVES(T1, T2); + CHECK_CHART_DERIVATIVES(T2, T1); } } diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 910d482b0..64d3e681d 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -67,28 +67,33 @@ TEST(SO3, ClosestTo) { } //****************************************************************************** +namespace so3_example { SO3 id; Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +} // namespace so3_example /* ************************************************************************* */ TEST(SO3, ChordalMean) { + using namespace so3_example; std::vector rotations = {R1, R1.inverse()}; EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations))); } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO3, Hat) { - // Check that Hat specialization is equal to dynamic version + using namespace so3_example; EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis))); EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2))); EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3))); } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO3, Vee) { - // Check that Hat specialization is equal to dynamic version + using namespace so3_example; auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1))); EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2))); @@ -97,6 +102,7 @@ TEST(SO3, Vee) { //****************************************************************************** TEST(SO3, Local) { + using namespace so3_example; Vector3 expected(0, 0, 0.1); Vector3 actual = traits::Local(R1, R2); EXPECT(assert_equal((Vector)expected, actual)); @@ -104,6 +110,7 @@ TEST(SO3, Local) { //****************************************************************************** TEST(SO3, Retract) { + using namespace so3_example; Vector3 v(0, 0, 0.1); SO3 actual = traits::Retract(R1, v); EXPECT(assert_equal(R2, actual)); @@ -111,6 +118,7 @@ TEST(SO3, Retract) { //****************************************************************************** TEST(SO3, Logmap) { + using namespace so3_example; Vector3 expected(0, 0, 0.1); Vector3 actual = SO3::Logmap(R1.between(R2)); EXPECT(assert_equal((Vector)expected, actual)); @@ -118,6 +126,7 @@ TEST(SO3, Logmap) { //****************************************************************************** TEST(SO3, Expmap) { + using namespace so3_example; Vector3 v(0, 0, 0.1); SO3 actual = R1 * SO3::Expmap(v); EXPECT(assert_equal(R2, actual)); @@ -125,6 +134,8 @@ TEST(SO3, Expmap) { //****************************************************************************** TEST(SO3, Invariants) { + using namespace so3_example; + EXPECT(check_group_invariants(id, id)); EXPECT(check_group_invariants(id, R1)); EXPECT(check_group_invariants(R2, id)); @@ -138,6 +149,7 @@ TEST(SO3, Invariants) { //****************************************************************************** TEST(SO3, LieGroupDerivatives) { + using namespace so3_example; CHECK_LIE_GROUP_DERIVATIVES(id, id); CHECK_LIE_GROUP_DERIVATIVES(id, R2); CHECK_LIE_GROUP_DERIVATIVES(R2, id); @@ -146,6 +158,7 @@ TEST(SO3, LieGroupDerivatives) { //****************************************************************************** TEST(SO3, ChartDerivatives) { + using namespace so3_example; CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, R2); CHECK_CHART_DERIVATIVES(R2, id); @@ -350,6 +363,7 @@ TEST(SO3, ApplyInvDexp) { //****************************************************************************** TEST(SO3, vec) { + using namespace so3_example; const Vector9 expected = Eigen::Map(R2.matrix().data()); Matrix actualH; const Vector9 actual = R2.vec(actualH); diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 5486755f7..7379049c3 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -48,6 +48,14 @@ TEST(SO4, Concept) { } //****************************************************************************** +TEST(SO4, Random) { + std::mt19937 rng(42); + auto Q = SO4::Random(rng); + EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); +} + +//****************************************************************************** +namespace so4_example { SO4 id; Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); SO4 Q1 = SO4::Expmap(v1); @@ -55,15 +63,12 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished(); SO4 Q2 = SO4::Expmap(v2); Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished(); SO4 Q3 = SO4::Expmap(v3); +} // namespace so4_example -//****************************************************************************** -TEST(SO4, Random) { - std::mt19937 rng(42); - auto Q = SO4::Random(rng); - EXPECT_LONGS_EQUAL(4, Q.matrix().rows()); -} //****************************************************************************** TEST(SO4, Expmap) { + using namespace so4_example; + // If we do exponential map in SO(3) subgroup, topleft should be equal to R1. auto R1 = SO3::Expmap(v1.tail<3>()).matrix(); EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1))); @@ -84,16 +89,18 @@ TEST(SO4, Expmap) { } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO4, Hat) { - // Check that Hat specialization is equal to dynamic version + using namespace so4_example; EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1))); EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2))); EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3))); } //****************************************************************************** +// Check that Hat specialization is equal to dynamic version TEST(SO4, Vee) { - // Check that Hat specialization is equal to dynamic version + using namespace so4_example; auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1))); EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2))); @@ -102,6 +109,8 @@ TEST(SO4, Vee) { //****************************************************************************** TEST(SO4, Retract) { + using namespace so4_example; + // Check that Cayley yields the same as Expmap for small values EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100))); EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100))); @@ -116,8 +125,9 @@ TEST(SO4, Retract) { } //****************************************************************************** +// Check that Cayley is identical to dynamic version TEST(SO4, Local) { - // Check that Cayley is identical to dynamic version + using namespace so4_example; EXPECT( assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1)))); EXPECT( @@ -130,6 +140,8 @@ TEST(SO4, Local) { //****************************************************************************** TEST(SO4, Invariants) { + using namespace so4_example; + EXPECT(check_group_invariants(id, id)); EXPECT(check_group_invariants(id, Q1)); EXPECT(check_group_invariants(Q2, id)); @@ -145,6 +157,8 @@ TEST(SO4, Invariants) { //****************************************************************************** TEST(SO4, compose) { + using namespace so4_example; + SO4 expected = Q1 * Q2; Matrix actualH1, actualH2; SO4 actual = Q1.compose(Q2, actualH1, actualH2); @@ -161,20 +175,22 @@ TEST(SO4, compose) { //****************************************************************************** TEST(SO4, vec) { + using namespace so4_example; + using Vector16 = SO4::VectorN2; const Vector16 expected = Eigen::Map(Q2.matrix().data()); Matrix actualH; const Vector16 actual = Q2.vec(actualH); EXPECT(assert_equal(expected, actual)); - std::function f = [](const SO4& Q) { - return Q.vec(); - }; + std::function f = [](const SO4& Q) { return Q.vec(); }; const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5); EXPECT(assert_equal(numericalH, actualH)); } //****************************************************************************** TEST(SO4, topLeft) { + using namespace so4_example; + const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>(); Matrix actualH; const Matrix3 actual = topLeft(Q3, actualH); @@ -188,6 +204,8 @@ TEST(SO4, topLeft) { //****************************************************************************** TEST(SO4, stiefel) { + using namespace so4_example; + const Matrix43 expected = Q3.matrix().leftCols<3>(); Matrix actualH; const Matrix43 actual = stiefel(Q3, actualH);