Skip to content

Commit

Permalink
Merge pull request #1589 from borglab/feature/add_quaternion_test
Browse files Browse the repository at this point in the history
Additional quaternion test
  • Loading branch information
dellaert authored Jul 23, 2023
2 parents 1f0d318 + 0fc6623 commit ba42f4e
Showing 1 changed file with 19 additions and 0 deletions.
19 changes: 19 additions & 0 deletions gtsam/geometry/tests/testRot3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit ba42f4e

Please sign in to comment.