Fix implementations of orthogonalize_qr and orthogonalize_svd (they were swapped)

SimoxUtility/math/pose/orthogonalize.cpp

@@ -12,6 +12,22 @@ Eigen::Matrix3f simox::math::orthogonalize(const Eigen::Matrix3f& matrix)
 
 
 Eigen::Matrix3f simox::math::orthogonalize_svd(const Eigen::Matrix3f& matrix)
+{
+    auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+    Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV().transpose();
+    if (orth.determinant() >= 0)
+    {
+        return orth;
+    }
+    else
+    {
+        return -orth;
+    }
+}
+
+
+Eigen::Matrix3f simox::math::orthogonalize_qr(const Eigen::Matrix3f& matrix)
 {
     auto householder = matrix.householderQr();
     Eigen::Matrix3f orth = householder.householderQ();
@@ -30,22 +46,6 @@ Eigen::Matrix3f simox::math::orthogonalize_svd(const Eigen::Matrix3f& matrix)
 }
 
 
-Eigen::Matrix3f simox::math::orthogonalize_qr(const Eigen::Matrix3f& matrix)
-{
-    auto svd = matrix.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
-
-    Eigen::Matrix3f orth = svd.matrixU() * svd.matrixV().transpose();
-    if (orth.determinant() >= 0)
-    {
-        return orth;
-    }
-    else
-    {
-        return -orth;
-    }
-}
-
-
 Eigen::Matrix4f simox::math::orthogonalize_pose(const Eigen::Matrix4f& pose)
 {
     Eigen::Matrix4f orth = pose;