diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 116efe90a..50907db25 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -270,6 +270,29 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) { return J; } +//------------------------------------------------------------------------------ +Matrix5 NavState::Hat(const Vector9& xi) { + Matrix5 X; + const double wx = xi(0), wy = xi(1), wz = xi(2); + const double px = xi(3), py = xi(4), pz = xi(5); + const double vx = xi(6), vy = xi(7), vz = xi(8); + X << 0., -wz, wy, px, vx, + wz, 0., -wx, py, vy, + -wy, wx, 0., pz, vz, + 0., 0., 0., 0., 0., + 0., 0., 0., 0., 0.; + return X; +} + +//------------------------------------------------------------------------------ +Vector9 NavState::Vee(const Matrix5& Xi) { + Vector9 xi; + xi << Xi(2, 1), Xi(0, 2), Xi(1, 0), + Xi(0, 3), Xi(1, 3), Xi(2, 3), + Xi(0, 4), Xi(1, 4), Xi(2, 4); + return xi; +} + //------------------------------------------------------------------------------ NavState NavState::ChartAtOrigin::Retract(const Vector9& xi, ChartJacobian Hxi) { diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e246cbe27..c535b37ea 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -56,18 +56,27 @@ public: NavState() : t_(0, 0, 0), v_(Vector3::Zero()) { } + /// Construct from attitude, position, velocity NavState(const Rot3& R, const Point3& t, const Velocity3& v) : R_(R), t_(t), v_(v) { } + /// Construct from pose and velocity NavState(const Pose3& pose, const Velocity3& v) : R_(pose.rotation()), t_(pose.translation()), v_(v) { } + /// Construct from SO(3) and R^6 NavState(const Matrix3& R, const Vector6& tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } + + /// Construct from Matrix5 + NavState(const Matrix5& T) : + R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) { + } + /// Named constructor with derivatives static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v, OptionalJacobian<9, 3> H1 = {}, @@ -154,10 +163,6 @@ public: /// Syntactic sugar const Rot3& rotation() const { return attitude(); }; - /// @} - /// @name Lie Group - /// @{ - // Tangent space sugar. // TODO(frank): move to private navstate namespace in cpp static Eigen::Block dR(Vector9& v) { @@ -189,6 +194,12 @@ public: OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 = {}) const; + /// @} + /// @name Lie Group + /// @{ + + using LieAlgebra = Matrix5; + /** * Exponential map at identity - create a NavState from canonical coordinates * \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$ @@ -242,6 +253,12 @@ public: static Vector9 Local(const NavState& state, ChartJacobian Hstate = {}); }; + /// Hat maps from tangent vector to Lie algebra + static LieAlgebra Hat(const TangentVector& xi); + + /// Vee maps from Lie algebra to tangent vector + static TangentVector Vee(const LieAlgebra& X); + /// @} /// @name Dynamics /// @{ diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index ceeab3b35..99567509a 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -54,9 +54,24 @@ class NavState { gtsam::Vector velocity() const; gtsam::Pose3 pose() const; + // Manifold gtsam::NavState retract(const gtsam::Vector& x) const; gtsam::Vector localCoordinates(const gtsam::NavState& g) const; + // Lie Group + static gtsam::NavState Expmap(gtsam::Vector v); + static gtsam::Vector Logmap(const gtsam::NavState& p); + static gtsam::NavState Expmap(gtsam::Vector v, Eigen::Ref H); + static gtsam::Vector Logmap(const gtsam::NavState& p, Eigen::Ref H); + gtsam::NavState expmap(gtsam::Vector v); + gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref H1, Eigen::Ref H2); + gtsam::Vector logmap(const gtsam::NavState& p); + gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref H1, Eigen::Ref H2); + gtsam::Matrix AdjointMap() const; + gtsam::Vector Adjoint(gtsam::Vector xi) const; + static gtsam::Matrix Hat(const gtsam::Vector& xi); + static gtsam::Vector Vee(const gtsam::Matrix& xi); + // enabling serialization functionality void serialize() const; }; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 427531415..8377b4e97 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -533,6 +533,63 @@ TEST(NavState, Adjoint_compose_full) { EXPECT(assert_equal(expected, actual, 1e-6)); } +/* ************************************************************************* */ +TEST(NavState, HatAndVee) { + // Create a few test vectors + Vector9 v1(1, 2, 3, 4, 5, 6, 7, 8, 9); + Vector9 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, 0.3, -0.2, 0.8); + Vector9 v3 = Vector9::Zero(); + + // Test that Vee(Hat(v)) == v for various inputs + EXPECT(assert_equal(v1, NavState::Vee(NavState::Hat(v1)))); + EXPECT(assert_equal(v2, NavState::Vee(NavState::Hat(v2)))); + EXPECT(assert_equal(v3, NavState::Vee(NavState::Hat(v3)))); + + // Check the structure of the Lie Algebra element + Matrix5 expected; + expected << 0, -3, 2, 4, 7, + 3, 0, -1, 5, 8, + -2, 1, 0, 6, 9, + 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0; + + EXPECT(assert_equal(expected, NavState::Hat(v1))); +} + +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(NavState, BruteForceExpmap1) { + const Vector9 xi(0, 0, 0, 14, 24, 34, 15, 25, 35); + EXPECT(assert_equal(NavState::Expmap(xi), expm(xi), 1e-6)); +} + +TEST(NavState, BruteForceExpmap2) { + const Vector9 xi(0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0); + EXPECT(assert_equal(NavState::Expmap(xi), expm(xi), 1e-6)); +} + +TEST(NavState, BruteForceExpmap3) { + const Vector9 xi(0.1, 0.2, 0.3, 4, 5, 6, 7, 8, 9); + EXPECT(assert_equal(NavState::Expmap(xi), expm(xi), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi)) +TEST(NavState, Adjoint_hat) +{ + Matrix5 expected = T.matrix() * NavState::Hat(screwNavState::xi) * T.matrix().inverse(); + Matrix5 xiprime = NavState::Hat(T.Adjoint(screwNavState::xi)); + EXPECT(assert_equal(expected, xiprime, 1e-6)); + + Matrix5 expected2 = T2.matrix() * NavState::Hat(screwNavState::xi) * T2.matrix().inverse(); + Matrix5 xiprime2 = NavState::Hat(T2.Adjoint(screwNavState::xi)); + EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + Matrix5 expected3 = T3.matrix() * NavState::Hat(screwNavState::xi) * T3.matrix().inverse(); + Matrix5 xiprime3 = NavState::Hat(T3.Adjoint(screwNavState::xi)); + EXPECT(assert_equal(expected3, xiprime3, 1e-6)); +} + /* ************************************************************************* */ TEST(NavState, Retract_LocalCoordinates) { Vector9 d;