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)); } /* ************************************************************************* */