From 069b576856c78e54eae9e670d273da0c3a0279ac Mon Sep 17 00:00:00 2001
From: Kristin <kberry@usgs.gov>
Date: Mon, 6 Jul 2020 11:44:41 -0700
Subject: [PATCH] Normalize quaternions in Rotations (#377)
* Update format of range conversion coefficients isd output to include a separate list for times. Also update driver, test data, and remove unneeded semicolons
* Fix documentaion error
* fix other documentation error
* Add quaternion normalization
* Add test that makes sure an unnormalized quaternion is normalized in the rotation constructor
---
include/ale/Rotation.h | 2 +-
src/Rotation.cpp | 5 +++--
tests/ctests/RotationTests.cpp | 12 ++++++++++++
3 files changed, 16 insertions(+), 3 deletions(-)
diff --git a/include/ale/Rotation.h b/include/ale/Rotation.h
index 5b28031..2df46d6 100644
--- a/include/ale/Rotation.h
+++ b/include/ale/Rotation.h
@@ -77,7 +77,7 @@ namespace ale {
std::vector<double> toRotationMatrix() const;
/**
- * Create a state rotation matrix from the rotation and an angula velocity.
+ * Create a state rotation matrix from the rotation and an angular velocity.
*
* @param av The angular velocity vector.
*
diff --git a/src/Rotation.cpp b/src/Rotation.cpp
index bde05fd..07c691f 100644
--- a/src/Rotation.cpp
+++ b/src/Rotation.cpp
@@ -65,7 +65,7 @@ namespace ale {
throw std::invalid_argument("Rotation matrix must be 3 by 3.");
}
// The data is in row major order, so take the transpose to get column major order
- quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()).transpose());
+ quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()).transpose()).normalized();
}
@@ -81,6 +81,7 @@ namespace ale {
for (size_t i = 0; i < angles.size(); i++) {
quat *= Eigen::Quaterniond(Eigen::AngleAxisd(angles[i], axis(axes[i])));
}
+ quat = quat.normalized();
}
@@ -89,7 +90,7 @@ namespace ale {
throw std::invalid_argument("Rotation axis must have 3 elements.");
}
Eigen::Vector3d eigenAxis((double *) axis.data());
- quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized()));
+ quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized())).normalized();
}
diff --git a/tests/ctests/RotationTests.cpp b/tests/ctests/RotationTests.cpp
index 2271ace..8e4517c 100644
--- a/tests/ctests/RotationTests.cpp
+++ b/tests/ctests/RotationTests.cpp
@@ -28,6 +28,18 @@ TEST(RotationTest, QuaternionConstructor) {
EXPECT_NEAR(quat[3], 0.0, 1e-10);
}
+TEST(RotationTest, UnnormalizedQuaternion) {
+ Rotation rotation(1.0, 1.0, 0.0, 0.0);
+ vector<double> quat = rotation.toQuaternion();
+ ASSERT_EQ(quat.size(), 4);
+ // The constructor will normalize the quaternion.
+ EXPECT_NEAR(quat[0], 1.0/sqrt(2), 1e-10);
+ EXPECT_NEAR(quat[1], 1.0/sqrt(2), 1e-10);
+ EXPECT_NEAR(quat[2], 0.0, 1e-10);
+ EXPECT_NEAR(quat[3], 0.0, 1e-10);
+}
+
+
TEST(RotationTest, MatrixConstructor) {
Rotation rotation(
{0.0, 1.0, 0.0,
--
GitLab