[eigen] Rigid Transformations in eigen: discussion thread

[Rohit Garg <rpg.314@xxxxxxxxx>]

I have written the rigid transform class and it's unit test. The code
compiles, and executes correctly. The API is as close to eigen's
transform class as possible. I hope this is good enough to make into
eigen. The documentation is sketchy, but it was secondary till now.
The unit tests are not eigen'ish, but it is trivial to add
VERIFY_IS_APPROX, and I didn't want to add that to my code base.

NOTE: If you remove the V1.normalize() and V2.normalize() calls,
eigen's results change. I spent a lot of brain cycles fighting this.
:(

It may be a good idea to keep the axis vector in the angleaxis class,
and the direction vector in the parametrizedline class normalized.

Comments/questions/cribs/criticism/commendation (!) welcome.

-- 
Rohit Garg

http://rpg-314.blogspot.com/

Senior Undergraduate
Department of Physics
Indian Institute of Technology
Bombay

#ifndef RIGID_TRANSFORM
#define RIGID_TRANSFORM
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<iostream>
using namespace Eigen;
template<typename _Scalar>
class RigidTransform
{
  public:
  /** the scalar type of the coefficients */
  typedef _Scalar Scalar;

  /** the type of the Coefficients 4-vector */
  typedef Matrix<Scalar, 4, 1> Coefficients;
  /** the type of a 3D vector */
  typedef Matrix<Scalar,3,1> VectorType;
  /** the equivalent rotation matrix type */
  typedef Matrix<Scalar,3,3> MatrixType;
  /** the equivalent angle-axis type */
  typedef AngleAxis<Scalar> AngleAxisType;
  /** the equivalent quaternion type */
  typedef Quaternion<Scalar> QuatType;
  /** the equivalent translation type */
  typedef Translation<Scalar, 3> TranslationType;  
  /** the equivalent line type */
  typedef ParametrizedLine<Scalar,3> LineType;
  /** the equivalent real data type */
  typedef typename NumTraits<Scalar>::Real RealScalar;
  
  protected:
    QuatType quats[2];
   
  inline RigidTransform()
	  {
	  }

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,8)

	inline RigidTransform(const RigidTransform &other)
  	{ 
	  quats[0]=other.quats[0];
  	quats[1]=other.quats[1];
	  }
  inline RigidTransform & operator=(const RigidTransform &other)
	  {
	  quats[0]=other.quats[0];
	  quats[1]=other.quats[1];
	  return *this;
	  }
  inline RigidTransform(const AngleAxisType& aa)
	  { 
	  VectorType axis_=aa.axis();
	  axis_.normalize();
	  quats[0]=AngleAxisType(aa.angle(), axis_);
//	  std::cout<<"\ndebugging angleaxis cons() "<<quats[0].coeffs();
	  quats[1].coeffs().setZero();
	  }
  inline RigidTransform(const TranslationType& delta)
	  {
	  quats[0].setIdentity();
	  quats[1].coeffs().template start<3>()=delta.vector()*(0.5f);
	  quats[1].w()=1.0f;
	  }
  inline RigidTransform(Scalar angle, const LineType& line)
	  {
	  VectorType dirn=line.direction();
	  dirn=dirn.normalize();
	  quats[0]=AngleAxisType(angle, dirn);
	  VectorType temp=sin(angle*0.5f) * line.through().cross(dirn);
	  quats[1]=QuatType(0.0f, temp.x(), temp.y(), temp.z());
	  }        
  inline RigidTransform operator+(const RigidTransform& other) const
	  {
	  RigidTransform temp;
	  temp.quats[0].coeffs()=quats[0].coeffs() + other.quats[0].coeffs();
	  temp.quats[1].coeffs()=quats[1].coeffs() + other.quats[1].coeffs();
	  return temp;
	  }	  
  inline RigidTransform operator*(const RigidTransform& other) const
	  {
	  RigidTransform temp;
	  temp.quats[0]=(quats[0] * other.quats[0]);
	  temp.quats[1].coeffs()=(quats[1] * other.quats[0]).coeffs() + (quats[0] * other.quats[1]).coeffs();
	  return temp;
	  }
  inline RigidTransform operator*(const AngleAxisType& aa) const
	  {
	  RigidTransform temp(aa);
	  return (*this)*temp;	  
	  }
  inline RigidTransform operator*(const TranslationType& delta) const
	  {
	  RigidTransform temp(delta);
	  return (*this)*temp;	  
	  }	  
  inline RigidTransform operator*(Scalar x) const
	  {
	  RigidTransform temp;
	  temp.quats[0].coeffs()=quats[0].coeffs() * x;
	  temp.quats[1].coeffs()=quats[1].coeffs() * x;
	  return temp;    
	  }
  MatrixType rotation() const
  	{
	  return quats[0].toRotationMatrix();
	  }
  VectorType translation() const
	  {
	  VectorType temp=(quats[1] * (quats[0].conjugate())).vec();
	  return temp*2.0f;
	  }
  void normalize()
	  {
	  RealScalar normalization_prim=quats[0].norm();
	  RealScalar inv_prim=1.0/normalization_prim;
	  RealScalar normalization_dual=quats[0].coeffs().dot(quats[1].coeffs())*inv_prim;
	  RealScalar inv_dual=-(normalization_dual)*inv_prim*inv_prim;
	  quats[0].coeffs()=inv_prim*quats[0].coeffs();
	  quats[1].coeffs()=(quats[0].coeffs()*inv_dual)+(quats[1].coeffs()*inv_prim);
	  } 
  inline static RigidTransform Identity() 
    { 
    RigidTransform temp;
    temp.quats[0].setIdentity();
    temp.quats[1].coeffs().setZero();
    return temp;
    }
  inline RigidTransform& setIdentity() 
    {
    this->quats[0].setIdentity();
    this->quats[1].coeffs().setZero();
    return *this; 
    }
	     
friend inline RigidTransform operator*(const AngleAxisType& aa, const RigidTransform& other)
  {
  RigidTransform temp(aa);
  return temp*other;
  }	  	 
friend inline RigidTransform operator*(const Scalar& x, const RigidTransform& other)
  {
  return other*x;
  }	  	 
friend inline RigidTransform operator*(const TranslationType& delta, const RigidTransform& other)
  {
  RigidTransform temp(delta);
  return temp*other;
  }	  

};
  
