MatrixLieGroup

release/4.3a0
Frank Dellaert 2025-03-08 15:25:22 -05:00
parent 8cf3d04336
commit 091ec68838
2 changed files with 43 additions and 9 deletions

View File

@ -184,7 +184,6 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
/// @{
using ManifoldType = Class;
inline constexpr static auto dimension = Class::dimension;
using LieAlgebra = Class::LieAlgebra;
using TangentVector = Eigen::Matrix<double, dimension, 1>;
using ChartJacobian = OptionalJacobian<dimension, dimension>;
@ -223,19 +222,29 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
ChartJacobian H = {}) {
return m.inverse(H);
}
/// @}
};
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> {
using LieAlgebra = typename Class::LieAlgebra;
using TangentVector = typename LieGroupTraits<Class>::TangentVector;
static LieAlgebra Hat(const TangentVector& v) {
return Class::Hat(v);
}
static TangentVector Vee(const LieAlgebra& X) {
return Class::Vee(X);
}
/// @}
};
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
template<class Class> struct MatrixLieGroup: MatrixLieGroupTraits<Class>, Testable<Class> {};
} // \ namespace internal
@ -270,7 +279,6 @@ class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
public:
typedef typename traits<T>::structure_category structure_category_tag;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::LieAlgebra LieAlgebra;
typedef typename traits<T>::TangentVector TangentVector;
typedef typename traits<T>::ChartJacobian ChartJacobian;
@ -289,17 +297,32 @@ public:
// log and exponential map with Jacobians
g = traits<T>::Expmap(v, Hg);
v = traits<T>::Logmap(g, Hg);
// hat and vee
X = traits<T>::Hat(v);
v = traits<T>::Vee(X);
}
private:
T g, h;
LieAlgebra X;
TangentVector v;
ChartJacobian Hg, Hh;
};
/**
* Matrix Lie Group Concept
*/
template<typename T>
class IsMatrixLieGroup: public IsLieGroup<T> {
public:
typedef typename traits<T>::LieAlgebra LieAlgebra;
typedef typename traits<T>::TangentVector TangentVector;
BOOST_CONCEPT_USAGE(IsMatrixLieGroup) {
// hat and vee
X = traits<T>::Hat(xi);
xi = traits<T>::Vee(X);
}
private:
LieAlgebra X;
TangentVector xi;
};
/**
* Three term approximation of the Baker-Campbell-Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)

View File

@ -420,6 +420,8 @@ struct DynamicTraits {
/// @name Lie Group
/// @{
using LieAlgebra = Dynamic;
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
if (H) *H = Eye(m);
TangentVector result(GetDimension(m));
@ -450,6 +452,15 @@ struct DynamicTraits {
if (H2) *H2 = Eye(v1);
return v2 - v1;
}
static LieAlgebra Hat(const TangentVector& v) {
return v;
}
static TangentVector Vee(const LieAlgebra& X) {
return X;
}
/// @}
};