fix tests
parent
5e7598606a
commit
b484a214e6
|
|
@ -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;
|
||||||
|
|
|
||||||
|
|
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue