12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add a test to debug issue #1588

release/4.3a0
Frank Dellaert 2023-07-22 10:12:40 +02:00
parent 1f0d318482
commit 0fc662399a
1 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/geometry/tests/testRot3.cpp
View File

@ -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();