diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index db5367c8f..b1e46308d 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 ffc468dfc..c372d403b 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 d4400b0dc..523255d87 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 e86029026..7b792f8bd 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);