TEST(rosToEigen, pose) {
Eigen::Isometry3d pose1 = toEigen(makePose(makePoint(0, 1.5, 2), makeQuaternion(1, 0, 0, 0)));
Eigen::Isometry3d pose2 = toEigen(makePose(makePoint(-1.5, -2.6, -3.7), makeQuaternion(0, 0, 0, 1)));
Eigen::Vector3d const & p1 = pose1.translation();
Eigen::Vector3d const & p2 = pose2.translation();
Eigen::Quaterniond const & q1 = Eigen::Quaterniond{pose1.rotation()};
Eigen::Quaterniond const & q2 = Eigen::Quaterniond{pose2.rotation()};
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
ASSERT_NEAR(1, q1.x(), 1e-5);
ASSERT_NEAR(0, q1.y(), 1e-5);
ASSERT_NEAR(0, q1.z(), 1e-5);
ASSERT_NEAR(0, q1.w(), 1e-5);
ASSERT_NEAR(0, q2.x(), 1e-5);
ASSERT_NEAR(0, q2.y(), 1e-5);
ASSERT_NEAR(0, q2.z(), 1e-5);
ASSERT_NEAR(1, q2.w(), 1e-5);
}
TEST(rosToEigen, transform) {
Eigen::Isometry3d transform1 = toEigen(makeTransform(makeVector3(0, 1.5, 2), makeQuaternion(1, 0, 0, 0)));
Eigen::Isometry3d transform2 = toEigen(makeTransform(makeVector3(-1.5, -2.6, -3.7), makeQuaternion(0, 0, 0, 1)));
Eigen::Vector3d const & p1 = transform1.translation();
Eigen::Vector3d const & p2 = transform2.translation();
Eigen::Quaterniond const & q1 = Eigen::Quaterniond{transform1.rotation()};
Eigen::Quaterniond const & q2 = Eigen::Quaterniond{transform2.rotation()};
ASSERT_NEAR(0.0, p1.x(), 1e-5);
ASSERT_NEAR(1.5, p1.y(), 1e-5);
ASSERT_NEAR(2.0, p1.z(), 1e-5);
ASSERT_NEAR(-1.5, p2.x(), 1e-5);
ASSERT_NEAR(-2.6, p2.y(), 1e-5);
ASSERT_NEAR(-3.7, p2.z(), 1e-5);
ASSERT_NEAR(1, q1.x(), 1e-5);
ASSERT_NEAR(0, q1.y(), 1e-5);
ASSERT_NEAR(0, q1.z(), 1e-5);
ASSERT_NEAR(0, q1.w(), 1e-5);
ASSERT_NEAR(0, q2.x(), 1e-5);
ASSERT_NEAR(0, q2.y(), 1e-5);
ASSERT_NEAR(0, q2.z(), 1e-5);
ASSERT_NEAR(1, q2.w(), 1e-5);
}
TEST(rosToEigen, quaternion) {
Eigen::Quaterniond q1 = toEigen(makeQuaternion(0, 1.5, 2, 3.6));
Eigen::Quaterniond q2 = toEigen(makeQuaternion(-1.5, -2.6, -3.7, 4.9));
ASSERT_NEAR(0.0, q1.x(), 1e-5);
ASSERT_NEAR(1.5, q1.y(), 1e-5);
ASSERT_NEAR(2.0, q1.z(), 1e-5);
ASSERT_NEAR(3.6, q1.w(), 1e-5);
ASSERT_NEAR(-1.5, q2.x(), 1e-5);
ASSERT_NEAR(-2.6, q2.y(), 1e-5);
ASSERT_NEAR(-3.7, q2.z(), 1e-5);
ASSERT_NEAR(4.9, q2.w(), 1e-5);
}
Quaternion makeNormalizedQuaternion(double x, double y, double z, double w)
{
auto q = makeQuaternion(x, y, z, w);
if (!isNormalized(q, 1e-5f))
{
std::stringstream err;
err << "Quaternion(" << x << ", " << y << ", " << z << ", " << w
<< ") is not normalized";
throw std::runtime_error(err.str());
}
return q;
}
