Eigen计算坐标系变换

#include 
#include 
#include 
#include 
#define Scalar double

 Eigen::Transform findTransformBetween2CS(const Eigen::Matrix from_line_x,
                                        const Eigen::Matrix from_line_y,
                                        const Eigen::Matrix to_line_x,
                                        const Eigen::Matrix to_line_y)
{

// Convert lines into Vector3 :
  Eigen::Matrix fr0 (from_line_x.template head<3>()),
                              fr1 (from_line_x.template head<3>() + from_line_x.template tail<3>()),
                              fr2 (from_line_y.template head<3>() + from_line_y.template tail<3>()),

                              to0 (to_line_x.template head<3>()),
                              to1 (to_line_x.template head<3>() + to_line_x.template tail<3>()),
                              to2 (to_line_y.template head<3>() + to_line_y.template tail<3>());

  // Code is inspired from http://stackoverflow.com/a/15277421/1816078
  // Define matrices and points :
  Eigen::Transform T, T1, T2 = Eigen::Transform::Identity ();
  Eigen::Matrix x1, y1, x2, y2;

    // 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
    T1.linear() << x1, y1, x1.cross(y1); 

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

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

    // T.translation() = to0;
  	T.linear () = T2.linear () * T1.linear ().inverse ();
  	// T.translation() = to0;
  	T.translation () = to0 - (T.linear () * fr0);
    return T;
}

int main(int argc, char const *argv[])
{
	Eigen::Transform transformation;
	Eigen::Matrix  from_line_x;
	Eigen::Matrix  from_line_y;
	Eigen::Matrix  to_line_x;
	Eigen::Matrix  to_line_y;

	from_line_x << 1234.56, 0.0, 0.0, 0.00387281179, 0.00572064891, -0.999976099;
	from_line_y << 1234.56, 0.0, 0.0, 0.6113801, -0.79133445, -0.00202810019;
	to_line_x << 0, 0, 0, 1, 0, 0;
	to_line_y << 0, 0, 0, 0, 1, 0;

	transformation = findTransformBetween2CS (from_line_x, from_line_y, to_line_x, to_line_y);
	std::cout<< "transformation" <

编译：

 g++ coordinate.cpp  -I/usr/include/eigen3 -L/opt/ros/kinetic/lib/ -leigen_conversions -std=c++11

输出：

transformation
 0.00395299  0.00561688   -0.999976     -4.8802
   0.611381   -0.791334 -0.00215923    -754.786
  -0.791327   -0.611358 -0.00656218     976.941
          0           0           0           1
from_test_x
1
1
2
 -1.99038
-0.184272
 -1.41581