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
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<NavState> : public internal::LieGroup<NavState> {};
struct traits<NavState> : public internal::MatrixLieGroup<NavState> {};
template <>
struct traits<const NavState> : public internal::LieGroup<NavState> {};
struct traits<const NavState> : public internal::MatrixLieGroup<NavState> {};
} // 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),
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) {
std::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =