| Re: [eigen] Eigen and rigid body simulation |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
Hi,
I have a working prototype with LieGroup<Quaternion> and
LieGroup<Matrix<1,7> >, but I'm not yet satisfied. Besides, several
names (for example the get function) should be renamed. The code is in
the attached file.
I choose to use partial specialization, so I have a main class which
methods call static methods from a structures.
For example :
template<class GG> EIGEN_STRONG_INLINE ReturnVal operator*(const
LieGroup<GG>& other) const {
return ei_lie_composition<ReturnVal>::run(this->get(), other.get());
}
template<class G> struct ei_lie_composition{
template<class GG> static EIGEN_STRONG_INLINE typename
LieGroup<G>::ReturnVal run(const G& a, const GG& b)
{
return LieGroup<G>::ReturnVal(a * b);
}
};
I use the property ReturnVal of the traits classes to manipulate
LieGroup<G> and LieGroup<Map<G> > together. For example, the return type
(ReturnVal) of Quaternion<Scalar> and Map<Quaternion<Scalar>,
PacketAccess> are Quaternion<Scalar>. It's the type of temporary that
will be created during the composition operation. That's why I have to
use the following template, with classes G and GG :
template<class G> struct ei_lie_composition{
template<class GG> static EIGEN_STRONG_INLINE typename
LieGroup<G>::ReturnVal run(const G& a, const GG& b)
{
return LieGroup<G>::ReturnVal(a * b);
}
};
so G and GG could be either Quaternion or Map<Quaternion>.
So i can then add some typedef :
typedef Eigen::LieGroup<Eigen::Quaterniond> Rotd;
typedef Eigen::LieGroup<Eigen::Map<Eigen::Quaterniond, 0> > RotMapd;
and I then use only one class, Rotd and the mapped one, RotMapd
I have several (at least 4) points to clarify (that's why I'm not
satisfied) :
1) I think that I have to used derived class instead of typedef to add
new constructor, for example a
Rotd(const scalar, const scalar, const scalar, const scalar);
instead using
LieGroup<Quaternion<Scalar> >((Quaternion<Scalar>(const scalar, const
scalar, const scalar, const scalar));
or additionnal methods, such as getTranslation or getRotation for the
SE(3) element (LieGroup<Matrix<Scalar, 1, 7> and LieGroup<Map...>)
2) I have some inlining problem with LieGroup<Matrix<Scalar, 1, 7> >
when I use a simpler class, there are less call in the generated assembly
3) I have issues with some operations, for example :
getTranslation(res) = getQuaternion(a) * getTranslation(b);
getTranslation(res) += getTranslation(a);
works where :
getTranslation(res) = getQuaternion(a) * getTranslation(b) +
getTranslation(a);
give a YOU_MIX_DIFFERENT_SIZE assert, although all types are
Matrix<Scalar, 1, 3> and the multiplication return a Matrix<Scalar, 1, 3>
4) I feel that is a little bit over designed or too complicated.
--
Mathieu
namespace Eigen{
// forward declaration
template<class G> struct LieGroup;
// dummy traits declaration (optionnal)
template<class G>
struct ei_traits<LieGroup<G> >
{
typedef typename ei_traits<G>::Scalar Scalar;
typedef Matrix<typename ei_traits<G>::Scalar,0,0> Algebra; // Lie algebra associatet to the Lie Group
typedef Matrix<typename ei_traits<G>::Scalar,0,0> Coalgebra; // Lie coalgebra associatet to the Lie Group
typedef LieGroup<G> ReturnVal; // Return type or unmapped value, when G = Map<G'>, return type has to be LieGroup<G'>
};
// Lie Group class
template<class G>
struct LieGroup {
// aliases
typedef typename ei_traits<G>::Scalar Scalar;
typedef typename ei_traits<LieGroup<G> >::Algebra Algebra;
typedef typename ei_traits<LieGroup<G> >::Coalgebra Coalgebra;
typedef typename ei_traits<LieGroup<G> >::ReturnVal ReturnVal;
typedef typename ei_traits<LieGroup<G> >::AdjMatrix AdjMatrix;
// inverse
ReturnVal inverse() const {
return ei_lie_inverse<G>::run(this->get());
}
// identity
static ReturnVal Identity() {
return ei_lie_identity<G>::run();
}
// composition
template<class GG> EIGEN_STRONG_INLINE ReturnVal operator*(const LieGroup<GG>& other) const {
return ei_lie_composition<ReturnVal>::run(this->get(), other.get());
}
// assignation
template<class GG> EIGEN_STRONG_INLINE LieGroup& operator=(const LieGroup<GG>& other){
m_element = other.get();
return *this;
}
AdjMatrix adjoint(void) const {return ei_lie_adjoint<ReturnVal>::run(this->get());}
AdjMatrix transposedAdjoint(void) const {return ei_lie_adjoint<ReturnVal>::run(this->get()).transpose();} // a distinct function will be faster
// log : map from the group to the algebra
Algebra log(const Scalar precision = 1e-6) const{return ei_lie_log<ReturnVal>::run(this->get(), precision);} // ReturnVal is used to allow log working with both G and Map<G>
// constructors
template<class GG> LieGroup(const GG& el) : m_element(el) {}; // /!\ [XXX] handle properly the map case
LieGroup(const double* data) : m_element(data){};
LieGroup(){};
// accessor to the wrapped element
G& get(){return m_element;}
const G& get() const {return m_element;}
protected:
G m_element;
};
//***** generic functions on Lie Group
template<class G> struct ei_lie_identity{
static inline typename LieGroup<G>::ReturnVal run(){
return LieGroup<G>::ReturnVal(G::Identity());
}
};
template<class G> struct ei_lie_inverse{
static inline typename LieGroup<G>::ReturnVal run(const G& g){
return LieGroup<G>::ReturnVal(g.inverse());
}
};
template<class G> struct ei_lie_composition{
template<class GG> static EIGEN_STRONG_INLINE typename LieGroup<G>::ReturnVal run(const G& a, const GG& b)
{
return LieGroup<G>::ReturnVal(a * b);
}
};
template<class G> struct ei_lie_log{
};
template<class G> struct ei_lie_adjoint{
};
//**************** specialization
// specialization : quaternion : rotation in 3D
template<typename Derived>
struct ei_traits<LieGroup<QuaternionBase<Derived> > >{
typedef typename ei_traits<Derived>::Scalar Scalar;
typedef Matrix<Scalar,1,3> Algebra;
typedef Matrix<Scalar,1,3> Coalgebra;
typedef Quaternion<Scalar> ReturnVal;
typedef Matrix<Scalar, 3, 3> AdjMatrix;
};
template<typename Scalar>
struct ei_traits<LieGroup<Quaternion<Scalar> > > : public ei_traits<LieGroup<QuaternionBase<Quaternion<Scalar> > > >
{
};
template<typename Scalar, int PacketAccess>
struct ei_traits<LieGroup<Map<Quaternion<Scalar>, PacketAccess> > > : public ei_traits<LieGroup<QuaternionBase<Map<Quaternion<Scalar>, PacketAccess> > > >
{
};
template<typename Scalar> struct ei_lie_log<Quaternion<Scalar> >
{
typedef typename ei_traits<LieGroup<Quaternion<Scalar> > >::Algebra Algebra;
template<class Derived> static Algebra run(const QuaternionBase<Derived>& q, const Scalar precision){
const Scalar n2 = q.vec().squaredNorm();
const Scalar n = ei_sqrt(n2);
if (n < precision)
return Algebra((2 / q.w()) * q.vec());
else
return Algebra(ei_atan2(2 * n * q.w(), q.w() * q.w() - n2) / n * q.vec());
}
};
template<typename Scalar> struct ei_lie_adjoint<Quaternion<Scalar> >
{
typedef typename ei_traits<LieGroup<Quaternion<Scalar> > >::AdjMatrix AdjMatrix;
template<class Derived> static inline AdjMatrix run(const QuaternionBase<Derived>& q){
return q.toRotationMatrix();
}
};
//specialization : Frame / Displacement : rotation and translation in 3D
// 1) helper
template<class Derived> inline Map<Matrix<typename ei_traits<Derived>::Scalar, 1, 3> > getTranslation(const MatrixBase<Derived>& d){
return Map<Matrix<typename ei_traits<Derived>::Scalar, 1, 3> >(d.derived().data() + 4);
}
template<class Derived> inline LieGroup<Map<Quaternion<typename ei_traits<Derived>::Scalar> > > getRotation(const MatrixBase<Derived>& d){
return LieGroup<Map<Quaternion<typename ei_traits<Derived>::Scalar> > >(d.derived().data());
}
template<class Derived> inline Map<Quaternion<typename ei_traits<Derived>::Scalar> > getQuaternion(const MatrixBase<Derived>& d){
return Map<Quaternion<typename ei_traits<Derived>::Scalar> >(d.derived().data());
}
// 2) specialization
template<typename _Scalar>
struct ei_traits<LieGroup<Matrix<_Scalar, 1, 7> > >{
typedef _Scalar Scalar;
typedef Matrix<Scalar, 1, 6> Algebra;
typedef Matrix<Scalar, 1, 6> Coalgebra;
typedef Matrix<Scalar, 1, 7> ReturnVal;
typedef Matrix<Scalar, 6, 6> AdjMatrix;
};
template<typename Scalar>
struct ei_traits<LieGroup<Map<Matrix<Scalar, 1, 7> > > > : ei_traits<LieGroup<Matrix<Scalar, 1, 7> > >{
};
// identity
template<typename Scalar> struct ei_lie_identity<Matrix<Scalar, 1, 7>>{
static inline typename LieGroup<Matrix<Scalar, 1, 7> >::ReturnVal run(){
return (LieGroup<Matrix<Scalar, 1, 7> >::ReturnVal() << 0,0,0,1,0,0,0);
}
};
// inverse
template<typename Scalar> struct ei_lie_inverse<Matrix<Scalar, 1, 7> >{
template<class Derived> static inline typename LieGroup<Matrix<Scalar, 1, 7> >::ReturnVal run(const MatrixBase<Derived>& d){
LieGroup<Matrix<Scalar, 1, 7> >::ReturnVal res;
getRotation(res) = getRotation(d).inverse(); // rotation : rot^-1
getTranslation(res) = -getRotation(res) * getTranslation(d); // translation : -rot^-1*trans
return res;
}
};
// composition :
// [ R1 p1 ] * [ R2 p2 ] = [ R1*R2 R1*p2 + p1 ]
// [ 0 1 ] [ 0 1 ] [ 0 1 ]
//
template<typename Scalar> struct ei_lie_composition<Matrix<Scalar, 1, 7> >{
template<class Derived, class OtherDerived >static EIGEN_STRONG_INLINE typename LieGroup<Matrix<Scalar,1,7> >::ReturnVal run(const MatrixBase<Derived>& a, const MatrixBase<OtherDerived>& b)
{
LieGroup<Matrix<Scalar, 1, 7>>::ReturnVal res;
getRotation(res) = getRotation(a) * getRotation(b); // rot1 * rot2;
//Matrix<Scalar, 1, 3>::Map(res.derived().data()) = Map<Quaternion<Scalar> >(a.derived().data()) * Matrix<Scalar, 1, 3>::Map(b.derived().data()) + Matrix<Scalar, 1, 3>::Map(b.derived().data());
getTranslation(res) = getQuaternion(a) * getTranslation(b); // + getTranslation(a); [XXX] ? ASSERT : YOU_MIX_DIFFERENT_SIZE ?
getTranslation(res) += getTranslation(a); //rot1 * trans2 + trans1
return res;
}
};
// log SE(3) -> se(3)
//
// log [ R p ] = [ wtilde B*p ]
// [ 0 1 ] [ 0 0 ]
//
// wtilde = log R
// B = I - 0.5*wtilde + (2*sin||w||-||w||(1+cos||w||))/(2*||w||^2*sin||w||) * wtilde^2
//
template<typename Scalar> struct ei_lie_log<Matrix<Scalar, 1, 7> >
{
typedef typename ei_traits<LieGroup<Matrix<Scalar, 1, 7> > >::Algebra Algebra;
typedef typename Matrix<Scalar, 1, 3> Vector3;
template<class Derived> static Algebra run(const MatrixBase<Derived>& m, const Scalar precision){
LieGroup<Quaternion<Scalar> >::Algebra ang = getRotation(m).log(precision);
const Scalar n2 = ang.squaredNorm(); // ||wtidle||^2
Algebra al; // [XXX]
if(n2 < precision){
al << ang, getTranslation(m); // [XXX]
return al; // return a matrix<Scalar, 1, 6> : first 3 : angle, last 3 : translation vector
}
else{
const Scalar n = ei_sqrt(n2);
const Scalar sn = ei_sin(n);
Scalar val = (2.0 * sn - n * (1.0 + ei_cos(n))) / (2.0 *n2 * sn);
Map<Vector3> mv = getTranslation(m);
Vector3 lin = -0.5*ang.cross(mv);
lin += (1. - val * n2 ) * mv;
lin += val * ang.dot(mv) * ang;
al << ang, lin;
return al;
}
}
};
/* template<class G> struct ei_lie_adjoint{
TODO
};*/
}
// Some example of typedef
typedef Eigen::LieGroup<Eigen::Quaterniond> Rotd;
typedef Eigen::LieGroup<Eigen::Map<Eigen::Quaterniond, 0> > RotMapd;
typedef Eigen::LieGroup<Eigen::Matrix<double, 1, 7> > Displd;