Merge pull request #555 from borglab/fix/372

Normalize rotations after composition

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
     /// @{

gtsam/geometry/Rot3M.cpp

@@ -108,6 +108,33 @@ 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
+
+  /// 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 = rot_.matrix(), rot_orth;
+
+  // 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);
+
+  Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x;
+  Vector3 z_ort = x_ort.cross(y_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_orth);
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::operator*(const Rot3& R2) const {
   return Rot3(rot_*R2.rot_);

gtsam/geometry/Rot3Q.cpp

@@ -86,6 +86,10 @@ namespace gtsam {
         gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
   }
 
+  /* ************************************************************************* */
+  Rot3 Rot3::normalized() const {
+    return Rot3(quaternion_.normalized());
+  }
   /* ************************************************************************* */
   Rot3 Rot3::operator*(const Rot3& R2) const {
     return Rot3(quaternion_ * R2.quaternion_);

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.normalized();
+    actual = R_w2.matrix().determinant();
+
+    EXPECT_DOUBLES_EQUAL(expected, actual, 1e-7);
+  }
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;