Re: [eigen] Eigen and rigid body simulation

[Maxime Tournier <maxime.tournier@xxxxxxxxxxxx>]

On Friday 27 November 2009 14:28:29 Mathieu Gautier wrote:
> > If that happened, would you still want to have this stuff in Eigen or
> > would your rather use KDL for that? Since you were able to reformulate
> > above everything in terms of Lie algebra and without any mechanics, it
> > seems like this is now really interesting to have in Eigen,
> > independently of the particular use case you had in mind, but I also
> > don't want to do it if its main author (you) is going to not use it in
> > the future because KDL does it!
> 
> I think that these elements have to be in Eigen. Actually, I'm speaking
> with Maxime Tournier who had an interesting implementation of such
> elements. Basically, he has some classes describing Lie groups and Lie
> algebras and their operations. They are specialized for certain type :
> 
> struct Lie<Quaternion> 		  <- rotation3D
> struct Lie<Matrix<Scalar, 1, 7> > <- 3d direct isometries (maybe Matrix
> is not a good choice)
> 
> This approach is interesting, since it describes the group structure.
> 
> so, the definition I proposed may change to better reflect the
> geometrical aspect of these elements.
> 

Hi everyone,

As Mathieu said, I have some generic code that allows one to abstract
the Lie group structure of a type, so that one can write any Lie group
algorithm no matter the group. I told Gael about it few days ago, and
since it could be of interest to others, here's an overview of what I
did.

The approach somehow follows c++0x's concepts proposal, in that the
Lie group structure for a type T is encoded in a separate template
class, Lie<T>, containing all the types and operations that define the
Lie group structure.

One can describe the Lie group structure for a type T by specializing
the template Lie for this type, giving the associated types and
implementing required operations.  Every specialization should conform
to the same "interface".

Here are two basic examples with only the algebraic structure:

// here is S^3 with quaternion multiplication
template<class Real>
struct Lie< Quaternion<Real> > {

  static Quaternion<Real> id() { return Quaternion<Real>::Identity(); }
  static Quaternion<Real> inv(const Quaternion<Real>& q) { return q.inverse(); 
}
  static Quaternion<Real> comp(const Quaternion<Real>& q1,
   	 		       const Quaternion<Real>& q2) { 
    return Quaternion<Real>::Identity(); 
  }

};

// here is R^3 with addition
template<>
struct Lie< Vector3d > {

  static Vector3d id() { return Vector3d::Zero(); }
  static Vector3d inv(const Vector3d& v) { 
    return -v;
  }
  static Vector3d comp(const Vector3d& a, 
		       const Vector3d& b) { 
    return a + b; 		
  }

};

You can then write an algorithm for any kind of group like this:

template<class G>
G fast_exponentiation(const G& g, int n) {
  if( n == 0 ) {
    return Lie<G>::id();
  }
 
  if( n % 2 ) {
    return Lie<G>::comp(  fast_exponentiation( Lie<G>::comp(g, g), n/2), g );
  } else {
    return fast_exponentiation( Lie<G>::comp(g, g), n/2) );
  }

}

Now I know it's a bit tedious to write, but: 

- No modification to exisiting types is needed whatsoever. This is A
  Good Thing. No curious inheritance pattern is introduced in
  particular. Actually, underlying types are completely independant
  from this new structure. This should make Eigen types developers
  happy since we're not touching anything in existing Eigen code :-)

- We can quite easily construct new groups from existing ones: 

template<class G1, class G2>
struct Lie< std::pair<G1, G2> > {

  // ... 

};

....which essentialy says that the product of two groups is a group for
the product law. This is particularly useful when using std::tuple and
variadic templates, since product-groups are automatically constructed
at compilation.

The complete Lie template specification in my code is the following:

template<class G>
struct Lie {
  typedef some_type algebra;
  typedef some_type coalgebra;

  static G id();
  static G inv(const G&);
  static G comp(const G&, const G& );

  // default-constructible	
  typedef some_functor exp;
  typedef some_functor log;

  // G-constructible
  typedef some_functor ad;
  typedef some_functor ad_T;

};

The only problem with it is the identity for dynamic-sized types, for
example VectorXd: what should be the size of the result ? So maybe it
should accept some optional contextual hint for the dimension.

Once this is done, it is quite straightforward to implement the
tangent bundle, then differentiable functions, and then automatically
compute differentials for complicated function compositions using
expression templates. When dealing with complicated functions on
complicated non-flat spaces, this is a real joy to use.

I have the Lie group structure implemented for quaternions, Rohit
Garg's rigid transforms, row/column vectors, as well as any std::tuple
combination of these. Based on this I have code that can interpolate
(linear/~spline), compute geometric mean, and do statistical analysis
while preserving the Lie group (hence manifold) structure.

So if you need to spline interpolate homographies
in a non degenerate way for example, all you have to do is to
implement the Lie group structure for homographies and the algorithm
is already coded for you :-)

So my question is: would anyone be interested in me porting all/parts
of this stuff into Eigen ? If so where/how should I start ?

Let me finish this mail by thanking Eigen developers for this great
library.

Best regards,

-- 
Maxime