12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

MatrixLieGroup in navigation

release/4.3a0
Frank Dellaert 2025-03-08 15:26:15 -05:00
parent 5e3f798c12
commit 39a15620bd
2 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/navigation/NavState.h
View File

@ -254,10 +254,10 @@ public:
}; };
/// Hat maps from tangent vector to Lie algebra /// 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 /// Vee maps from Lie algebra to tangent vector
static TangentVector Vee(const LieAlgebra& X); static Vector9 Vee(const Matrix5& X);
/// @} /// @}
/// @name Dynamics /// @name Dynamics
@ -300,9 +300,9 @@ private:
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors // Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template <> template <>
struct traits<NavState> : public internal::LieGroup<NavState> {}; struct traits<NavState> : public internal::MatrixLieGroup<NavState> {};
template <> template <>
struct traits<const NavState> : public internal::LieGroup<NavState> {}; struct traits<const NavState> : public internal::MatrixLieGroup<NavState> {};
} // namespace gtsam } // namespace gtsam

7
gtsam/navigation/tests/testNavState.cpp
View File

@ -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), static const NavState T3(Rot3::Rodrigues(-90, 0, 0), Point3(5, 6, 7),
Point3(1, 2, 3)); Point3(1, 2, 3));
//******************************************************************************
TEST(NavState, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<NavState >);
GTSAM_CONCEPT_ASSERT(IsManifold<NavState >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<NavState >);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(NavState, Constructor) { TEST(NavState, Constructor) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create = std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =