Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rework of eigen features in common #660

Merged
merged 19 commits into from
May 24, 2014
Merged
Changes from 1 commit
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Added transformBetween2CoordinateSystems + test unit
  • Loading branch information
VictorLamoine committed May 22, 2014
commit 2ff5b6ae7fc0c3aeba6b9f1e3f4bee094810e271
39 changes: 39 additions & 0 deletions common/include/pcl/common/eigen.h
Original file line number Diff line number Diff line change
@@ -672,6 +672,45 @@ namespace pcl
return (checkCoordinateSystem<float> (line_x, line_y, norm_limit, dot_limit));
}

/** \brief Compute the transformation between two coordinate systems
* \param[in] from_line_x X axis from the origin coordinate system
* \param[in] from_line_y Y axis from the origin coordinate system
* \param[in] to_line_x X axis from the destination coordinate system
* \param[in] to_line_y Y axis from the destination coordinate system
* \param[out] transformation the transformation matrix to fill
* \return true if transformation was filled, false otherwise.
*
* Line must be filled in this form:\n
* line[0-2] = Coordinate system origin coordinates \n
* line[3-5] = Direction vector (norm doesn't matter)
*/
template <typename Scalar> bool
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);

inline bool
transformBetween2CoordinateSystems (const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_x,
const Eigen::Matrix<double, Eigen::Dynamic, 1> from_line_y,
const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_x,
const Eigen::Matrix<double, Eigen::Dynamic, 1> to_line_y,
Eigen::Transform<double, 3, Eigen::Affine> &transformation)
{
return (transformBetween2CoordinateSystems<double> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
}

inline bool
transformBetween2CoordinateSystems (const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_x,
const Eigen::Matrix<float, Eigen::Dynamic, 1> from_line_y,
const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_x,
const Eigen::Matrix<float, Eigen::Dynamic, 1> to_line_y,
Eigen::Transform<float, 3, Eigen::Affine> &transformation)
{
return (transformBetween2CoordinateSystems<float> (from_line_x, from_line_y, to_line_x, to_line_y, transformation));
}

}

#include <pcl/common/impl/eigen.hpp>
58 changes: 58 additions & 0 deletions common/include/pcl/common/impl/eigen.hpp
Original file line number Diff line number Diff line change
@@ -939,5 +939,63 @@ pcl::checkCoordinateSystem (const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &line
return (true);
}

//////////////////////////////////////////////////////////////////////////////////////////
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);
}

#endif //PCL_COMMON_EIGEN_IMPL_HPP_

94 changes: 94 additions & 0 deletions test/common/test_eigen.cpp
Original file line number Diff line number Diff line change
@@ -1014,6 +1014,100 @@ TEST (PCL, checkCoordinateSystem)
EXPECT_TRUE (pcl::checkCoordinateSystem (origind, vector_xd, vector_yd, 1e-7, 5e-3));
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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);
}

/* ---[ */
int
main (int argc, char** argv)