Moved normalize next to ClosestTo and add more docs

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;
 

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