Hello,

I'm trying to find the change of basis between two known coordinates systems.

This is what I started with:
http://stackoverflow.com/questions/1525 ... 1#15277421

Code: Select all

#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;
}

This line makes no sense because t0 does not exist; what should be here ?

Code: Select all

  T.translation () = t0;

I tried this but it doesn't seem to be right:

Code: Select all

  T.translation () = to0 - fr0;

If I apply the rotation only (or rotation+translation) the axes are aligned but the translation is wrong.
If I apply the translation only, the 2 origins matches but the rotation is of course wrong because we did not rotate the base.

Thanks for reading me,
Bye

After a good nap I found the answer by myself

This is the distance between the two origins before transformation. It is wrong because after rotating the axes "fr0" (the origin) has moved.

Code: Select all

T.translation () = to0 - fr0;

So the distance should be computed with the rotated fr0 point :

Code: Select all

T.translation () = to0 - (transformation_m.linear () * fr0);

Bye !

EDIT: My implementation within the Point Cloud Library:
https://github.com/PointCloudLibrary/pc ... 094810e271