fix tests

release/4.3a0
Varun Agrawal 2022-02-06 00:09:33 -05:00
parent 5e7598606a
commit b484a214e6
2 changed files with 38 additions and 24 deletions

View File

@ -27,7 +27,6 @@
#include <GeographicLib/Config.h> #include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
@ -71,8 +70,10 @@ TEST( GPSFactor, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>( Matrix expectedH = numericalDerivative11<Vector, Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1,
boost::none),
T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH; Matrix actualH;
@ -100,8 +101,10 @@ TEST( GPSFactor2, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>( Matrix expectedH = numericalDerivative11<Vector, NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1,
boost::none),
T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH; Matrix actualH;

View File

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
@ -63,8 +62,10 @@ TEST( MagFactor, unrotate ) {
Matrix H; Matrix H;
Point3 expected(22735.5, 314.502, 44202.5); Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> // EXPECT(assert_equal(
(std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); numericalDerivative11<Point3, Rot2> //
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta),
H, 1e-6));
} }
// ************************************************************************* // *************************************************************************
@ -75,36 +76,46 @@ TEST( MagFactor, Factors ) {
// MagFactor // MagFactor
MagFactor f(1, measured, s, dir, bias, model); MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> // EXPECT(assert_equal(
(std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); (Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none),
theta),
H1, 1e-7));
// MagFactor1 // MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model); MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> // EXPECT(assert_equal(
(std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none),
nRb),
H1, 1e-7));
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// (std::bind(&MagFactor2::evaluateError, &f2,
H1, 1e-7)); std::placeholders::_1, bias, none, none),
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // scaled), //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// H1, 1e-7));
H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(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); 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(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> // EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(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)); H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> // EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(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)); H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> // EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(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)); H3, 1e-7));
} }