Add public eigen conversion functions.

fizyr · Jul 29, 2016 · 640c978 · 640c978
1 parent c2ade63
commit 640c978
Show file tree

Hide file tree

Showing 4 changed files with 176 additions and 29 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -29,8 +29,10 @@ add_library(${PROJECT_NAME}
 
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ${orocos_kdl_LIBRARIES})
 
-dr_add_gtest(kdl test/kdl.cpp)
-target_link_libraries(${PROJECT_NAME}_test_kdl ${PROJECT_NAME} ${catkin_LIBRARIES})
+dr_add_gtest(kdl   test/kdl.cpp)
+dr_add_gtest(eigen test/eigen.cpp)
+target_link_libraries(${PROJECT_NAME}_test_kdl   ${PROJECT_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_test_eigen ${PROJECT_NAME} ${catkin_LIBRARIES})
 
 install(
 	TARGETS "${PROJECT_NAME}"

diff --git a/include/dr_kdl/eigen.hpp b/include/dr_kdl/eigen.hpp
@@ -0,0 +1,57 @@
+#include <kdl/frames.hpp>
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+namespace dr {
+
+/// Convert a KDL vector to an Eigen vector.
+Eigen::Vector3d toEigen(KDL::Vector const & input) {
+	return Eigen::Vector3d{input[0], input[1], input[2]};
+}
+
+/// Convert an Eigen vector to a KDL vector.
+KDL::Vector toKdlVector(Eigen::Vector3d const & input) {
+	return KDL::Vector{input.x(), input.y(), input.z()};
+}
+
+/// Convert a KDL rotation to an Eigen quaternion.
+Eigen::Quaterniond toEigen(KDL::Rotation const & input) {
+	Eigen::Quaterniond result;
+	input.GetQuaternion(result.x(), result.y(), result.z(), result.w());
+	return result;
+}
+
+/// Convert a KDL rotation to an Eigen rotation matrix.
+Eigen::Matrix3d toEigenMatrix(KDL::Rotation const & input) {
+	return (Eigen::Matrix3d{} <<
+		input(0, 0),  input(0, 1), input(0, 2),
+		input(1, 0),  input(1, 1), input(1, 2),
+		input(2, 0),  input(2, 1), input(2, 2)
+	).finished();
+}
+
+/// Convert an Eigen quaternion to a KDL rotation.
+KDL::Rotation toKdlRotation(Eigen::Quaterniond const & input) {
+	return KDL::Rotation::Quaternion(input.x(), input.y(), input.z(), input.w());
+}
+
+/// Convert an Eigen rotation matrix to a KDL rotation.
+KDL::Rotation toKdlRotation(Eigen::Matrix3d const & input) {
+	KDL::Rotation result;
+	for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) {
+		result(i, j) = input(i, j);
+	}
+	return result;
+}
+
+/// Convert a KDL frame to an Eigen isometry.
+Eigen::Isometry3d toEigen(KDL::Frame const & input) {
+	return Eigen::Translation3d(toEigen(input.p)) * toEigen(input.M);
+}
+
+/// Convert an Eigen isometry to a KDL frame.
+KDL::Frame toKdlFrame(Eigen::Isometry3d const & input) {
+	return {toKdlRotation(input.rotation()), toKdlVector(Eigen::Vector3d{input.translation()})};
+}
+
+}
diff --git a/src/dr_kdl.cpp b/src/dr_kdl.cpp
@@ -4,36 +4,10 @@
 #include <kdl_parser/kdl_parser.hpp>
 
 #include "dr_kdl.hpp"
+#include "eigen.hpp"
 
 namespace dr {
 
-namespace {
-	/// Create an Eigen transform from a KDL Frame.
-	Eigen::Isometry3d toEigen(KDL::Frame const & frame) {
-		Eigen::Isometry3d result;
-
-		// Rotation.
-		for (int i = 0; i < 3; ++i) {
-			for (int j = 0; j < 3; ++j) {
-				result(i, j) = frame.M(i, j);
-			}
-		}
-
-		// Translation.
-		for (int i = 0; i < 3; ++i) {
-			result(i, 3) = frame.p(i);
-		}
-
-		// Homogenous bit.
-		result(3, 0) = 0;
-		result(3, 1) = 0;
-		result(3, 2) = 0;
-		result(3, 3) = 1;
-
-		return result;
-	}
-}
-
 /// Get a the pose of the end frame relative to the start frame of a chain.
 Eigen::Isometry3d getPose(KDL::Chain const & chain) {
 	KDL::Frame transform = KDL::Frame::Identity();

diff --git a/test/eigen.cpp b/test/eigen.cpp
@@ -0,0 +1,114 @@
+#include "eigen.hpp"
+
+#include <dr_util/geometry.hpp>
+#include <dr_eigen/eigen.hpp>
+
+#include <gtest/gtest.h>
+#include <string>
+
+
+int main(int argc, char * * argv) {
+	testing::InitGoogleTest(&argc, argv);
+	return RUN_ALL_TESTS();
+}
+
+namespace dr {
+
+std::string toString(Eigen::Vector3d const & q) {
+	return "[" + std::to_string(q.w())  + ", " + std::to_string(q.x()) + ", " + std::to_string(q.y()) + ", " + std::to_string(q.z()) + "]";
+}
+
+std::string toString(Eigen::Quaterniond const & q) {
+	return "[" + std::to_string(q.w())  + ", " + std::to_string(q.x()) + ", " + std::to_string(q.y()) + ", " + std::to_string(q.z()) + "]";
+}
+
+std::string toString(Eigen::Matrix3d const & q) {
+	return toString(Eigen::Quaterniond(q));
+}
+
+std::string toString(Eigen::Isometry3d const & q) {
+	return "{" + toString(Eigen::Vector3d{q.translation()}) + " * " + toString(q.rotation()) + "}";
+}
+
+std::string toString(KDL::Vector const & v) {
+	return "[" + std::to_string(v.x()) + ", " + std::to_string(v.y()) + ", " + std::to_string(v.z()) + "]";
+}
+
+std::string toString(KDL::Rotation const & q) {
+	double w, x, y, z;
+	q.GetQuaternion(x, y, z, w);
+	return "[" + std::to_string(w)  + ", " + std::to_string(x) + ", " + std::to_string(y) + ", " + std::to_string(z) + "]";
+}
+
+std::string toString(KDL::Frame const & q) {
+	return "{" + toString(q.p) + " * " + toString(q.M) + "}";
+}
+
+TEST(EigenConversions, vector) {
+	ASSERT_EQ(toEigen(KDL::Vector(0, 1, 2)), Eigen::Vector3d(0, 1, 2));
+	ASSERT_NE(toEigen(KDL::Vector(0, 1, 2)), Eigen::Vector3d(0, 1, 3));
+	ASSERT_EQ(toKdlVector(Eigen::Vector3d(0, 1, 2)), KDL::Vector(0, 1, 2));
+	ASSERT_NE(toKdlVector(Eigen::Vector3d(0, 1, 2)), KDL::Vector(0, 1, 3));
+}
+
+testing::AssertionResult testVector(Eigen::Vector3d const & eigen, KDL::Vector const & kdl) {
+	Eigen::Vector3d eigen_converted = toEigen(kdl);
+	KDL::Vector     kdl_converted   = toKdlVector(eigen);
+	if (!eigen_converted.isApprox(eigen)) return testing::AssertionFailure() << "toEigen(kdl) " << toString(eigen_converted) << " does not match eigen " << toString(eigen);
+	if (!KDL::Equal(kdl_converted, kdl))  return testing::AssertionFailure() << "toKDL(eigen) " << toString(kdl_converted)   << " does not match kdl " << toString(kdl);
+	return testing::AssertionSuccess() << "eigen and kdl values convert to eachother";
+}
+
+testing::AssertionResult testRotation(Eigen::Quaterniond const & eigen, KDL::Rotation const & kdl) {
+	Eigen::Quaterniond eigen_converted = toEigen(kdl);
+	KDL::Rotation      kdl_converted   = toKdlRotation(eigen);
+	if (!eigen_converted.isApprox(eigen)) return testing::AssertionFailure() << "toEigen(kdl) " << toString(eigen_converted) << " does not match eigen " << toString(eigen);
+	if (!KDL::Equal(kdl_converted, kdl))  return testing::AssertionFailure() << "toKDL(eigen) " << toString(kdl_converted)   << " does not match kdl " << toString(kdl);
+	return testing::AssertionSuccess() << "eigen and kdl values convert to eachother";
+}
+
+testing::AssertionResult testRotation(Eigen::AngleAxisd const & eigen, KDL::Rotation const & kdl) {
+	return testRotation(Eigen::Quaterniond(eigen), kdl);
+}
+
+testing::AssertionResult testRotation(Eigen::Matrix3d const & eigen, KDL::Rotation const & kdl) {
+	Eigen::Matrix3d eigen_converted = toEigenMatrix(kdl);
+	KDL::Rotation   kdl_converted   = toKdlRotation(eigen);
+	if (!eigen_converted.isApprox(eigen)) return testing::AssertionFailure() << "toEigen(kdl) " << toString(eigen_converted) << " does not match eigen " << toString(eigen);
+	if (!KDL::Equal(kdl_converted, kdl))  return testing::AssertionFailure() << "toKDL(eigen) " << toString(kdl_converted)   << " does not match kdl " << toString(kdl);
+	return testing::AssertionSuccess() << "eigen and kdl values convert to eachother";
+}
+
+testing::AssertionResult testIsometry(Eigen::Isometry3d const & eigen, KDL::Frame const & kdl) {
+	Eigen::Isometry3d eigen_converted = toEigen(kdl);
+	KDL::Frame        kdl_converted   = toKdlFrame(eigen);
+	if (!eigen_converted.isApprox(eigen)) return testing::AssertionFailure() << "toEigen(kdl) " << toString(eigen_converted) << " does not match eigen " << toString(eigen);
+	if (!KDL::Equal(kdl_converted, kdl))  return testing::AssertionFailure() << "toKDL(eigen) " << toString(kdl_converted)   << " does not match kdl " << toString(kdl);
+	return testing::AssertionSuccess() << "eigen and kdl values convert to eachother";
+}
+
+TEST(EigenConversions, quaternion) {
+	ASSERT_TRUE(testRotation(Eigen::Quaterniond::Identity(), KDL::Rotation::Identity()));
+	ASSERT_TRUE(testRotation(rotateX(1), KDL::Rotation::RotX(1)));
+	ASSERT_TRUE(testRotation(rotateY(2), KDL::Rotation::RotY(2)));
+	ASSERT_TRUE(testRotation(rotateZ(3), KDL::Rotation::RotZ(3)));
+
+	// TODO: Why does this one fail?
+	//ASSERT_TRUE(testRotation(rotateX(1) * rotateY(2) * rotateZ(3), KDL::Rotation::RotX(1) * KDL::Rotation::RotY(2) * KDL::Rotation::RotZ(3)));
+}
+
+TEST(EigenConversions, matrix) {
+	ASSERT_TRUE(testRotation(Eigen::Matrix3d::Identity(), KDL::Rotation::Identity()));
+	ASSERT_TRUE(testRotation(rotateX(1).matrix(), KDL::Rotation::RotX(1)));
+	ASSERT_TRUE(testRotation(rotateY(2).matrix(), KDL::Rotation::RotY(2)));
+	ASSERT_TRUE(testRotation(rotateZ(3).matrix(), KDL::Rotation::RotZ(3)));
+	ASSERT_TRUE(testRotation((rotateX(1) * rotateY(2) * rotateZ(3)).matrix(), KDL::Rotation::RotX(1) * KDL::Rotation::RotY(2) * KDL::Rotation::RotZ(3)));
+}
+
+TEST(EigenConversions, isometry) {
+	ASSERT_TRUE(testIsometry(Eigen::Isometry3d::Identity(), KDL::Frame::Identity()));
+	ASSERT_TRUE(testIsometry(translate(0, 1, 2) * rotateZ(3), KDL::Frame(KDL::Rotation::RotZ(3), KDL::Vector(0, 1, 2))));
+	ASSERT_TRUE(testIsometry(translate(0, 1, 2) * rotateX(1) * rotateY(2) * rotateZ(3), KDL::Frame(KDL::Rotation::RotX(1) * KDL::Rotation::RotY(2) * KDL::Rotation::RotZ(3), KDL::Vector(0, 1, 2))));
+}
+
+}