#endif

#include<RigidTransform.h>
#include<iostream>
using namespace std;
using namespace Eigen;
int main()
    {
    typedef RigidTransform<float> RigidTransformF;  	       
    typedef Translation<float,3> Trans;
    typedef AngleAxis<float> Rot;
    typedef Transform<float,3> Xform;
    Vector3f V1(1,2,3), V2(1,5,-0.5);
    Trans T1(V1);//pure translations
    Trans T2(V2);
//translation-translation test
    RigidTransformF RT1(T1),RT2(T2);//pure translates
    RigidTransformF RT3=RT1*RT2;
    Vector3f V3=V1+V2;
    cout<<"xlate-xlate test\n";
    cout<<V3<<endl<<RT3.translation();

//translation-rotation test

    cout<<"xlate-rotate test\n";
    V1.normalize();
    V2.normalize();
    Rot R1(1.1,V1),R2(0.8,V2);//pure rotations
    cout<<"\nsetting up R2\n";
    RigidTransformF RT4(R1);//a pure rotation
    cout<<"\nsetting up R2, In multiplication\n";
    RigidTransformF RT5=RT2*R2;
//eigen's xform
    Xform X1=T2*R2;
    cout<<"\n rot part\n";
    cout<<"\nrt\n"<<RT5.rotation()<<"\neigen\n"<<X1.rotation();
    cout<<"\nxlate part\n";
    cout<<"\nrt\n"<<RT5.translation()<<"\neigen\n"<<X1.translation();

//rotation-translation test
    cout<<"\nrotate-xlate test\n";
    RigidTransformF RT6=RT4*T2;
    Xform X2=R1*T2;
    cout<<"\n rot part\n";
    cout<<"\nrt\n"<<RT6.rotation()<<"\neigen\n"<<X2.rotation();
    cout<<"\nxlate part\n";
    cout<<"\nrt\n"<<RT6.translation()<<"\neigen\n"<<X2.translation();

//rotation-rotation test
    cout<<"\nrotate-rotate test\n";
    RigidTransformF RT7(R2);//another pure rotation
    RigidTransformF RT8=RT4*RT7;
    cout<<"\nsetting up R3, In multiplication\n";
    Xform X3(R1);
    Xform X4(R2);
    cout<<"\nrigid xform"<<RT8.rotation()<<"\nangle axis"<<(X3*X4).rotation();

    return 0;
    }