added normalize function to orthogonalize the rotation after composition

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;
 

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_));
 }
 
 /* ************************************************************************* */

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_));
   }
 
   /* ************************************************************************* */

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;