diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index bb5f8ed79..b5c426597 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -204,8 +204,8 @@ Pose2 Pose2::inverse() const { } /* ************************************************************************* */ -Pose2::LieAlgebra Pose2::Hat(const Pose2::TangentVector& xi) { - LieAlgebra X; +Matrix3 Pose2::Hat(const Pose2::TangentVector& xi) { + Matrix3 X; X << 0., -xi.z(), xi.x(), xi.z(), 0., xi.y(), 0., 0., 0.; @@ -213,7 +213,7 @@ Pose2::LieAlgebra Pose2::Hat(const Pose2::TangentVector& xi) { } /* ************************************************************************* */ -Pose2::TangentVector Pose2::Vee(const Pose2::LieAlgebra& X) { +Pose2::TangentVector Pose2::Vee(const Matrix3& X) { return TangentVector(X(0, 2), X(1, 2), X(1,0)); } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 6c7644123..93bc6751b 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -201,10 +201,10 @@ public: using LieGroup::inverse; // version with derivative /// Hat maps from tangent vector to Lie algebra - static LieAlgebra Hat(const TangentVector& xi); + static Matrix3 Hat(const Vector3& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector3 Vee(const Matrix3& X); /// @} /// @name Group Action on Point2 @@ -338,7 +338,7 @@ public: #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 /// @deprecated: use Hat - static inline LieAlgebra wedge(double vx, double vy, double w) { + static inline Matrix3 wedge(double vx, double vy, double w) { return Hat(TangentVector(vx, vy, w)); } #endif @@ -375,10 +375,10 @@ using Pose2Pair = std::pair; using Pose2Pairs = std::vector; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; // bearing and range traits, used in RangeFactor template diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 28f7817e6..154e24a36 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -238,10 +238,10 @@ public: * v (vx,vy,vz) = 3D velocity * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ - static LieAlgebra Hat(const TangentVector& xi); + static Matrix4 Hat(const Vector6& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector6 Vee(const Matrix4& X); /// @} /// @name Group Action on Point3 @@ -446,10 +446,10 @@ using Pose3Pairs = std::vector >; typedef std::vector Pose3Vector; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; // bearing and range traits, used in RangeFactor template <> diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 86fc366ec..f0deba5a9 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -140,11 +140,11 @@ struct traits { using LieAlgebra = Matrix3; - static LieAlgebra Hat(const Vector3& v) { + static Matrix3 Hat(const Vector3& v) { return SO3::Hat(v); } - static Vector3 Vee(const LieAlgebra& X) { + static Vector3 Vee(const Matrix3& X) { return SO3::Vee(X); } diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 9b57740a3..f1eca66d8 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -82,15 +82,15 @@ Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { return v; } /* ************************************************************************* */ -Rot2::LieAlgebra Rot2::Hat(const Rot2::TangentVector& xi) { - LieAlgebra X; +Matrix2 Rot2::Hat(const Vector1& xi) { + Matrix2 X; X << 0., -xi.x(), xi.x(), 0.; return X; } /* ************************************************************************* */ -Rot2::TangentVector Rot2::Vee(const Rot2::LieAlgebra& X) { +Vector1 Rot2::Vee(const Matrix2& X) { TangentVector v; v << X(1, 0); return v; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 74a4540cb..85b41e72c 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -158,10 +158,10 @@ namespace gtsam { using LieGroup::inverse; // version with derivative /// Hat maps from tangent vector to Lie algebra - static LieAlgebra Hat(const TangentVector& xi); + static Matrix2 Hat(const Vector1& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector1 Vee(const Matrix2& X); /// @} /// @name Group Action on Point2 @@ -239,9 +239,9 @@ namespace gtsam { }; template<> - struct traits : public internal::LieGroup {}; + struct traits : public internal::MatrixLieGroup {}; template<> - struct traits : public internal::LieGroup {}; + struct traits : public internal::MatrixLieGroup {}; } // gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 09977816c..5768399f8 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -410,10 +410,10 @@ class GTSAM_EXPORT Rot3 : public LieGroup { using LieGroup::inverse; // version with derivative /// Hat maps from tangent vector to Lie algebra - static inline LieAlgebra Hat(const TangentVector& xi) { return SO3::Hat(xi); } + static inline Matrix3 Hat(const Vector3& xi) { return SO3::Hat(xi); } /// Vee maps from Lie algebra to tangent vector - static inline TangentVector Vee(const LieAlgebra& X) { return SO3::Vee(X); } + static inline Vector3 Vee(const Matrix3& X) { return SO3::Vee(X); } /// @} /// @name Group Action on Point3 @@ -589,10 +589,10 @@ class GTSAM_EXPORT Rot3 : public LieGroup { const Matrix3& A, OptionalJacobian<3, 9> H = {}); 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/geometry/SO3.h b/gtsam/geometry/SO3.h index 1bdcd82c7..ee51f4d83 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -224,13 +224,13 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { } // namespace so3 /* - * Define the traits. internal::LieGroup provides both Lie group and Testable + * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable */ template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; } // end namespace gtsam diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index 43b74f7aa..6a37f84bb 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -106,13 +106,13 @@ void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) { #endif /* - * Define the traits. internal::LieGroup provides both Lie group and Testable + * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable */ template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::MatrixLieGroup {}; } // end namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 910b848fd..b24070c91 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -396,14 +396,14 @@ void serialize( #endif /* - * Define the traits. internal::LieGroup provides both Lie group and Testable + * Define the traits. internal::MatrixLieGroup provides both Lie group and Testable */ 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/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index 9a27842ac..cc4ffc792 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -170,10 +170,10 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { using LieGroup::inverse; /// Hat maps from tangent vector to Lie algebra - static LieAlgebra Hat(const TangentVector& xi); + static Matrix3 Hat(const Vector4& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector4 Vee(const Matrix3& X); /// @} /// @name Standard interface @@ -201,9 +201,9 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { }; 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/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index dc66e8721..6eb1d5584 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -174,10 +174,10 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { * @param xi 7-dim twist (w,u,lambda) where * @return 4*4 element of Lie algebra that can be exponentiated */ - static LieAlgebra Hat(const TangentVector& xi); + static Matrix4 Hat(const Vector7& xi); /// Vee maps from Lie algebra to tangent vector - static TangentVector Vee(const LieAlgebra& X); + static Vector7 Vee(const Matrix4& X); /// @} /// @name Standard interface @@ -243,9 +243,9 @@ inline Matrix wedge(const Vector& xi) { } #endif 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/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 54eb80669..9bbc8f5ca 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -37,7 +37,7 @@ GTSAM_CONCEPT_LIE_INST(Pose2) TEST(Pose2 , Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index a4a6eb574..d14af7ac0 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -45,7 +45,7 @@ static const double tol=1e-5; TEST(Pose3 , Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index c99c67e0f..40781b832 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -31,7 +31,7 @@ typedef traits::ChartJacobian QuaternionJacobian; TEST(Quaternion , Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 76cd99919..65b97fc09 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -40,7 +40,7 @@ static double error = 1e-9, epsilon = 0.001; TEST(Rot3 , Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 17b27daea..3f06624e1 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -38,7 +38,7 @@ TEST(SO3, Identity) { TEST(SO3, Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index 7a3198901..b084ace35 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -44,7 +44,7 @@ TEST(SO4, Identity) { TEST(SO4, Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index 741ddd3d0..b9609116b 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -86,7 +86,7 @@ TEST(SOn, SO5) { TEST(SOn, Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index b1af74aa8..78b25b50f 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -37,7 +37,7 @@ static const double s = 4; TEST(Similarity2, Concepts) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index 86a4ad940..ce0814f7c 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -56,7 +56,7 @@ const double degree = M_PI / 180; TEST(Similarity3, Concepts) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); - GTSAM_CONCEPT_ASSERT(IsLieGroup); + GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup); } //******************************************************************************