MatrixLieGroup
parent
8cf3d04336
commit
091ec68838
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue