From 999a19a57d26a78cbfb470007d0a0c896ba77be8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jul 2015 11:52:46 -0700 Subject: [PATCH] More tests on localCoordinates --- gtsam/navigation/tests/testNavState.cpp | 26 +++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 1d71d4e7c..6732643e1 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -40,7 +40,9 @@ TEST(NavState, Constructor) { boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none, boost::none); 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(numericalDerivative22(construct, kPose, kVelocity), aH2)); } @@ -136,16 +138,20 @@ TEST( NavState, Manifold ) { EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2)); // Check localCoordinates derivatives + boost::function local = + boost::bind(&NavState::localCoordinates, _1, _2, boost::none, boost::none); + // from state1 to state2 kState1.localCoordinates(state2, aH1, aH2); - boost::function localCoordinates = - boost::bind(&NavState::localCoordinates, _1, _2, boost::none, - boost::none); - EXPECT( - assert_equal(numericalDerivative21(localCoordinates, kState1, state2), - aH1)); - EXPECT( - assert_equal(numericalDerivative22(localCoordinates, kState1, state2), - aH2)); + EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2)); + // from identity to state2 + kIdentity.localCoordinates(state2, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2)); + // from state2 to identity + state2.localCoordinates(kIdentity, aH1, aH2); + EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1)); + EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2)); } /* ************************************************************************* */