From 5ca67d0a3a4a29458de7a49b939248e64f93582f Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jul 2015 11:20:35 -0700 Subject: [PATCH 1/3] FromPoseVelocity --- gtsam/navigation/NavState.cpp | 16 +++++++++++++--- gtsam/navigation/NavState.h | 3 +++ gtsam/navigation/tests/testNavState.cpp | 22 +++++++++++++++++++--- 3 files changed, 35 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index a52b74704..c45b86450 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -24,6 +24,16 @@ namespace gtsam { #define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_; +//------------------------------------------------------------------------------ +NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) { + if (H1) + *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; + if (H2) + *H2 << Z_3x3, Z_3x3, pose.rotation().transpose(); + return NavState(pose, vel); +} + //------------------------------------------------------------------------------ const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const { if (H) @@ -252,9 +262,9 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, //------------------------------------------------------------------------------ Vector9 NavState::correctPIM(const Vector9& pim, double dt, - const Vector3& n_gravity, - const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis, - OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { + const Vector3& n_gravity, const boost::optional& omegaCoriolis, + bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! const double dt2 = dt * dt; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 8eb8c54d7..996ae763b 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -67,6 +67,9 @@ public: NavState(const Matrix3& R, const Vector9 tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } + /// Named constructor with derivatives + static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2); /// @} /// @name Component Access diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index c0797bd9c..1d71d4e7c 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -26,6 +26,7 @@ using namespace gtsam; static const Rot3 kRotation = Rot3::RzRyRx(0.1, 0.2, 0.3); static const Point3 kPosition(1.0, 2.0, 3.0); +static const Pose3 kPose(kRotation, kPosition); static const Velocity3 kVelocity(0.4, 0.5, 0.6); static const NavState kIdentity; static const NavState kState1(kRotation, kPosition, kVelocity); @@ -33,6 +34,16 @@ static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04); static const Vector3 kGravity(0, 0, 9.81); static const Vector9 kZeroXi = Vector9::Zero(); +/* ************************************************************************* */ +TEST(NavState, Constructor) { + boost::function construct = + 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(numericalDerivative21(construct, kPose, kVelocity), aH1)); + EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2)); +} /* ************************************************************************* */ TEST( NavState, Attitude) { Matrix39 aH, eH; @@ -127,9 +138,14 @@ TEST( NavState, Manifold ) { // Check localCoordinates derivatives 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)); + 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)); } /* ************************************************************************* */ From 999a19a57d26a78cbfb470007d0a0c896ba77be8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jul 2015 11:52:46 -0700 Subject: [PATCH 2/3] 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)); } /* ************************************************************************* */ From 9d1111a76702d327c1ee0ebec61da516b6169768 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 29 Jul 2015 11:53:24 -0700 Subject: [PATCH 3/3] Derivative accuracy --- gtsam/navigation/PreintegrationBase.cpp | 2 ++ gtsam/navigation/tests/testImuFactor.cpp | 3 +-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 367a62360..3b1cea06f 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -183,6 +183,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + // Note that derivative of constructors below is not identity for velocity, but + // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() NavState state_i(pose_i, vel_i); NavState state_j(pose_j, vel_j); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index c95c1d667..79497f63c 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -872,8 +872,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) { values.insert(B(1), bias); // Make sure linearization is correct - double diffDelta = 1e-5; - // This fails, except if tol = 1e-1: probably wrong! + double diffDelta = 1e-7; EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); }