通过Eigen计算直线在不同坐标系统的转换

//
template <typename Scalar> bool
pcl::transformBetween2CoordinateSystems (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_x,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> from_line_y,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_x,
                                         const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> to_line_y,
                                         Eigen::Transform<Scalar, 3, Eigen::Affine> &transformation)
{
  if (from_line_x.innerSize () != 6 || from_line_y.innerSize () != 6 || to_line_x.innerSize () != 6 || to_line_y.innerSize () != 6)
  {
    PCL_DEBUG ("transformBetween2CoordinateSystems: lines size != 6\n");
    return (false);
  }

  // Check if coordinate systems are valid
  if (!pcl::checkCoordinateSystem (from_line_x, from_line_y) || !pcl::checkCoordinateSystem (to_line_x, to_line_y))
  {
    PCL_DEBUG ("transformBetween2CoordinateSystems: coordinate systems invalid !\n");
    return (false);
  }

  // Convert lines into Vector3 :
  Eigen::Matrix<Scalar, 3, 1> 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<Scalar, 3, Eigen::Affine> T2, T3 = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
  Eigen::Matrix<Scalar, 3, 1> 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);

  // Identity matrix = transform to CS2 to CS3
  // Note: if CS1 == CS2 --> transformation = T3
  transformation = Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity ();
  transformation.linear () = T3.linear () * T2.linear ().inverse ();
  transformation.translation () = to0 - (transformation.linear () * fr0);
  return (true);
}

测试代码：

//
TEST (PCL, transformBetween2CoordinateSystems)
{
  Eigen::Affine3f transformation;
  Eigen::Affine3d transformationd;
  Eigen::VectorXf from_line_x, from_line_y, to_line_x, to_line_y;
  Eigen::VectorXd from_line_xd, from_line_yd, to_line_xd, to_line_yd;
  from_line_x.resize(6); from_line_y.resize(6);
  to_line_x.resize(6); to_line_y.resize(6);
  from_line_xd.resize(6); from_line_yd.resize(6);
  to_line_xd.resize(6); to_line_yd.resize(6);

  Eigen::Matrix4d test;
  double tolerance = 1e-3;

  // Simple translation
  test << 1, 0, 0, 10,
  0, 1, 0, -5,
  0, 0, 1, 1,
  0, 0, 0, 1;

  from_line_x << 0, 0, 0, 1, 0, 0;
  from_line_y << 0, 0, 0, 0, 1, 0;
  to_line_x << 10, -5, 1, 1, 0, 0;
  to_line_y << 10, -5, 1, 0, 1, 0;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformation.matrix())(i,j) - test(i,j), tolerance);

  from_line_xd << 0, 0, 0, 1, 0, 0;
  from_line_yd << 0, 0, 0, 0, 1, 0;
  to_line_xd << 10, -5, 1, 1, 0, 0;
  to_line_yd << 10, -5, 1, 0, 1, 0;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_xd, from_line_yd, to_line_xd, to_line_yd, transformationd));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformationd.matrix())(i,j) - test(i,j), tolerance);

  // Simple rotation
  test << 0, 0, 1, 0,
  1, 0, 0, 0,
  0, 1, 0, 0,
  0, 0, 0, 1;

  from_line_x << 0, 0, 0, 1, 0, 0;
  from_line_y << 0, 0, 0, 0, 1, 0;
  to_line_x << 0, 0, 0, 0, 1, 0;
  to_line_y << 0, 0, 0, 0, 0, 1;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformation.matrix())(i,j) - test(i,j), tolerance);

  from_line_xd << 0, 0, 0, 1, 0, 0;
  from_line_yd << 0, 0, 0, 0, 1, 0;
  to_line_xd << 0, 0, 1, 0, 1, 0;
  to_line_yd << 0, 0, 1, 0, 0, 1;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_xd, from_line_yd, to_line_xd, to_line_yd, transformationd));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformationd.matrix())(i,j) - test(i,j), tolerance);

  // General case
  test << 0.00397405, 0.00563289, -0.999976, -4.9062,
  0.611348, -0.791359, -0.00213906, -754.746,
  -0.791352, -0.611325, -0.00658855, 976.972,
  0, 0, 0, 1;

  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;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_x, from_line_y, to_line_x, to_line_y, transformation));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformation.matrix())(i,j) - test(i,j), tolerance);

  from_line_xd << 1234.56, 0.0, 0.0, 0.00387281179, 0.00572064891, -0.999976099;
  from_line_yd << 1234.56, 0.0, 0.0, 0.6113801, -0.79133445, -0.00202810019;
  to_line_xd << 0, 0, 0, 1, 0, 0;
  to_line_yd << 0, 0, 0, 0, 1, 0;
  EXPECT_TRUE (pcl::transformBetween2CoordinateSystems (from_line_xd, from_line_yd, to_line_xd, to_line_yd, transformationd));

  for (int i = 0; i < 3; i++)
  for (int j = 0; j < 3; j++)
  EXPECT_LE ((transformationd.matrix())(i,j) - test(i,j), tolerance);
}

参考：
[1]. https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/common/impl/eigen.hpp#L944