From 0fc662399aa1c30b7ac9d2c8875791b65eeadc97 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 22 Jul 2023 10:12:40 +0200 Subject: [PATCH] Add a test to debug issue #1588 --- gtsam/geometry/tests/testRot3.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 49b9785679..1232348f03 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -597,6 +597,25 @@ TEST(Rot3, quaternion) { EXPECT(assert_equal(expected2, actual2)); } +/* ************************************************************************* */ +TEST(Rot3, ConvertQuaternion) { + Eigen::Quaterniond eigenQuaternion; + eigenQuaternion.w() = 1.0; + eigenQuaternion.x() = 2.0; + eigenQuaternion.y() = 3.0; + eigenQuaternion.z() = 4.0; + EXPECT_DOUBLES_EQUAL(1, eigenQuaternion.w(), 1e-9); + EXPECT_DOUBLES_EQUAL(2, eigenQuaternion.x(), 1e-9); + EXPECT_DOUBLES_EQUAL(3, eigenQuaternion.y(), 1e-9); + EXPECT_DOUBLES_EQUAL(4, eigenQuaternion.z(), 1e-9); + + Rot3 R(eigenQuaternion); + EXPECT_DOUBLES_EQUAL(1, R.toQuaternion().w(), 1e-9); + EXPECT_DOUBLES_EQUAL(2, R.toQuaternion().x(), 1e-9); + EXPECT_DOUBLES_EQUAL(3, R.toQuaternion().y(), 1e-9); + EXPECT_DOUBLES_EQUAL(4, R.toQuaternion().z(), 1e-9); +} + /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { Matrix::Index n = A.cols();