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

More tests on localCoordinates

release/4.3a0
dellaert 2015-07-29 11:52:46 -07:00
parent 5ca67d0a3a
commit 999a19a57d
1 changed files with 16 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/navigation/tests/testNavState.cpp
View File

@ -40,7 +40,9 @@ TEST(NavState, Constructor) {
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none); boost::none);
Matrix aH1, aH2; Matrix aH1, aH2;
EXPECT(assert_equal(kState1, NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2))); EXPECT(
assert_equal(kState1,
NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1)); EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
} }
@ -136,16 +138,20 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
// Check localCoordinates derivatives // Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none);
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2); kState1.localCoordinates(state2, aH1, aH2);
boost::function<Vector9(const NavState&, const NavState&)> localCoordinates = EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
boost::bind(&NavState::localCoordinates, _1, _2, boost::none, EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
boost::none); // from identity to state2
EXPECT( kIdentity.localCoordinates(state2, aH1, aH2);
assert_equal(numericalDerivative21(localCoordinates, kState1, state2), EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
aH1)); EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
EXPECT( // from state2 to identity
assert_equal(numericalDerivative22(localCoordinates, kState1, state2), state2.localCoordinates(kIdentity, aH1, aH2);
aH2)); EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */