Merge branch 'math-helpers'

VirtualRobot/math/Helpers.cpp

@@ -280,18 +280,14 @@ Eigen::Matrix3f Helpers::TransformOrientation(const Eigen::Matrix4f& transform,
 
 Eigen::Matrix3f Helpers::Orthogonalize(const Eigen::Matrix3f& matrix)
 {
-    return OrthogonalizeQR(matrix);
+    return OrthogonalizeSVD(matrix);
 }
 
 Eigen::Matrix3f Helpers::OrthogonalizeSVD(const Eigen::Matrix3f& matrix)
 {
-    /* Currently, tests fail for SVD. The returned matrices are orthogonal,
-     * but have high angular distances to the original rotation.
-     * (e.g. 1.06 rad on Identity with noise, where householder QR has 0 rad)
-     */
     auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
     
-    Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV();
+    Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV().transpose();
     if (orth.determinant() >= 0)
         return orth;
     else

VirtualRobot/math/Helpers.h

@@ -140,7 +140,7 @@ namespace math
         template <typename Derived>
         static bool IsMatrixOrthogonal(const Eigen::MatrixBase<Derived>& matrix, float precision = 1e-6f);
         
-        /// Compute the closest orthogonal matrix to the given matrix. 
+        /// Compute the closest orthogonal matrix to the given matrix.
         /// (Note: All rotation matrices must be orthogonal.)
         static Eigen::Matrix3f Orthogonalize(const Eigen::Matrix3f& matrix);
         
@@ -148,7 +148,6 @@ namespace math
         static Eigen::Matrix3f OrthogonalizeQR(const Eigen::Matrix3f& matrix);
         
         /// Orthogonolize the given matrix using Jacobi SVD decomposition.
-        /// (Currently not recommended since it yields high angular distances to the original matrix.)
         static Eigen::Matrix3f OrthogonalizeSVD(const Eigen::Matrix3f& matrix);
         
         /// Orthogonolize the orientation of the given pose, and sanitize its lower row.