12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -27,7 +27,6 @@
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp>
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<Vector,Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
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<Vector,NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T);
Matrix expectedH = numericalDerivative11<Vector, NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1,
boost::none),
T);
// Use the factor to calculate the derivative
Matrix actualH;

49
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp>
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<Point3,Rot2> //
(std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
EXPECT(assert_equal(
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 f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
EXPECT(assert_equal(
(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);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
EXPECT(assert_equal(
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);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2,
std::placeholders::_1, bias, none, none),
scaled), //
H1, 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);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
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));
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));
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));
}