From 06ac62724927abc947aec6e87017f8b2bd5df39a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 4 Oct 2020 19:55:08 -0400 Subject: [PATCH 1/3] added normalize function to orthogonalize the rotation after composition --- gtsam/geometry/Rot3.h | 7 +++++++ gtsam/geometry/Rot3M.cpp | 28 +++++++++++++++++++++++++++- gtsam/geometry/Rot3Q.cpp | 6 +++++- gtsam/geometry/tests/testRot3.cpp | 20 ++++++++++++++++++++ 4 files changed, 59 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index de9d2b4205..db5367c8f0 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -430,6 +430,13 @@ namespace gtsam { */ Matrix3 transpose() const; + /** + * Normalize rotation so that its determinant is 1. + * This means either re-orthogonalizing the Matrix representation or + * normalizing the quaternion representation. + */ + Rot3 normalize(const Rot3& R) const; + /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 500941a16d..ffc468dfcc 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -108,9 +108,35 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, ); } +/* ************************************************************************* */ +Rot3 Rot3::normalize(const Rot3& R) const { + /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 + /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view + + /// Essentially, this computes the orthogonalization error, distributes the + /// error to the x and y rows, and then performs a Taylor expansion to + /// orthogonalize. + + Matrix3 rot = R.matrix(), rot_new; + + if (std::fabs(rot.determinant()-1) < 1e-12) return R; + + Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0); + double error = x.dot(y); + + Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x; + Vector3 z_ort = x_ort.cross(y_ort); + + rot_new.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; + rot_new.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; + rot_new.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; + + return Rot3(rot_new); +} + /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(rot_*R2.rot_); + return normalize(Rot3(rot_*R2.rot_)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 6e1871c64d..d4400b0dca 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -86,9 +86,13 @@ namespace gtsam { gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } + /* ************************************************************************* */ + Rot3 Rot3::normalize(const Rot3& R) const { + return Rot3(R.quaternion_.normalized()); + } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); + return normalize(Rot3(quaternion_ * R2.quaternion_)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a7c6f5a77d..e860290261 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -910,6 +910,26 @@ TEST(Rot3, yaw_derivative) { CHECK(assert_equal(num, calc)); } +/* ************************************************************************* */ +TEST(Rot3, determinant) { + size_t degree = 1; + Rot3 R_w0; // Zero rotation + Rot3 R_w1 = Rot3::Ry(degree * M_PI / 180); + + Rot3 R_01, R_w2; + double actual, expected = 1.0; + + for (size_t i = 2; i < 360; ++i) { + R_01 = R_w0.between(R_w1); + R_w2 = R_w1 * R_01; + R_w0 = R_w1; + R_w1 = R_w2; + actual = R_w2.matrix().determinant(); + + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7); + } +} + /* ************************************************************************* */ int main() { TestResult tr; From 2e1cc3ca3517eca5431a07405453f776b28c9aeb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Oct 2020 13:25:30 -0400 Subject: [PATCH 2/3] normalized needs to be called explicitly --- gtsam/geometry/Rot3.h | 2 +- gtsam/geometry/Rot3M.cpp | 18 ++++++++++-------- gtsam/geometry/Rot3Q.cpp | 6 +++--- gtsam/geometry/tests/testRot3.cpp | 2 +- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index db5367c8f0..b1e46308d2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -435,7 +435,7 @@ namespace gtsam { * This means either re-orthogonalizing the Matrix representation or * normalizing the quaternion representation. */ - Rot3 normalize(const Rot3& R) const; + Rot3 normalized() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index ffc468dfcc..c372d403b3 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -109,7 +109,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, } /* ************************************************************************* */ -Rot3 Rot3::normalize(const Rot3& R) const { +Rot3 Rot3::normalized() const { /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view @@ -117,9 +117,11 @@ Rot3 Rot3::normalize(const Rot3& R) const { /// error to the x and y rows, and then performs a Taylor expansion to /// orthogonalize. - Matrix3 rot = R.matrix(), rot_new; + Matrix3 rot = rot_.matrix(), rot_orth; - if (std::fabs(rot.determinant()-1) < 1e-12) return R; + // Check if determinant is already 1. + // If yes, then return the current Rot3. + if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_); Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0); double error = x.dot(y); @@ -127,16 +129,16 @@ Rot3 Rot3::normalize(const Rot3& R) const { Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x; Vector3 z_ort = x_ort.cross(y_ort); - rot_new.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; - rot_new.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; - rot_new.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; + rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort; + rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort; + rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort; - return Rot3(rot_new); + return Rot3(rot_orth); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return normalize(Rot3(rot_*R2.rot_)); + return Rot3(rot_*R2.rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d4400b0dca..523255d87d 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,12 +87,12 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::normalize(const Rot3& R) const { - return Rot3(R.quaternion_.normalized()); + Rot3 Rot3::normalized() const { + return Rot3(quaternion_.normalized()); } /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { - return normalize(Rot3(quaternion_ * R2.quaternion_)); + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index e860290261..7b792f8bdc 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -923,7 +923,7 @@ TEST(Rot3, determinant) { R_01 = R_w0.between(R_w1); R_w2 = R_w1 * R_01; R_w0 = R_w1; - R_w1 = R_w2; + R_w1 = R_w2.normalized(); actual = R_w2.matrix().determinant(); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7); From 5fb7229fa67fd14b671611dc5b8daf21f5179342 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 5 Oct 2020 22:28:27 -0400 Subject: [PATCH 3/3] Moved normalize next to ClosestTo and add more docs --- gtsam/geometry/Rot3.h | 29 +++++++++++++++++++++-------- gtsam/geometry/Rot3M.cpp | 1 - 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index b1e46308d2..2334312f69 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -262,9 +262,29 @@ namespace gtsam { static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, // const Unit3& a_q, const Unit3& b_q); - /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + /** + * Static, named constructor that finds Rot3 element closest to M in Frobenius norm. + * + * Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust. + * + * N. J. Higham. Matrix nearness problems and applications. + * In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27. + * Oxford University Press, 1989. + */ static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); } + /** + * Normalize rotation so that its determinant is 1. + * This means either re-orthogonalizing the Matrix representation or + * normalizing the quaternion representation. + * + * This method is akin to `ClosestTo` but uses a computationally cheaper + * algorithm. + * + * Ref: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view + */ + Rot3 normalized() const; + /// @} /// @name Testable /// @{ @@ -430,13 +450,6 @@ namespace gtsam { */ Matrix3 transpose() const; - /** - * Normalize rotation so that its determinant is 1. - * This means either re-orthogonalizing the Matrix representation or - * normalizing the quaternion representation. - */ - Rot3 normalized() const; - /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index c372d403b3..02e5b771fc 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -111,7 +111,6 @@ Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, /* ************************************************************************* */ Rot3 Rot3::normalized() const { /// Implementation from here: https://stackoverflow.com/a/23082112/1236990 - /// Theory: https://drive.google.com/file/d/0B9rLLz1XQKmaZTlQdV81QjNoZTA/view /// Essentially, this computes the orthogonalization error, distributes the /// error to the x and y rows, and then performs a Taylor expansion to