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

MatrixLieGroup in geometry

release/4.3a0
Frank Dellaert 2025-03-08 15:25:59 -05:00
parent 091ec68838
commit 5e3f798c12
21 changed files with 51 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Pose2.cpp
View File

@ -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));
}

10
gtsam/geometry/Pose2.h
View File

@ -201,10 +201,10 @@ public:
using LieGroup<Pose2, 3>::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<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
struct traits<Pose2> : public internal::MatrixLieGroup<Pose2> {};
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
struct traits<const Pose2> : public internal::MatrixLieGroup<Pose2> {};
// bearing and range traits, used in RangeFactor
template <typename T>

8
gtsam/geometry/Pose3.h
View File

@ -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<std::pair<Pose3, Pose3> >;
typedef std::vector<Pose3> Pose3Vector;
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
struct traits<Pose3> : public internal::MatrixLieGroup<Pose3> {};
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
struct traits<const Pose3> : public internal::MatrixLieGroup<Pose3> {};
// bearing and range traits, used in RangeFactor
template <>

4
gtsam/geometry/Quaternion.h
View File

@ -140,11 +140,11 @@ struct traits<QUATERNION_TYPE> {
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);
}

6
gtsam/geometry/Rot2.cpp
View File

@ -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;

8
gtsam/geometry/Rot2.h
View File

@ -158,10 +158,10 @@ namespace gtsam {
using LieGroup<Rot2, 1>::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<Rot2> : public internal::LieGroup<Rot2> {};
struct traits<Rot2> : public internal::MatrixLieGroup<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
struct traits<const Rot2> : public internal::MatrixLieGroup<Rot2> {};
} // gtsam

8
gtsam/geometry/Rot3.h
View File

@ -410,10 +410,10 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
using LieGroup<Rot3, 3>::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<Rot3, 3> {
const Matrix3& A, OptionalJacobian<3, 9> H = {});
template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
struct traits<Rot3> : public internal::MatrixLieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
struct traits<const Rot3> : public internal::MatrixLieGroup<Rot3> {};
} // namespace gtsam

6
gtsam/geometry/SO3.h
View File

@ -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<SO3> : public internal::LieGroup<SO3> {};
struct traits<SO3> : public internal::MatrixLieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
struct traits<const SO3> : public internal::MatrixLieGroup<SO3> {};
} // end namespace gtsam

6
gtsam/geometry/SO4.h
View File

@ -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<SO4> : public internal::LieGroup<SO4> {};
struct traits<SO4> : public internal::MatrixLieGroup<SO4> {};
template <>
struct traits<const SO4> : public internal::LieGroup<SO4> {};
struct traits<const SO4> : public internal::MatrixLieGroup<SO4> {};
} // end namespace gtsam

6
gtsam/geometry/SOn.h
View File

@ -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 <int N>
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
struct traits<SO<N>> : public internal::MatrixLieGroup<SO<N>> {};
template <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
struct traits<const SO<N>> : public internal::MatrixLieGroup<SO<N>> {};
} // namespace gtsam

8
gtsam/geometry/Similarity2.h
View File

@ -170,10 +170,10 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
using LieGroup<Similarity2, 4>::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<Similarity2, 4> {
};
template <>
struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
struct traits<Similarity2> : public internal::MatrixLieGroup<Similarity2> {};
template <>
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
struct traits<const Similarity2> : public internal::MatrixLieGroup<Similarity2> {};
} // namespace gtsam

8
gtsam/geometry/Similarity3.h
View File

@ -174,10 +174,10 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
* @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<Similarity3>(const Vector& xi) {
}
#endif
template <>
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
struct traits<Similarity3> : public internal::MatrixLieGroup<Similarity3> {};
template <>
struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
struct traits<const Similarity3> : public internal::MatrixLieGroup<Similarity3> {};
} // namespace gtsam

2
gtsam/geometry/tests/testPose2.cpp
View File

@ -37,7 +37,7 @@ GTSAM_CONCEPT_LIE_INST(Pose2)
TEST(Pose2 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose2 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose2 >);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testPose3.cpp
View File

@ -45,7 +45,7 @@ static const double tol=1e-5;
TEST(Pose3 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Pose3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Pose3 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Pose3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Pose3 >);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -31,7 +31,7 @@ typedef traits<Q>::ChartJacobian QuaternionJacobian;
TEST(Quaternion , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsManifold<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Quaternion >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Quaternion >);
}
//******************************************************************************

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -40,7 +40,7 @@ static double error = 1e-9, epsilon = 0.001;
TEST(Rot3 , Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Rot3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Rot3 >);
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testSO3.cpp
View File

@ -38,7 +38,7 @@ TEST(SO3, Identity) {
TEST(SO3, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SO3>);
GTSAM_CONCEPT_ASSERT(IsManifold<SO3>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SO3>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO3>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSO4.cpp
View File

@ -44,7 +44,7 @@ TEST(SO4, Identity) {
TEST(SO4, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SO4>);
GTSAM_CONCEPT_ASSERT(IsManifold<SO4>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SO4>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SO4>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSOn.cpp
View File

@ -86,7 +86,7 @@ TEST(SOn, SO5) {
TEST(SOn, Concept) {
GTSAM_CONCEPT_ASSERT(IsGroup<SOn>);
GTSAM_CONCEPT_ASSERT(IsManifold<SOn>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<SOn>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<SOn>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSimilarity2.cpp
View File

@ -37,7 +37,7 @@ static const double s = 4;
TEST(Similarity2, Concepts) {
GTSAM_CONCEPT_ASSERT(IsGroup<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsManifold<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity2>);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity2>);
}
//******************************************************************************

2
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -56,7 +56,7 @@ const double degree = M_PI / 180;
TEST(Similarity3, Concepts) {
GTSAM_CONCEPT_ASSERT(IsGroup<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsManifold<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsLieGroup<Similarity3 >);
GTSAM_CONCEPT_ASSERT(IsMatrixLieGroup<Similarity3 >);
}
//******************************************************************************