diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index c535b37ea..6c93a6b94 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -254,10 +254,10 @@ public: }; /// Hat maps from tangent vector to Lie algebra - static LieAlgebra Hat(const TangentVector& xi); + static Matrix5 Hat(const Vector9& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector9 Vee(const Matrix5& X); /// @} /// @name Dynamics @@ -300,9 +300,9 @@ private: // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; } // namespace gtsam diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 8377b4e97..e36b654b0 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -52,6 +52,13 @@ static const NavState T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2, V2); static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7), Point3(1, 2, 3)); +//****************************************************************************** +TEST(NavState, Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); +} + /* ************************************************************************* */ TEST(NavState, Constructor) { std::function create =