From b484a214e6793655206786fa8fd1d55af43b8b9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 6 Feb 2022 00:09:33 -0500 Subject: [PATCH] fix tests --- gtsam/navigation/tests/testGPSFactor.cpp | 13 ++++--- gtsam/navigation/tests/testMagFactor.cpp | 49 +++++++++++++++--------- 2 files changed, 38 insertions(+), 24 deletions(-) diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b784c0c94..5b96b80a8 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -27,7 +27,6 @@ #include #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -71,8 +70,10 @@ TEST( GPSFactor, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + Matrix expectedH = numericalDerivative11( + std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, + boost::none), + T); // Use the factor to calculate the derivative Matrix actualH; @@ -100,8 +101,10 @@ TEST( GPSFactor2, Constructor ) { EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + Matrix expectedH = numericalDerivative11( + std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, + boost::none), + T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5107b3b6b..2450b16c7 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -26,7 +26,6 @@ #include -using namespace std::placeholders; using namespace std; using namespace gtsam; using namespace GeographicLib; @@ -63,8 +62,10 @@ TEST( MagFactor, unrotate ) { Matrix H; Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); - EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + EXPECT(assert_equal( + numericalDerivative11 // + (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), + H, 1e-6)); } // ************************************************************************* @@ -75,36 +76,46 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + EXPECT(assert_equal( + (Matrix)numericalDerivative11 // + (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), + theta), + H1, 1e-7)); -// MagFactor1 + // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + EXPECT(assert_equal( + numericalDerivative11 // + (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), + nRb), + H1, 1e-7)); -// MagFactor2 + // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// - H1, 1e-7)); - EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// - H2, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (std::bind(&MagFactor2::evaluateError, &f2, + std::placeholders::_1, bias, none, none), + scaled), // + H1, 1e-7)); + EXPECT(assert_equal(numericalDerivative11 // + (std::bind(&MagFactor2::evaluateError, &f2, scaled, + std::placeholders::_1, none, none), + bias), // + H2, 1e-7)); -// MagFactor2 + // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),// H3, 1e-7)); }