RT orthonormal

include/Utils/Matrix3d.h

@@ -50,6 +50,29 @@ class BIORBD_API Matrix3d : public RigidBodyDynamics::Math::Matrix3d
                                            other) :
         RigidBodyDynamics::Math::Matrix3d(other) {}
 #endif
+
+#ifndef BIORBD_USE_CASADI_MATH
+    ///
+    /// \brief Return the 1-norm of the matrix
+    /// \return The 1-norm of the matrix
+    ///
+    utils::Scalar normOne() const;
+
+    ///
+    /// \brief Return the inf-norm of the matrix
+    /// \return The inf-norm of the matrix
+    ///
+    utils::Scalar normInf() const;
+
+    ///
+    /// \brief Produce an orthonormal matrix from the current matrix
+    /// \return The othonormal matrix
+    ///
+    /// Code adapted from https://github.com/brainexcerpts/3x3_polar_decomposition/blob/master/src/polar_decomposition_3x3.inl
+    ///
+    utils::Matrix3d orthoNormalize() const;
+#endif
+
 #ifdef BIORBD_USE_CASADI_MATH
 
     ///

src/Utils/Matrix3d.cpp

@@ -20,6 +20,41 @@ utils::Matrix3d::Matrix3d(
 
 }
 
+#ifndef BIORBD_USE_CASADI_MATH
+utils::Scalar utils::Matrix3d::normOne() const
+{
+    utils::Scalar value = 0.0;
+    for (size_t i = 0; i < 3; ++i)
+    {
+        utils::Scalar col_sum = fabs((*this)(0, i)) + fabs((*this)(1, i)) + fabs((*this)(2, i));
+        if (col_sum > value)
+            value = col_sum;
+    }
+    return value;
+}
+
+utils::Scalar utils::Matrix3d::normInf() const
+{
+    utils::Scalar value = 0.0;
+    for (size_t i = 0; i < 3; ++i)
+    {
+        utils::Scalar row_sum = fabs((*this)(i, 0)) + fabs((*this)(i, 1)) + fabs((*this)(i, 2));
+        if (row_sum > value)
+            value = row_sum;
+    }
+    return value;
+}
+
+utils::Matrix3d utils::Matrix3d::orthoNormalize() const
+{
+#ifdef BIORBD_USE_EIGEN3_MATH
+    Eigen::JacobiSVD<Eigen::MatrixXd> svd(*this, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    return svd.matrixU() * svd.matrixV().transpose();
+#else
+#error "SVD decomposition not implemented for non-eigen3 backend"
+#endif
+}
+#endif
 
 #ifdef BIORBD_USE_CASADI_MATH
 

src/Utils/Rotation.cpp

@@ -311,11 +311,19 @@ void utils::Rotation::checkUnitary()
 #ifndef BIORBD_USE_CASADI_MATH
 #ifndef SKIP_ASSERT
     double sqrtNorm = static_cast<double>(this->squaredNorm());
-    utils::Error::check(fabs(sqrtNorm - 3.) < 1e-4,
-                                utils::String("The Rotation matrix square norm is equal to ")
-                                + sqrtNorm + ", but should be equal to 3. Alternatively, you "
-                                             "can compile with SKIP_ASSERT set to ON to turn off "
-                                             "this error message");
+    bool checkUnit(fabs(sqrtNorm - 3.) < 1e-4);
+    if (!checkUnit)
+    {
+        utils::Matrix3d R(orthoNormalize());
+        utils::Error::raise(utils::String("The Rotation matrix square norm is equal to ")
+                        + sqrtNorm + ", but should be equal to 3. Consider replacing the RT matrix by this closest orthonormal matrix \n[" 
+                        + R(0, 0) + ", " + R(0, 1) + ", " + R(0, 2) + "\n" 
+                        + R(1, 0) + ", " + R(1, 1) + ", " + R(1, 2) + "\n" 
+                        + R(2, 0) + ", " + R(2, 1) + ", " + R(2, 2) + "]\n" 
+                        + "Alternatively, you can compile with SKIP_ASSERT set to ON to turn off this error message");
+
+    }
+
 #endif
 #endif
 }

test/test_utils.cpp

@@ -507,6 +507,40 @@ TEST(Matrix, unitTest)
     }
 }
 
+#ifndef BIORBD_USE_CASADI_MATH
+TEST(matrix3d, unitTest){
+    utils::Matrix3d R(1, 2, 3,
+                      4, 5, 6,
+                      7, 8, 9);
+    {
+        SCALAR_TO_DOUBLE(one, R.normOne());
+        EXPECT_NEAR(one, 18, requiredPrecision);
+    }
+
+    {
+        SCALAR_TO_DOUBLE(inf, R.normInf());
+        EXPECT_NEAR(inf, 24, requiredPrecision);
+    }
+
+    {
+        utils::Matrix3d orthoNorm = R.orthoNormalize();
+        std::vector<double> expectedMult({
+                                             -0.419386184128578,  -0.2775187761147384,  0.8643486318991006,
+                                             -0.2775187761147384,  0.9457394527596399,  0.16899768163401963,
+                                              0.8643486318991006,  0.16899768163401974, 0.47364673136893787
+                                         });
+        int cmp(0);
+        for (unsigned int i=0; i<3; ++i) {
+            for (unsigned int j=0; j<3; ++j) {
+                SCALAR_TO_DOUBLE(m, orthoNorm(i, j));
+                EXPECT_NEAR(m, expectedMult[cmp++], requiredPrecision);
+            }
+        }
+    }
+
+}
+#endif
+
 TEST(Rotation, unitTest)
 {
     utils::Rotation rot1(