From 3a49c79ee86921b11819cfa754fdfa07a44ac935 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 08:25:10 -0500 Subject: [PATCH 01/19] Fix check.geometry --- gtsam/base/serializationTestHelpers.h | 2 +- gtsam/geometry/tests/testCal3Bundler.cpp | 2 +- gtsam/geometry/tests/testPose2.cpp | 49 +++++++++++---------- gtsam/geometry/tests/testQuaternion.cpp | 26 +++++++---- gtsam/geometry/tests/testRot2.cpp | 52 +++++++++++----------- gtsam/geometry/tests/testRot3.cpp | 56 +++++++++++++----------- gtsam/geometry/tests/testSO3.cpp | 18 +++++++- gtsam/geometry/tests/testSO4.cpp | 42 +++++++++++++----- 8 files changed, 145 insertions(+), 102 deletions(-) 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); From 10bf3a0199f0d630c5d08ec050c1cf27b7d8ddb5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 08:54:36 -0500 Subject: [PATCH 02/19] fix check.sam --- gtsam/nonlinear/factorTesting.h | 39 +++--- gtsam/sam/tests/testBearingFactor.cpp | 32 +---- gtsam/sam/tests/testBearingRangeFactor.cpp | 32 +---- gtsam/sam/tests/testRangeFactor.cpp | 68 ++-------- gtsam/sam/tests/testSerializationSam.cpp | 140 +++++++++++++++++++++ 5 files changed, 179 insertions(+), 132 deletions(-) create mode 100644 gtsam/sam/tests/testSerializationSam.cpp diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 3a9b6fb11..266aa841c 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -21,6 +21,8 @@ #include #include +#include +#include namespace gtsam { @@ -34,13 +36,14 @@ namespace gtsam { * This is fixable but expensive, and does not matter in practice as most factors will sit near * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ -JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { +inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, + double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; // Get size - const Eigen::VectorXd e = factor.whitenedError(values); + const Vector e = factor.whitenedError(values); const size_t rows = e.size(); // Loop over all variables @@ -51,18 +54,18 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + Vector dx = Vector::Zero(cols); dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); - const Eigen::VectorXd left = factor.whitenedError(eval_values); + const Vector left = factor.whitenedError(eval_values); dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); - const Eigen::VectorXd right = factor.whitenedError(eval_values); + const Vector right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key, J)); + jacobians.emplace_back(key, J); } // Next step...return JacobianFactor @@ -71,15 +74,15 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, namespace internal { // CPPUnitLite-style test for linearization of a factor -bool testFactorJacobians(const std::string& name_, - const NoiseModelFactor& factor, const gtsam::Values& values, double delta, - double tolerance) { - +inline bool testFactorJacobians(const std::string& name_, + const NoiseModelFactor& factor, + const gtsam::Values& values, double delta, + double tolerance) { // Create expected value by numerical differentiation JacobianFactor expected = linearizeNumerically(factor, values, delta); // Create actual value by linearize - boost::shared_ptr actual = // + auto actual = boost::dynamic_pointer_cast(factor.linearize(values)); if (!actual) return false; @@ -89,17 +92,19 @@ bool testFactorJacobians(const std::string& name_, // if not equal, test individual jacobians: if (!equal) { for (size_t i = 0; i < actual->size(); i++) { - bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)), - (Matrix) (actual->getA(actual->begin() + i)), tolerance); + bool i_good = + assert_equal((Matrix)(expected.getA(expected.begin() + i)), + (Matrix)(actual->getA(actual->begin() + i)), tolerance); if (!i_good) { - std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl; + std::cout << "Mismatch in Jacobian " << i + 1 + << " (base 1), as shown above" << std::endl; } } } return equal; } -} +} // namespace internal /// \brief Check the Jacobians produced by a factor against finite differences. /// \param factor The factor to test. @@ -109,4 +114,4 @@ bool testFactorJacobians(const std::string& name_, #define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); } -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/sam/tests/testBearingFactor.cpp b/gtsam/sam/tests/testBearingFactor.cpp index 17a049a1d..904bdba31 100644 --- a/gtsam/sam/tests/testBearingFactor.cpp +++ b/gtsam/sam/tests/testBearingFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -41,43 +40,18 @@ typedef BearingFactor BearingFactor3D; Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values! static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); - -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); - -/* ************************************************************************* */ -TEST(BearingFactor, Serialization2D) { - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); } /* ************************************************************************* */ TEST(BearingFactor, 2D) { - // Serialize the factor - std::string serialized = serializeXML(factor2D); - - // And de-serialize it - BearingFactor2D factor; - deserializeXML(serialized, factor); - // Set the linearization point Values values; values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}), values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} - -/* ************************************************************************* */ -TEST(BearingFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testBearingRangeFactor.cpp b/gtsam/sam/tests/testBearingRangeFactor.cpp index 735358d89..0dcc227c7 100644 --- a/gtsam/sam/tests/testBearingRangeFactor.cpp +++ b/gtsam/sam/tests/testBearingRangeFactor.cpp @@ -21,14 +21,13 @@ #include #include #include -#include -#include #include using namespace std; using namespace gtsam; +namespace { Key poseKey(1); Key pointKey(2); @@ -40,43 +39,18 @@ typedef BearingRangeFactor BearingRangeFactor3D; static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); BearingRangeFactor3D factor3D(poseKey, pointKey, Pose3().bearing(Point3(1, 0, 0)), 1, model3D); - -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); - -/* ************************************************************************* */ -TEST(BearingRangeFactor, Serialization2D) { - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); } /* ************************************************************************* */ TEST(BearingRangeFactor, 2D) { - // Serialize the factor - std::string serialized = serializeXML(factor2D); - - // And de-serialize it - BearingRangeFactor2D factor; - deserializeXML(serialized, factor); - // Set the linearization point Values values; values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); values.insert(pointKey, Point2(-4.0, 11.0)); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}), values, 1e-7, 1e-5); - EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); -} - -/* ************************************************************************* */ -TEST(BearingRangeFactor, Serialization3D) { - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); } /* ************************************************************************* */ diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 5f5d4f4dd..200e1236a 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -32,42 +31,40 @@ using namespace std::placeholders; using namespace std; using namespace gtsam; -// Create a noise model for the pixel error -static SharedNoiseModel model(noiseModel::Unit::Create(1)); - typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; typedef RangeFactorWithTransform RangeFactorWithTransform2D; typedef RangeFactorWithTransform RangeFactorWithTransform3D; // Keys are deliberately *not* in sorted order to test that case. +namespace { +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(1)); + constexpr Key poseKey(2); constexpr Key pointKey(1); constexpr double measurement(10.0); -/* ************************************************************************* */ Vector factorError2D(const Pose2& pose, const Point2& point, - const RangeFactor2D& factor) { + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorError3D(const Pose3& pose, const Point3& point, - const RangeFactor3D& factor) { + const RangeFactor3D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, - const RangeFactorWithTransform2D& factor) { + const RangeFactorWithTransform2D& factor) { return factor.evaluateError(pose, point); } -/* ************************************************************************* */ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, - const RangeFactorWithTransform3D& factor) { + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } +} // namespace /* ************************************************************************* */ TEST( RangeFactor, Constructor) { @@ -75,27 +72,6 @@ TEST( RangeFactor, Constructor) { RangeFactor3D factor3D(poseKey, pointKey, measurement, model); } -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit); - -/* ************************************************************************* */ -TEST(RangeFactor, Serialization2D) { - RangeFactor2D factor2D(poseKey, pointKey, measurement, model); - EXPECT(serializationTestHelpers::equalsObj(factor2D)); - EXPECT(serializationTestHelpers::equalsXML(factor2D)); - EXPECT(serializationTestHelpers::equalsBinary(factor2D)); -} - -/* ************************************************************************* */ -TEST(RangeFactor, Serialization3D) { - RangeFactor3D factor3D(poseKey, pointKey, measurement, model); - EXPECT(serializationTestHelpers::equalsObj(factor3D)); - EXPECT(serializationTestHelpers::equalsXML(factor3D)); - EXPECT(serializationTestHelpers::equalsBinary(factor3D)); -} - /* ************************************************************************* */ TEST( RangeFactor, ConstructorWithTransform) { Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); @@ -142,28 +118,6 @@ TEST( RangeFactor, EqualsWithTransform ) { body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } -/* ************************************************************************* */ -TEST( RangeFactor, EqualsAfterDeserializing) { - // Check that the same results are obtained after deserializing: - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); - - RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, - body_P_sensor_3D), factor3D_2; - - // check with Equal() trait: - gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2); - CHECK(assert_equal(factor3D_1, factor3D_2)); - - const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)); - const Point3 point(-2.0, 11.0, 1.0); - const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}; - - const Vector error_1 = factor3D_1.unwhitenedError(values); - const Vector error_2 = factor3D_2.unwhitenedError(values); - CHECK(assert_equal(error_1, error_2)); -} - /* ************************************************************************* */ TEST( RangeFactor, Error2D ) { // Create a factor @@ -411,7 +365,7 @@ TEST( RangeFactor, Camera) { /* ************************************************************************* */ // Do a test with non GTSAM types -namespace gtsam{ +namespace gtsam { template <> struct Range { typedef double result_type; @@ -421,7 +375,7 @@ struct Range { // derivatives not implemented } }; -} +} // namespace gtsam TEST(RangeFactor, NonGTSAM) { // Create a factor diff --git a/gtsam/sam/tests/testSerializationSam.cpp b/gtsam/sam/tests/testSerializationSam.cpp new file mode 100644 index 000000000..8fdd8f37e --- /dev/null +++ b/gtsam/sam/tests/testSerializationSam.cpp @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationSam.cpp + * @brief All serialization test in this directory + * @author Frank Dellaert + * @date February 2022 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +namespace { +Key poseKey(1); +Key pointKey(2); +constexpr double rangeMmeasurement(10.0); +} // namespace + +/* ************************************************************************* */ +// Export Noisemodels +// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit); + +/* ************************************************************************* */ +TEST(SerializationSam, BearingFactor2D) { + using BearingFactor2D = BearingFactor; + double measurement2D(10.0); + static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5)); + BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(SerializationSam, BearingFactor3D) { + using BearingFactor3D = BearingFactor; + Unit3 measurement3D = + Pose3().bearing(Point3(1, 0, 0)); // has to match values! + static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5)); + BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +namespace { +static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1)); +} + +TEST(SerializationSam, RangeFactor2D) { + using RangeFactor2D = RangeFactor; + RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel); + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(SerializationSam, RangeFactor3D) { + using RangeFactor3D = RangeFactor; + RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +TEST(RangeFactor, EqualsAfterDeserializing) { + // Check that the same results are obtained after deserializing: + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform factor3D_1( + poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D), + factor3D_2; + + // check with Equal() trait: + gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2); + CHECK(assert_equal(factor3D_1, factor3D_2)); + + const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0)); + const Point3 point(-2.0, 11.0, 1.0); + const Values values = {{poseKey, genericValue(pose)}, + {pointKey, genericValue(point)}}; + + const Vector error_1 = factor3D_1.unwhitenedError(values); + const Vector error_2 = factor3D_2.unwhitenedError(values); + CHECK(assert_equal(error_1, error_2)); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization2D) { + using BearingRangeFactor2D = BearingRangeFactor; + static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); + BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D); + + EXPECT(serializationTestHelpers::equalsObj(factor2D)); + EXPECT(serializationTestHelpers::equalsXML(factor2D)); + EXPECT(serializationTestHelpers::equalsBinary(factor2D)); +} + +/* ************************************************************************* */ +TEST(BearingRangeFactor, Serialization3D) { + using BearingRangeFactor3D = BearingRangeFactor; + static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); + BearingRangeFactor3D factor3D(poseKey, pointKey, + Pose3().bearing(Point3(1, 0, 0)), 1, model3D); + EXPECT(serializationTestHelpers::equalsObj(factor3D)); + EXPECT(serializationTestHelpers::equalsXML(factor3D)); + EXPECT(serializationTestHelpers::equalsBinary(factor3D)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 1aedbf76a2b33c38b450d8c3283eb1fabc340736 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 08:57:23 -0500 Subject: [PATCH 03/19] fix check.discrete --- gtsam/discrete/tests/testDecisionTreeFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 92145b8b7..846653c38 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -107,7 +107,7 @@ TEST(DecisionTreeFactor, enumerate) { } /* ************************************************************************* */ -TEST(DiscreteFactorGraph, DotWithNames) { +TEST(DecisionTreeFactor, DotWithNames) { DiscreteKey A(12, 3), B(5, 2); DecisionTreeFactor f(A & B, "1 2 3 4 5 6"); auto formatter = [](Key key) { return key == 12 ? "A" : "B"; }; From 8e33e96efaab9885f9b11b3be10adee8d2b2e6ab Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 09:30:59 -0500 Subject: [PATCH 04/19] Fix check.slam --- gtsam/slam/tests/PinholeFactor.h | 52 +++++++++ gtsam/slam/tests/smartFactorScenarios.h | 88 +++++++-------- gtsam/slam/tests/testSerializationSlam.cpp | 104 ++++++++++++++++++ gtsam/slam/tests/testSmartFactorBase.cpp | 91 ++++----------- .../slam/tests/testSmartProjectionFactor.cpp | 32 ++---- .../tests/testSmartProjectionPoseFactor.cpp | 44 +------- 6 files changed, 235 insertions(+), 176 deletions(-) create mode 100644 gtsam/slam/tests/PinholeFactor.h create mode 100644 gtsam/slam/tests/testSerializationSlam.cpp diff --git a/gtsam/slam/tests/PinholeFactor.h b/gtsam/slam/tests/PinholeFactor.h new file mode 100644 index 000000000..35500ca35 --- /dev/null +++ b/gtsam/slam/tests/PinholeFactor.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file PinholeFactor.h + * @brief helper class for tests + * @author Frank Dellaert + * @date February 2022 + */ + +#pragma once + +namespace gtsam { +template +struct traits; +} + +#include +#include +#include +#include + +namespace gtsam { + +class PinholeFactor : public SmartFactorBase > { + public: + typedef SmartFactorBase > Base; + PinholeFactor() {} + PinholeFactor(const SharedNoiseModel& sharedNoiseModel, + boost::optional body_P_sensor = boost::none, + size_t expectedNumberCameras = 10) + : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} + double error(const Values& values) const override { return 0.0; } + boost::shared_ptr linearize( + const Values& values) const override { + return boost::shared_ptr(new JacobianFactor()); + } +}; + +/// traits +template <> +struct traits : public Testable {}; + +} // namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 66be08c67..eff942799 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -29,6 +29,7 @@ using namespace std; using namespace gtsam; +namespace { // three landmarks ~5 meters infront of camera Point3 landmark1(5, 0.5, 1.2); Point3 landmark2(5, -0.5, 1.2); @@ -48,23 +49,24 @@ static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static double fov = 60; // degrees static size_t w = 640, h = 480; +} /* ************************************************************************* */ // default Cal3_S2 cameras namespace vanilla { typedef PinholeCamera Camera; typedef SmartProjectionFactor SmartFactor; -static Cal3_S2 K(fov, w, h); -static Cal3_S2 K2(1500, 1200, 0, w, h); -Camera level_camera(level_pose, K2); -Camera level_camera_right(pose_right, K2); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Camera cam1(level_pose, K2); -Camera cam2(pose_right, K2); -Camera cam3(pose_above, K2); +static const Cal3_S2 K(fov, w, h); +static const Cal3_S2 K2(1500, 1200, 0, w, h); +static const Camera level_camera(level_pose, K2); +static const Camera level_camera_right(pose_right, K2); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Camera cam1(level_pose, K2); +static const Camera cam2(pose_right, K2); +static const Camera cam3(pose_above, K2); typedef GeneralSFMFactor SFMFactor; -SmartProjectionParams params; +static const SmartProjectionParams params; } // namespace vanilla /* ************************************************************************* */ @@ -74,12 +76,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Camera level_camera(level_pose, sharedK); -Camera level_camera_right(pose_right, sharedK); -Camera cam1(level_pose, sharedK); -Camera cam2(pose_right, sharedK); -Camera cam3(pose_above, sharedK); +static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +static const Camera level_camera(level_pose, sharedK); +static const Camera level_camera_right(pose_right, sharedK); +static const Camera cam1(level_pose, sharedK); +static const Camera cam2(pose_right, sharedK); +static const Camera cam3(pose_above, sharedK); } // namespace vanillaPose /* ************************************************************************* */ @@ -89,12 +91,12 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); -Camera level_camera(level_pose, sharedK2); -Camera level_camera_right(pose_right, sharedK2); -Camera cam1(level_pose, sharedK2); -Camera cam2(pose_right, sharedK2); -Camera cam3(pose_above, sharedK2); +static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); +static const Camera level_camera(level_pose, sharedK2); +static const Camera level_camera_right(pose_right, sharedK2); +static const Camera cam1(level_pose, sharedK2); +static const Camera cam2(pose_right, sharedK2); +static const Camera cam3(pose_above, sharedK2); } // namespace vanillaPose2 /* *************************************************************************/ @@ -103,15 +105,15 @@ namespace bundler { typedef PinholeCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; -static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); -Camera level_camera(level_pose, K); -Camera level_camera_right(pose_right, K); -Point2 level_uv = level_camera.project(landmark1); -Point2 level_uv_right = level_camera_right.project(landmark1); -Pose3 pose1 = level_pose; -Camera cam1(level_pose, K); -Camera cam2(pose_right, K); -Camera cam3(pose_above, K); +static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); +static const Camera level_camera(level_pose, K); +static const Camera level_camera_right(pose_right, K); +static const Point2 level_uv = level_camera.project(landmark1); +static const Point2 level_uv_right = level_camera_right.project(landmark1); +static const Pose3 pose1 = level_pose; +static const Camera cam1(level_pose, K); +static const Camera cam2(pose_right, K); +static const Camera cam3(pose_above, K); typedef GeneralSFMFactor SFMFactor; } // namespace bundler @@ -122,14 +124,14 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, +static const boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); -Camera level_camera(level_pose, sharedBundlerK); -Camera level_camera_right(pose_right, sharedBundlerK); -Camera cam1(level_pose, sharedBundlerK); -Camera cam2(pose_right, sharedBundlerK); -Camera cam3(pose_above, sharedBundlerK); +static const Camera level_camera(level_pose, sharedBundlerK); +static const Camera level_camera_right(pose_right, sharedBundlerK); +static const Camera cam1(level_pose, sharedBundlerK); +static const Camera cam2(pose_right, sharedBundlerK); +static const Camera cam3(pose_above, sharedBundlerK); } // namespace bundlerPose /* ************************************************************************* */ @@ -138,12 +140,12 @@ namespace sphericalCamera { typedef SphericalCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionRigFactor SmartFactorP; -static EmptyCal::shared_ptr emptyK(new EmptyCal()); -Camera level_camera(level_pose); -Camera level_camera_right(pose_right); -Camera cam1(level_pose); -Camera cam2(pose_right); -Camera cam3(pose_above); +static const EmptyCal::shared_ptr emptyK(new EmptyCal()); +static const Camera level_camera(level_pose); +static const Camera level_camera_right(pose_right); +static const Camera cam1(level_pose); +static const Camera cam2(pose_right); +static const Camera cam3(pose_above); } // namespace sphericalCamera /* *************************************************************************/ diff --git a/gtsam/slam/tests/testSerializationSlam.cpp b/gtsam/slam/tests/testSerializationSlam.cpp new file mode 100644 index 000000000..02b0e3191 --- /dev/null +++ b/gtsam/slam/tests/testSerializationSlam.cpp @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationSlam.cpp + * @brief all serialization tests in this directory + * @author Frank Dellaert + * @date February 2022 + */ + +#include +#include +#include +#include + +#include +#include + +#include "smartFactorScenarios.h" +#include "PinholeFactor.h" + +namespace { +static const double rankTol = 1.0; +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); +} // namespace + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal") + +/* ************************************************************************* */ +TEST(SmartFactorBase, serialize) { + using namespace serializationTestHelpers; + PinholeFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(SerializationSlam, SmartProjectionFactor) { + using namespace vanilla; + using namespace serializationTestHelpers; + SmartFactor factor(unit2); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(SerializationSlam, SmartProjectionPoseFactor) { + using namespace vanillaPose; + using namespace serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(SerializationSlam, SmartProjectionPoseFactor2) { + using namespace vanillaPose; + using namespace serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + KeyVector key_view; + Point2Vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index ba46dce8d..544fd3264 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -16,47 +16,29 @@ * @date Feb 2015 */ -#include -#include -#include #include +#include +#include +#include +#include +#include -using namespace std; +#include "PinholeFactor.h" + +namespace { using namespace gtsam; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); +} // namespace -/* ************************************************************************* */ -#include -#include +using namespace std; namespace gtsam { -class PinholeFactor: public SmartFactorBase > { -public: - typedef SmartFactorBase > Base; - PinholeFactor() {} - PinholeFactor(const SharedNoiseModel& sharedNoiseModel, - boost::optional body_P_sensor = boost::none, - size_t expectedNumberCameras = 10) - : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {} - double error(const Values& values) const override { return 0.0; } - boost::shared_ptr linearize( - const Values& values) const override { - return boost::shared_ptr(new JacobianFactor()); - } -}; - -/// traits -template<> -struct traits : public Testable {}; -} - TEST(SmartFactorBase, Pinhole) { - PinholeFactor f= PinholeFactor(unit2); - f.add(Point2(0,0), 1); - f.add(Point2(0,0), 2); + PinholeFactor f = PinholeFactor(unit2); + f.add(Point2(0, 0), 1); + f.add(Point2(0, 0), 2); EXPECT_LONGS_EQUAL(2 * 2, f.dim()); } @@ -71,7 +53,7 @@ TEST(SmartFactorBase, PinholeWithSensor) { // Camera coordinates in world frame. Pose3 wTc = world_P_body * body_P_sensor; cameras.push_back(PinholeCamera(wTc)); - + // Simple point to project slightly off image center Point3 p(0, 0, 10); Point2 measurement = cameras[0].project(p); @@ -81,9 +63,10 @@ TEST(SmartFactorBase, PinholeWithSensor) { Matrix E; Vector error = f.unwhitenedError(cameras, p, Fs, E); - Vector expectedError = Vector::Zero(2); + Vector expectedError = Vector::Zero(2); Matrix29 expectedFs; - expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0; + expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, + 0, 0, 0, 0; Matrix23 expectedE; expectedE << 0.1, 0, 0.01, 0, 0.1, 0; @@ -94,20 +77,13 @@ TEST(SmartFactorBase, PinholeWithSensor) { EXPECT(assert_equal(expectedE, E)); } -/* ************************************************************************* */ -#include - -namespace gtsam { - -class StereoFactor: public SmartFactorBase { -public: +class StereoFactor : public SmartFactorBase { + public: typedef SmartFactorBase Base; StereoFactor() {} - StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { - } - double error(const Values& values) const override { - return 0.0; - } + StereoFactor(const SharedNoiseModel& sharedNoiseModel) + : Base(sharedNoiseModel) {} + double error(const Values& values) const override { return 0.0; } boost::shared_ptr linearize( const Values& values) const override { return boost::shared_ptr(new JacobianFactor()); @@ -115,9 +91,8 @@ public: }; /// traits -template<> +template <> struct traits : public Testable {}; -} TEST(SmartFactorBase, Stereo) { StereoFactor f(unit3); @@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) { f.add(StereoPoint2(), 2); EXPECT_LONGS_EQUAL(2 * 3, f.dim()); } - -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartFactorBase, serialize) { - using namespace gtsam::serializationTestHelpers; - PinholeFactor factor(unit2); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} +} // namespace gtsam /* ************************************************************************* */ int main() { @@ -150,4 +108,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 133f81511..ecdb5287f 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -22,18 +22,19 @@ #include "smartFactorScenarios.h" #include #include -#include #include #include #include using namespace boost::assign; +namespace { static const bool isDebugTest = false; static const Symbol l1('l', 1), l2('l', 2), l3('l', 3); static const Key c1 = 1, c2 = 2, c3 = 3; static const Point2 measurement1(323.0, 240.0); static const double rankTol = 1.0; +} template PinholeCamera perturbCameraPoseAndCalibration( @@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor2) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = params; + myParams.setRankTolerance(rankTol); + SmartFactor factor1(unit2, myParams); } /* ************************************************************************* */ @@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionFactor, Constructor4) { using namespace vanilla; - params.setRankTolerance(rankTol); - SmartFactor factor1(unit2, params); + auto myParams = params; + myParams.setRankTolerance(rankTol); + SmartFactor factor1(unit2, myParams); factor1.add(measurement1, c1); } @@ -810,25 +813,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { EXPECT(assert_equal(yActual, yExpected, 1e-7)); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartProjectionFactor, serialize) { - using namespace vanilla; - using namespace gtsam::serializationTestHelpers; - SmartFactor factor(unit2); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f3706fa02..5c38233c1 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,7 +24,6 @@ #include #include #include -#include #include #include #include @@ -32,6 +31,7 @@ using namespace boost::assign; using namespace std::placeholders; +namespace { static const double rankTol = 1.0; // Create a noise model for the pixel error static const double sigma = 0.1; @@ -51,6 +51,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; +} /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { @@ -1332,47 +1333,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(x3))); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") - -TEST(SmartProjectionPoseFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -TEST(SmartProjectionPoseFactor, serialize2) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - Pose3 bts; - SmartFactor factor(model, sharedK, bts, params); - - // insert some measurments - KeyVector key_view; - Point2Vector meas_view; - key_view.push_back(Symbol('x', 1)); - meas_view.push_back(Point2(10, 10)); - factor.add(meas_view, key_view); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; From f71f12498db60837eda7bb1e76d01e846273b393 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 09:39:22 -0500 Subject: [PATCH 05/19] fixed check.navigation --- gtsam/navigation/tests/testAttitudeFactor.cpp | 29 ----------- gtsam/navigation/tests/testMagFactor.cpp | 5 +- gtsam/navigation/tests/testMagPoseFactor.cpp | 7 +-- ...on.cpp => testSerializationNavigation.cpp} | 49 ++++++++++++++----- 4 files changed, 43 insertions(+), 47 deletions(-) rename gtsam/navigation/tests/{testImuFactorSerialization.cpp => testSerializationNavigation.cpp} (63%) diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 9304e8412..26d793528 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -63,22 +61,6 @@ TEST( Rot3AttitudeFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -// Export Noisemodels -// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic) - -/* ************************************************************************* */ -TEST(Rot3AttitudeFactor, Serialization) { - Unit3 nDown(0, 0, -1); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); - Rot3AttitudeFactor factor(0, nDown, model); - - EXPECT(serializationTestHelpers::equalsObj(factor)); - EXPECT(serializationTestHelpers::equalsXML(factor)); - EXPECT(serializationTestHelpers::equalsBinary(factor)); -} - /* ************************************************************************* */ TEST(Rot3AttitudeFactor, CopyAndMove) { Unit3 nDown(0, 0, -1); @@ -129,17 +111,6 @@ TEST( Pose3AttitudeFactor, Constructor ) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } -/* ************************************************************************* */ -TEST(Pose3AttitudeFactor, Serialization) { - Unit3 nDown(0, 0, -1); - SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); - Pose3AttitudeFactor factor(0, nDown, model); - - EXPECT(serializationTestHelpers::equalsObj(factor)); - EXPECT(serializationTestHelpers::equalsXML(factor)); - EXPECT(serializationTestHelpers::equalsBinary(factor)); -} - /* ************************************************************************* */ TEST(Pose3AttitudeFactor, CopyAndMove) { Unit3 nDown(0, 0, -1); diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 85447facd..e2a623710 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -31,7 +31,7 @@ using namespace std; using namespace gtsam; using namespace GeographicLib; -// ************************************************************************* +namespace { // Convert from Mag to ENU // ENU Origin is where the plane was in hold next to runway // const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; @@ -51,10 +51,11 @@ Point3 bias(10, -10, 50); Point3 scaled = scale * nM; Point3 measured = nRb.inverse() * (scale * nM) + bias; -double s(scale * nM.norm()); +double s(scale* nM.norm()); Unit3 dir(nM); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); +} // namespace using boost::none; diff --git a/gtsam/navigation/tests/testMagPoseFactor.cpp b/gtsam/navigation/tests/testMagPoseFactor.cpp index 204c1d38f..e10409f4c 100644 --- a/gtsam/navigation/tests/testMagPoseFactor.cpp +++ b/gtsam/navigation/tests/testMagPoseFactor.cpp @@ -20,7 +20,7 @@ using namespace std::placeholders; using namespace gtsam; -// ***************************************************************************** +namespace { // Magnetic field in the nav frame (NED), with units of nT. Point3 nM(22653.29982, -1956.83010, 44202.47862); @@ -51,8 +51,9 @@ SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25); // Make up a rotation and offset of the sensor in the body frame. Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0)); -Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3)); -// ***************************************************************************** +Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), + Point3(-0.1, 0.2, 0.3)); +} // namespace // ***************************************************************************** TEST(MagPoseFactor, Constructors) { diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testSerializationNavigation.cpp similarity index 63% rename from gtsam/navigation/tests/testImuFactorSerialization.cpp rename to gtsam/navigation/tests/testSerializationNavigation.cpp index e72b1fb5b..63d2606a1 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testSerializationNavigation.cpp @@ -10,17 +10,19 @@ * -------------------------------------------------------------------------- */ /** - * @file testImuFactor.cpp - * @brief Unit test for ImuFactor + * @file testSerializationNavigation.cpp + * @brief serialization tests for navigation * @author Luca Carlone * @author Frank Dellaert * @author Richard Roberts * @author Stephen Williams * @author Varun Agrawal + * @date February 2022 */ #include #include +#include #include #include @@ -30,17 +32,13 @@ using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, - "gtsam_noiseModel_Constrained") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, - "gtsam_noiseModel_Diagonal") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, - "gtsam_noiseModel_Gaussian") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, - "gtsam_noiseModel_Isotropic") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained") +BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian") +BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit") +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal") template P getPreintegratedMeasurements() { @@ -69,6 +67,7 @@ P getPreintegratedMeasurements() { return pim; } +/* ************************************************************************* */ TEST(ImuFactor, serialization) { auto pim = getPreintegratedMeasurements(); @@ -83,6 +82,7 @@ TEST(ImuFactor, serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ TEST(ImuFactor2, serialization) { auto pim = getPreintegratedMeasurements(); @@ -93,6 +93,7 @@ TEST(ImuFactor2, serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ TEST(CombinedImuFactor, Serialization) { auto pim = getPreintegratedMeasurements(); @@ -107,6 +108,28 @@ TEST(CombinedImuFactor, Serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(Rot3AttitudeFactor, Serialization) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Rot3AttitudeFactor factor(0, nDown, model); + + EXPECT(serializationTestHelpers::equalsObj(factor)); + EXPECT(serializationTestHelpers::equalsXML(factor)); + EXPECT(serializationTestHelpers::equalsBinary(factor)); +} + +/* ************************************************************************* */ +TEST(Pose3AttitudeFactor, Serialization) { + Unit3 nDown(0, 0, -1); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); + Pose3AttitudeFactor factor(0, nDown, model); + + EXPECT(serializationTestHelpers::equalsObj(factor)); + EXPECT(serializationTestHelpers::equalsXML(factor)); + EXPECT(serializationTestHelpers::equalsBinary(factor)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 65a3928d0ae47443e548941ec67217a8b051aa1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 09:40:41 -0500 Subject: [PATCH 06/19] fixed check.basis --- gtsam/basis/tests/testChebyshev.cpp | 3 ++- gtsam/basis/tests/testChebyshev2.cpp | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp index 64c925886..7d7f9323d 100644 --- a/gtsam/basis/tests/testChebyshev.cpp +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -25,9 +25,10 @@ using namespace std; using namespace gtsam; +namespace { auto model = noiseModel::Unit::Create(1); - const size_t N = 3; +} // namespace //****************************************************************************** TEST(Chebyshev, Chebyshev1) { diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 4cee70daf..6fafc0ccf 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -27,9 +27,11 @@ using namespace std; using namespace gtsam; using namespace boost::placeholders; +namespace { noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); const size_t N = 32; +} // namespace //****************************************************************************** TEST(Chebyshev2, Point) { From 3d6a7bf970a1e1a28f269a80ad86449ed0bd7604 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 09:43:12 -0500 Subject: [PATCH 07/19] Fix more stuff in check.slam --- .../testSmartStereoProjectionFactorPP.cpp | 21 ++++++++++++------- .../testSmartStereoProjectionPoseFactor.cpp | 19 ++++++++++------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 61836d098..c71c19038 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -31,12 +31,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -45,8 +46,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3); static Key poseKey1(x1); static Key poseExtrinsicKey1(body_P_cam1_key); static Key poseExtrinsicKey2(body_P_cam2_key); -static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? -static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value? +static StereoPoint2 measurement1( + 323.0, 300.0, 240.0); // potentially use more reasonable measurement value? +static StereoPoint2 measurement2( + 350.0, 200.0, 240.0); // potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); + Point3(0.25, -0.10, 1.0)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -82,6 +86,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionFactorPP, params) { diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index a0bfc3649..fc56b1a9f 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,13 +32,13 @@ using namespace std; using namespace boost::assign; using namespace gtsam; +namespace { // make a realistic calibration matrix static double b = 1; static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b)); -static Cal3_S2Stereo::shared_ptr K2( - new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b)); - +static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, + b)); static SmartStereoProjectionParams params; @@ -47,8 +47,8 @@ static SmartStereoProjectionParams params; static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -56,15 +56,17 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Key poseKey1(x1); -static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value? +static StereoPoint2 measurement1( + 323.0, 300.0, 240.0); // potentially use more reasonable measurement value? static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); + Point3(0.25, -0.10, 1.0)); static double missing_uR = std::numeric_limits::quiet_NaN(); vector stereo_projectToMultipleCameras(const StereoCamera& cam1, - const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { - + const StereoCamera& cam2, + const StereoCamera& cam3, + Point3 landmark) { vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); @@ -78,6 +80,7 @@ vector stereo_projectToMultipleCameras(const StereoCamera& cam1, } LevenbergMarquardtParams lm_params; +} // namespace /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, params) { From c979a6f1364aeeac43c559bed956f6bcfdbb7199 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 13:07:26 -0500 Subject: [PATCH 08/19] use anonymous namespace --- gtsam/geometry/tests/testPose2.cpp | 10 ++-------- gtsam/geometry/tests/testQuaternion.cpp | 10 ++-------- gtsam/geometry/tests/testRot2.cpp | 7 ++----- gtsam/geometry/tests/testRot3.cpp | 10 ++-------- gtsam/geometry/tests/testSO3.cpp | 16 ++-------------- gtsam/geometry/tests/testSO4.cpp | 21 ++------------------- 6 files changed, 12 insertions(+), 62 deletions(-) diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index c199a7af0..0df858aa8 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -796,16 +796,14 @@ TEST(Pose2, align_4) { } //****************************************************************************** -namespace pose2_example { +namespace { 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 +} // namespace //****************************************************************************** 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)); @@ -819,8 +817,6 @@ TEST(Pose2, Invariants) { //****************************************************************************** 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); @@ -829,8 +825,6 @@ TEST(Pose2, LieGroupDerivatives) { //****************************************************************************** TEST(Pose2, ChartDerivatives) { - using namespace pose2_example; - CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, T2); CHECK_CHART_DERIVATIVES(T2, id); diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 88548d15e..281c71f7c 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -102,17 +102,15 @@ TEST(Quaternion , Inverse) { } //****************************************************************************** -namespace q_example { +namespace { 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 +} // namespace //****************************************************************************** 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)); @@ -126,8 +124,6 @@ TEST(Quaternion, Invariants) { //****************************************************************************** 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); @@ -136,8 +132,6 @@ TEST(Quaternion, LieGroupDerivatives) { //****************************************************************************** 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 f08f3e2a5..5a087edcd 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -156,15 +156,14 @@ TEST( Rot2, relativeBearing ) } //****************************************************************************** -namespace rot2_example { +namespace { Rot2 id; Rot2 T1(0.1); Rot2 T2(0.2); -} // namespace rot2_example +} // namespace //****************************************************************************** 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)); @@ -178,7 +177,6 @@ TEST(Rot2, Invariants) { //****************************************************************************** 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); @@ -187,7 +185,6 @@ TEST(Rot2, LieGroupDerivatives) { //****************************************************************************** TEST(Rot2, ChartDerivatives) { - using namespace rot2_example; CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, T2); CHECK_CHART_DERIVATIVES(T2, id); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 537ec8f4f..1df342d57 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -640,16 +640,14 @@ TEST( Rot3, slerp) } //****************************************************************************** -namespace rot3_example { +namespace { Rot3 id; Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1)); Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2)); -} // namespace rot3_example +} // namespace //****************************************************************************** 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)); @@ -665,8 +663,6 @@ TEST(Rot3, Invariants) { //****************************************************************************** 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); @@ -676,8 +672,6 @@ TEST(Rot3, LieGroupDerivatives) { //****************************************************************************** TEST(Rot3, ChartDerivatives) { - using namespace rot3_example; - if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { CHECK_CHART_DERIVATIVES(id, id); CHECK_CHART_DERIVATIVES(id, T2); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 64d3e681d..96c1cce32 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -67,16 +67,15 @@ TEST(SO3, ClosestTo) { } //****************************************************************************** -namespace so3_example { +namespace { 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 +} // namespace /* ************************************************************************* */ TEST(SO3, ChordalMean) { - using namespace so3_example; std::vector rotations = {R1, R1.inverse()}; EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations))); } @@ -84,7 +83,6 @@ TEST(SO3, ChordalMean) { //****************************************************************************** // Check that Hat specialization is equal to dynamic version TEST(SO3, Hat) { - 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))); @@ -93,7 +91,6 @@ TEST(SO3, Hat) { //****************************************************************************** // Check that Hat specialization is equal to dynamic version TEST(SO3, Vee) { - 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))); @@ -102,7 +99,6 @@ 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)); @@ -110,7 +106,6 @@ 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)); @@ -118,7 +113,6 @@ 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)); @@ -126,7 +120,6 @@ 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)); @@ -134,8 +127,6 @@ 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)); @@ -149,7 +140,6 @@ 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); @@ -158,7 +148,6 @@ 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); @@ -363,7 +352,6 @@ 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 7379049c3..fa550723a 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -55,7 +55,7 @@ TEST(SO4, Random) { } //****************************************************************************** -namespace so4_example { +namespace { SO4 id; Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished(); SO4 Q1 = SO4::Expmap(v1); @@ -63,12 +63,10 @@ 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 +} // namespace //****************************************************************************** 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))); @@ -91,7 +89,6 @@ TEST(SO4, Expmap) { //****************************************************************************** // Check that Hat specialization is equal to dynamic version TEST(SO4, Hat) { - 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))); @@ -100,7 +97,6 @@ TEST(SO4, Hat) { //****************************************************************************** // Check that Hat specialization is equal to dynamic version TEST(SO4, Vee) { - 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))); @@ -109,8 +105,6 @@ 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))); @@ -127,7 +121,6 @@ TEST(SO4, Retract) { //****************************************************************************** // Check that Cayley is identical to dynamic version TEST(SO4, Local) { - using namespace so4_example; EXPECT( assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1)))); EXPECT( @@ -140,8 +133,6 @@ 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)); @@ -157,8 +148,6 @@ TEST(SO4, Invariants) { //****************************************************************************** TEST(SO4, compose) { - using namespace so4_example; - SO4 expected = Q1 * Q2; Matrix actualH1, actualH2; SO4 actual = Q1.compose(Q2, actualH1, actualH2); @@ -175,8 +164,6 @@ TEST(SO4, compose) { //****************************************************************************** TEST(SO4, vec) { - using namespace so4_example; - using Vector16 = SO4::VectorN2; const Vector16 expected = Eigen::Map(Q2.matrix().data()); Matrix actualH; @@ -189,8 +176,6 @@ TEST(SO4, vec) { //****************************************************************************** TEST(SO4, topLeft) { - using namespace so4_example; - const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>(); Matrix actualH; const Matrix3 actual = topLeft(Q3, actualH); @@ -204,8 +189,6 @@ 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); From 67c1b26f2b9e40c08b5596bd3ce257260cd9f005 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 13:07:38 -0500 Subject: [PATCH 09/19] Try if combined tests are faster --- .github/scripts/unix.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index d890577b6..5bca4dca4 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -91,6 +91,7 @@ function build () { export GTSAM_BUILD_EXAMPLES_ALWAYS=ON export GTSAM_BUILD_TESTS=OFF + export GTSAM_SINGLE_TEST_EXE=ON configure @@ -108,6 +109,7 @@ function test () { export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF export GTSAM_BUILD_TESTS=ON + export GTSAM_SINGLE_TEST_EXE=ON configure From 7b124f4953c3de8364e1d9d1c94169d6740649cd Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 13:24:54 -0500 Subject: [PATCH 10/19] Try grouped tests - again --- .github/scripts/unix.sh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 5bca4dca4..3091531ea 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -71,6 +71,7 @@ function configure() -DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \ -DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ + -DGTSAM_SINGLE_TEST_EXE=ON \ -DBOOST_ROOT=$BOOST_ROOT \ -DBoost_NO_SYSTEM_PATHS=ON \ -DBoost_ARCHITECTURE=-x64 @@ -91,7 +92,6 @@ function build () { export GTSAM_BUILD_EXAMPLES_ALWAYS=ON export GTSAM_BUILD_TESTS=OFF - export GTSAM_SINGLE_TEST_EXE=ON configure @@ -109,7 +109,6 @@ function test () { export GTSAM_BUILD_EXAMPLES_ALWAYS=OFF export GTSAM_BUILD_TESTS=ON - export GTSAM_SINGLE_TEST_EXE=ON configure From 311325cc2f4a9e10d138438bfddb4f9dfb89e968 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 14:06:58 -0500 Subject: [PATCH 11/19] Fixed another serialization clash --- tests/testSerializationSLAM.cpp | 70 +++++++++++++++++++++++++++ tests/testSubgraphPreconditioner.cpp | 71 ---------------------------- 2 files changed, 70 insertions(+), 71 deletions(-) diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 496122419..3f030cdff 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -592,6 +592,76 @@ TEST (testSerializationSLAM, factors) { EXPECT(equalsBinary(genericStereoFactor3D)); } +/* ************************************************************************* */ +// Read from XML file +namespace { +static GaussianFactorGraph read(const string& name) { + auto inputFile = findExampleDataFile(name); + ifstream is(inputFile); + if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); + boost::archive::xml_iarchive in_archive(is); + GaussianFactorGraph Ab; + in_archive >> boost::serialization::make_nvp("graph", Ab); + return Ab; +} +} // namespace + +/* ************************************************************************* */ +// Read from XML file +TEST(SubgraphSolver, Solves) { + // Create preconditioner + SubgraphPreconditioner system; + + // We test on three different graphs + const auto Ab1 = planarGraph(3).first; + const auto Ab2 = read("toy3D"); + const auto Ab3 = read("randomGrid3D"); + + // For all graphs, test solve and solveTranspose + for (const auto& Ab : {Ab1, Ab2, Ab3}) { + // Call build, a non-const method needed to make solve work :-( + KeyInfo keyInfo(Ab); + std::map lambda; + system.build(Ab, keyInfo, lambda); + + // Create a perturbed (non-zero) RHS + const auto xbar = system.Rc1().optimize(); // merely for use in zero below + auto values_y = VectorValues::Zero(xbar); + auto it = values_y.begin(); + it->second.setConstant(100); + ++it; + it->second.setConstant(-100); + + // Solve the VectorValues way + auto values_x = system.Rc1().backSubstitute(values_y); + + // Solve the matrix way, this really just checks BN::backSubstitute + // This only works with Rc1 ordering, not with keyInfo ! + // TODO(frank): why does this not work with an arbitrary ordering? + const auto ord = system.Rc1().ordering(); + const Matrix R1 = system.Rc1().matrix(ord).first; + auto ord_y = values_y.vector(ord); + auto vector_x = R1.inverse() * ord_y; + EXPECT(assert_equal(vector_x, values_x.vector(ord))); + + // Test that 'solve' does implement x = R^{-1} y + // We do this by asserting it gives same answer as backSubstitute + // Only works with keyInfo ordering: + const auto ordering = keyInfo.ordering(); + auto vector_y = values_y.vector(ordering); + const size_t N = R1.cols(); + Vector solve_x = Vector::Zero(N); + system.solve(vector_y, solve_x); + EXPECT(assert_equal(values_x.vector(ordering), solve_x)); + + // Test that transposeSolve does implement x = R^{-T} y + // We do this by asserting it gives same answer as backSubstituteTranspose + auto values_x2 = system.Rc1().backSubstituteTranspose(values_y); + Vector solveT_x = Vector::Zero(N); + system.transposeSolve(vector_y, solveT_x); + EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); + } +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index eeba38b68..c5b4e42ec 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -29,10 +29,8 @@ #include -#include #include #include -#include #include using namespace boost::assign; @@ -197,75 +195,6 @@ TEST(SubgraphPreconditioner, system) { EXPECT(assert_equal(expected_g, vec(g))); } -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") - -// Read from XML file -static GaussianFactorGraph read(const string& name) { - auto inputFile = findExampleDataFile(name); - ifstream is(inputFile); - if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile); - boost::archive::xml_iarchive in_archive(is); - GaussianFactorGraph Ab; - in_archive >> boost::serialization::make_nvp("graph", Ab); - return Ab; -} - -TEST(SubgraphSolver, Solves) { - // Create preconditioner - SubgraphPreconditioner system; - - // We test on three different graphs - const auto Ab1 = planarGraph(3).first; - const auto Ab2 = read("toy3D"); - const auto Ab3 = read("randomGrid3D"); - - // For all graphs, test solve and solveTranspose - for (const auto& Ab : {Ab1, Ab2, Ab3}) { - // Call build, a non-const method needed to make solve work :-( - KeyInfo keyInfo(Ab); - std::map lambda; - system.build(Ab, keyInfo, lambda); - - // Create a perturbed (non-zero) RHS - const auto xbar = system.Rc1().optimize(); // merely for use in zero below - auto values_y = VectorValues::Zero(xbar); - auto it = values_y.begin(); - it->second.setConstant(100); - ++it; - it->second.setConstant(-100); - - // Solve the VectorValues way - auto values_x = system.Rc1().backSubstitute(values_y); - - // Solve the matrix way, this really just checks BN::backSubstitute - // This only works with Rc1 ordering, not with keyInfo ! - // TODO(frank): why does this not work with an arbitrary ordering? - const auto ord = system.Rc1().ordering(); - const Matrix R1 = system.Rc1().matrix(ord).first; - auto ord_y = values_y.vector(ord); - auto vector_x = R1.inverse() * ord_y; - EXPECT(assert_equal(vector_x, values_x.vector(ord))); - - // Test that 'solve' does implement x = R^{-1} y - // We do this by asserting it gives same answer as backSubstitute - // Only works with keyInfo ordering: - const auto ordering = keyInfo.ordering(); - auto vector_y = values_y.vector(ordering); - const size_t N = R1.cols(); - Vector solve_x = Vector::Zero(N); - system.solve(vector_y, solve_x); - EXPECT(assert_equal(values_x.vector(ordering), solve_x)); - - // Test that transposeSolve does implement x = R^{-T} y - // We do this by asserting it gives same answer as backSubstituteTranspose - auto values_x2 = system.Rc1().backSubstituteTranspose(values_y); - Vector solveT_x = Vector::Zero(N); - system.transposeSolve(vector_y, solveT_x); - EXPECT(assert_equal(values_x2.vector(ordering), solveT_x)); - } -} - /* ************************************************************************* */ TEST(SubgraphPreconditioner, conjugateGradients) { // Build a planar graph From d2f8041e13e8558352c938e457ea83338ff239f3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 15:15:14 -0500 Subject: [PATCH 12/19] rename --- ...alizationSLAM.cpp => testSerializationSam.cpp} | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) rename tests/{testSerializationSLAM.cpp => testSerializationSam.cpp} (99%) diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSam.cpp similarity index 99% rename from tests/testSerializationSLAM.cpp rename to tests/testSerializationSam.cpp index 3f030cdff..bc2f5e864 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSam.cpp @@ -19,16 +19,16 @@ #include #include + +#include #include + #include #include -#include #include -#include +#include #include -#include -#include -#include + #include #include #include @@ -44,6 +44,11 @@ #include #include +#include +#include +#include +#include +#include #include using namespace std; From 63b643e0bf908d8acb6766469c4c0840742d463f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 15:16:05 -0500 Subject: [PATCH 13/19] rename again --- tests/{testSerializationSam.cpp => testSerializationSlam.cpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tests/{testSerializationSam.cpp => testSerializationSlam.cpp} (100%) diff --git a/tests/testSerializationSam.cpp b/tests/testSerializationSlam.cpp similarity index 100% rename from tests/testSerializationSam.cpp rename to tests/testSerializationSlam.cpp From bd487ac1f6461d770354a97808b22d821b790345 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 16:12:47 -0500 Subject: [PATCH 14/19] add using... --- tests/testSerializationSlam.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index bc2f5e864..3a1798f5e 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -614,6 +614,8 @@ static GaussianFactorGraph read(const string& name) { /* ************************************************************************* */ // Read from XML file TEST(SubgraphSolver, Solves) { + using gtsam::example::planarGraph; + // Create preconditioner SubgraphPreconditioner system; From b975cdcc3d3beea4b58a285c9fdf5b5543cc07aa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 20:58:20 -0500 Subject: [PATCH 15/19] Compilation issues with linux system --- gtsam/base/FastSet.h | 1 + gtsam/discrete/tests/testDecisionTree.cpp | 22 ++++++++++--------- ...onSlam.cpp => testSerializationInSlam.cpp} | 0 3 files changed, 13 insertions(+), 10 deletions(-) rename gtsam/slam/tests/{testSerializationSlam.cpp => testSerializationInSlam.cpp} (100%) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index a5ee77495..6fe2d06e3 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,6 +18,7 @@ #pragma once +#include #if BOOST_VERSION >= 107400 #include #endif diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index dbfb2dc40..967023eeb 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -17,17 +17,19 @@ * @date Jan 30, 2012 */ -#include -using namespace boost::assign; - -#include -#include -#include - // #define DT_DEBUG_MEMORY // #define DT_NO_PRUNING #define DISABLE_DOT #include + +#include +#include + +#include + +#include +using namespace boost::assign; + using namespace std; using namespace gtsam; @@ -148,9 +150,9 @@ TEST(DecisionTree, example) { DOT(notb); // Check supplying empty trees yields an exception - CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error); - CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error); - CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(empty, &Ring::id), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(empty, a, &Ring::mul), std::runtime_error); + CHECK_EXCEPTION(gtsam::apply(a, empty, &Ring::mul), std::runtime_error); // apply, two nodes, in natural order DT anotb = apply(a, notb, &Ring::mul); diff --git a/gtsam/slam/tests/testSerializationSlam.cpp b/gtsam/slam/tests/testSerializationInSlam.cpp similarity index 100% rename from gtsam/slam/tests/testSerializationSlam.cpp rename to gtsam/slam/tests/testSerializationInSlam.cpp From 2f53a67b4f936bc53c2d205f07823146888087f4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 20:58:57 -0500 Subject: [PATCH 16/19] rename to avoid conflict with tests --- gtsam/slam/tests/testSerializationInSlam.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/tests/testSerializationInSlam.cpp b/gtsam/slam/tests/testSerializationInSlam.cpp index 02b0e3191..6aec8ecb0 100644 --- a/gtsam/slam/tests/testSerializationInSlam.cpp +++ b/gtsam/slam/tests/testSerializationInSlam.cpp @@ -16,17 +16,18 @@ * @date February 2022 */ -#include -#include -#include +#include "smartFactorScenarios.h" +#include "PinholeFactor.h" + #include +#include +#include + +#include #include #include -#include "smartFactorScenarios.h" -#include "PinholeFactor.h" - namespace { static const double rankTol = 1.0; static const double sigma = 0.1; From bac5d4f1209f26f939dec6099028dccade4f96e6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 20:59:26 -0500 Subject: [PATCH 17/19] Fix serialization class name --- examples/Data/randomGrid3D.xml | 2 +- examples/Data/toy3D.xml | 2 +- tests/testSerializationSlam.cpp | 5 ++++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/examples/Data/randomGrid3D.xml b/examples/Data/randomGrid3D.xml index 6a82ce31c..42eb473be 100644 --- a/examples/Data/randomGrid3D.xml +++ b/examples/Data/randomGrid3D.xml @@ -7,7 +7,7 @@ 32 1 - + diff --git a/examples/Data/toy3D.xml b/examples/Data/toy3D.xml index 13dbcbe6c..26bd231ca 100644 --- a/examples/Data/toy3D.xml +++ b/examples/Data/toy3D.xml @@ -7,7 +7,7 @@ 2 1 - + diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index 3a1798f5e..ea7038635 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -51,6 +51,9 @@ #include #include +#include +#include + using namespace std; using namespace gtsam; using namespace gtsam::serializationTestHelpers; @@ -615,7 +618,7 @@ static GaussianFactorGraph read(const string& name) { // Read from XML file TEST(SubgraphSolver, Solves) { using gtsam::example::planarGraph; - + // Create preconditioner SubgraphPreconditioner system; From 0167716037cc1b6463b43346aed71f038a05ecd2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 21:27:28 -0500 Subject: [PATCH 18/19] avoid more warnings on Linux --- gtsam/geometry/PinholeCamera.h | 14 ++++++++------ gtsam/nonlinear/tests/testCallRecord.cpp | 3 +-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 61e9f0909..339e95e70 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -230,13 +230,15 @@ public: Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera, OptionalJacobian<2, FixedDimension::value> Dpoint) const { // We just call 3-derivative version in Base - Matrix26 Dpose; - Eigen::Matrix Dcal; - Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, - Dcamera ? &Dcal : 0); - if (Dcamera) + if (Dcamera){ + Matrix26 Dpose; + Eigen::Matrix Dcal; + const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal); *Dcamera << Dpose, Dcal; - return pi; + return pi; + } else { + return Base::project(pw, boost::none, Dpoint, boost::none); + } } /// project a 3D point from world coordinates into the image diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 5d0d5d5f2..419172f74 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -153,7 +153,7 @@ TEST(CallRecord, virtualReverseAdDispatching) { } { const int Rows = 6; - record.CallRecord::reverseAD2(Eigen::Matrix(), NJM); + record.CallRecord::reverseAD2(Eigen::Matrix::Zero(), NJM); EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols)))); record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM); EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols)))); @@ -168,4 +168,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 59fbdf9cff4dc55671adfaa8cb93d420d4c1f6bc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 16 Feb 2022 21:29:59 -0500 Subject: [PATCH 19/19] use anonymous namespace --- gtsam/navigation/tests/imuFactorTesting.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/navigation/tests/imuFactorTesting.h b/gtsam/navigation/tests/imuFactorTesting.h index 5aa83e87e..6160db2a1 100644 --- a/gtsam/navigation/tests/imuFactorTesting.h +++ b/gtsam/navigation/tests/imuFactorTesting.h @@ -28,6 +28,7 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; +namespace { static const Vector3 kZero = Z_3x1; typedef imuBias::ConstantBias Bias; static const Bias kZeroBiasHat, kZeroBias; @@ -43,6 +44,7 @@ static const Vector3 kGravityAlongNavZDown(0, 0, kGravity); auto radians = [](double t) { return t * M_PI / 180; }; static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW static const double kAccelSigma = 0.1 / 60; // 10 cm VRW +} namespace testing {