CODEWITHSUNDEEP
c++geometryeigen
rmbianchi
rmbianchi

Reputation: 6521

how to find the transformation matrix of a change of basis with Eigen

I'm porting a program to Eigen.

Now I have to rewrite a method that returns the 3D transformation matrix from one coordinate system A, defined by its origin and two axes, to a second coordinate system, still defined by the origin and two axes.

I was wondering if there's a method in Eigen to find that matrix. I browsed the reference guide but I did not find any useful method yet...

More in details:

The method I'm porting to Eigen accepts 6 Points (vectors) (fr0, fr1, fr2, to0, to1, to2). "fr0" is the origin of CS1 (Coordinate System 1), "fr1" is a point defining an axis of the CS1, "fr2" is the point defining a second axis of CS1; "to0" is the origin of CS2, and so on...

Upvotes: 2

Views: 6725

Answers (2)

rmbianchi
rmbianchi

Reputation: 6521

Ok, I found the solution and I post it here for reference. I hope it can be useful to someone else as well.

Actually ggael's answer triggered the right solution, so many thanks to him and +1 for him.

#include <Eigen/Geometry>

typedef Eigen::Affine3d Transformation;
typedef Eigen::Vector3d   Point;
typedef Eigen::Vector3d  Vector;
typedef Eigen::Translation<double,3>  Translation;

Transformation findTransformBetween2CS(Point fr0,Point fr1,Point fr2,Point to0,Point to1,Point to2) {

  Transformation T, T2, T3 = Transformation::Identity();
  Vector3d x1,y1,z1, x2,y2,z2;

  // Axes of the coordinate system "fr"
  x1 = (fr1 - fr0).normalized(); // the versor (unitary vector) of the (fr1-fr0) axis vector
  y1 = (fr2 - fr0).normalized();

  // Axes of the coordinate system "to"
  x2 = (to1 - to0).normalized();
  y2 = (to2 - to0).normalized();

  // transform from CS1 to CS2 
  // Note: if fr0==(0,0,0) --> CS1==CS2 --> T2=Identity
  T2.linear() << x1, y1, x1.cross(y1); 

  // transform from CS1 to CS3
  T3.linear() << x2, y2, x2.cross(y2); 

  // T = transform to CS2 to CS3
  // Note: if CS1==CS2 --> T = T3
  T.linear() = T3.linear() * T2.linear().inverse(); 


  T.translation() = t0;

  return T;

}

Inspiration also came from this post:

https://gamedev.stackexchange.com/questions/26084/transform-between-two-3d-cartesian-coordinate-systems

Upvotes: 7

ggael
ggael

Reputation: 29205

No need for a special function for that, just use the comma initializer:

Matrix4f M;
M << X, Y, X.cross(Y), O,
     0, 0, 0,          1;

This assume the two axes X and Y are unitary and orthogonal. O is the origin.

You might also look at the geometry module for more advanced space transformation classes and functions, like the Transform<> class. Here is the same exemple using the Affine3f typedef instead of a raw matrix:

Affine3f M;
M.linear() << X, Y, X.cross(Y);
M.translation(O);

Upvotes: 4

Related Questions