Use a single inverse implementation for a single matrix size

matrix/PseudoInverse.hpp

@@ -4,6 +4,7 @@
  * Implementation of matrix pseudo inverse
  *
  * @author Julien Lecoeur <julien.lecoeur@gmail.com>
+ * @author Julian Kent <julian@auterion.com>
  */
 
 #pragma once
@@ -13,17 +14,6 @@
 namespace matrix
 {
 
-/**
- * Full rank Cholesky factorization of A
- */
-template<typename Type, size_t N>
-SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A, size_t& rank);
-
-/**
- * Geninv implementation detail
- */
-template<typename Type, size_t M, size_t N, size_t R> class GeninvImpl;
-
 /**
  * Geninv
  * Fast pseudoinverse based on full rank cholesky factorisation
@@ -38,18 +28,84 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
         SquareMatrix<Type, M> A = G * G.transpose();
         SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
 
-        return GeninvImpl<Type, M, N, M>::genInvUnderdetermined(G, L, rank);
+        SquareMatrix<Type, M> LtL = L.transpose() * L;
+        SquareMatrix<Type, M> X;
+        if (!inv(LtL, X, rank)) {
+            return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+        }
+        return G.transpose() * L * X * X * L.transpose();
 
     } else {
         SquareMatrix<Type, N> A = G.transpose() * G;
         SquareMatrix<Type, N> L = fullRankCholesky(A, rank);
 
-        return GeninvImpl<Type, M, N, N>::genInvOverdetermined(G, L, rank);
+        SquareMatrix<Type, N> LtL = L.transpose() * L;
+        SquareMatrix<Type, N> X;
+        if(!inv(LtL, X, rank)) {
+            return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
+        }
+        return L * X * X * L.transpose() * G.transpose();
     }
 }
 
 
-#include "PseudoInverse.hxx"
+template<typename Type>
+Type typeEpsilon();
+
+template<> inline
+float typeEpsilon<float>()
+{
+    return FLT_EPSILON;
+}
+
+/**
+ * Full rank Cholesky factorization of A
+ */
+template<typename Type, size_t N>
+SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
+                                       size_t& rank)
+{
+    // Loses one ulp accuracy per row of diag, relative to largest magnitude
+    const Type tol = N * typeEpsilon<Type>() * A.diag().max();
+
+    Matrix<Type, N, N> L;
+
+    size_t r = 0;
+    for (size_t k = 0; k < N; k++) {
+
+        if (r == 0) {
+            for (size_t i = k; i < N; i++) {
+                L(i, r) = A(i, k);
+            }
+
+        } else {
+            for (size_t i = k; i < N; i++) {
+                // Compute LL = L[k:n, :r] * L[k, :r].T
+                Type LL = Type();
+                for (size_t j = 0; j < r; j++) {
+                    LL += L(i, j) * L(k, j);
+                }
+                L(i, r) = A(i, k) - LL;
+            }
+        }
+        if (L(k, r) > tol) {
+            L(k, r) = sqrt(L(k, r));
+
+            if (k < N - 1) {
+                for (size_t i = k + 1; i < N; i++) {
+                    L(i, r) = L(i, r) / L(k, r);
+                }
+            }
+
+            r = r + 1;
+        }
+    }
+
+    // Return rank
+    rank = r;
+
+    return L;
+}
 
 } // namespace matrix
 

matrix/PseudoInverse.hxx

@@ -1,126 +0,0 @@
-/**
- * @file pseudoinverse.hxx
- *
- * Implementation of matrix pseudo inverse
- *
- * @author Julien Lecoeur <julien.lecoeur@gmail.com>
- */
-
-
-
-/**
- * Full rank Cholesky factorization of A
- */
-template<typename Type>
-Type typeEpsilon();
-
-template<> inline
-float typeEpsilon<float>()
-{
-    return FLT_EPSILON;
-}
-
-template<typename Type, size_t N>
-SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
-                               size_t& rank)
-{
-    // Loses one ulp accuracy per row of diag, relative to largest magnitude
-    const Type tol = N * typeEpsilon<Type>() * A.diag().max();
-
-    Matrix<Type, N, N> L;
-
-    size_t r = 0;
-    for (size_t k = 0; k < N; k++) {
-
-        if (r == 0) {
-            for (size_t i = k; i < N; i++) {
-                L(i, r) = A(i, k);
-            }
-
-        } else {
-            for (size_t i = k; i < N; i++) {
-                // Compute LL = L[k:n, :r] * L[k, :r].T
-                Type LL = Type();
-                for (size_t j = 0; j < r; j++) {
-                    LL += L(i, j) * L(k, j);
-                }
-                L(i, r) = A(i, k) - LL;
-            }
-        }
-        if (L(k, r) > tol) {
-            L(k, r) = sqrt(L(k, r));
-
-            if (k < N - 1) {
-                for (size_t i = k + 1; i < N; i++) {
-                    L(i, r) = L(i, r) / L(k, r);
-                }
-            }
-
-            r = r + 1;
-        }
-    }
-
-    // Return rank
-    rank = r;
-
-    return L;
-}
-
-template< typename Type, size_t M, size_t N, size_t R>
-class GeninvImpl
-{
-public:
-    static Matrix<Type, N, M> genInvUnderdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, M, M> & L, size_t rank)
-    {
-        if (rank < R) {
-            // Recursive call
-            return GeninvImpl<Type, M, N, R - 1>::genInvUnderdetermined(G, L, rank);
-
-        } else if (rank > R) {
-            // Error
-            return Matrix<Type, N, M>();
-
-        } else {
-            // R == rank
-            Matrix<Type, M, R> LL = L. template slice<M, R>(0, 0);
-            SquareMatrix<Type, R> X = inv(SquareMatrix<Type, R>(LL.transpose() * LL));
-            return G.transpose() * LL * X * X * LL.transpose();
-
-        }
-    }
-
-    static Matrix<Type, N, M> genInvOverdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, N, N> & L, size_t rank)
-    {
-        if (rank < R) {
-            // Recursive call
-            return GeninvImpl<Type, M, N, R - 1>::genInvOverdetermined(G, L, rank);
-
-        } else if (rank > R) {
-            // Error
-            return Matrix<Type, N, M>();
-
-        } else {
-            // R == rank
-            Matrix<Type, N, R> LL = L. template slice<N, R>(0, 0);
-            SquareMatrix<Type, R> X = inv(SquareMatrix<Type, R>(LL.transpose() * LL));
-            return LL * X * X * LL.transpose() * G.transpose();
-
-        }
-    }
-};
-
-// Partial template specialisation for R==0, allows to stop recursion in genInvUnderdetermined and genInvOverdetermined
-template< typename Type, size_t M, size_t N>
-class GeninvImpl<Type, M, N, 0>
-{
-public:
-    static Matrix<Type, N, M> genInvUnderdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, M, M> & L, size_t rank)
-    {
-        return Matrix<Type, N, M>();
-    }
-
-    static Matrix<Type, N, M> genInvOverdetermined(const Matrix<Type, M, N> & G, const Matrix<Type, N, N> & L, size_t rank)
-    {
-        return Matrix<Type, N, M>();
-    }
-};

matrix/SquareMatrix.hpp

@@ -305,7 +305,7 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5)
  * inverse based on LU factorization with partial pivotting
  */
 template<typename Type, size_t M>
-bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
+bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv, size_t rank = M)
 {
     SquareMatrix<Type, M> L;
     L.setIdentity();
@@ -316,12 +316,12 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     //printf("A:\n"); A.print();
 
     // for all diagonal elements
-    for (size_t n = 0; n < M; n++) {
+    for (size_t n = 0; n < rank; n++) {
 
         // if diagonal is zero, swap with row below
         if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
             //printf("trying pivot for row %d\n",n);
-            for (size_t i = n + 1; i < M; i++) {
+            for (size_t i = n + 1; i < rank; i++) {
 
                 //printf("\ttrying row %d\n",i);
                 if (fabs(static_cast<float>(U(i, n))) > 1e-8f) {
@@ -349,12 +349,12 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
         }
 
         // for all rows below diagonal
-        for (size_t i = (n + 1); i < M; i++) {
+        for (size_t i = (n + 1); i < rank; i++) {
             L(i, n) = U(i, n) / U(n, n);
 
             // add i-th row and n-th row
             // multiplied by: -a(i,n)/a(n,n)
-            for (size_t k = n; k < M; k++) {
+            for (size_t k = n; k < rank; k++) {
                 U(i, k) -= L(i, n) * U(n, k);
             }
         }
@@ -367,9 +367,9 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     //SquareMatrix<Type, M> Y = P;
 
     // for all columns of Y
-    for (size_t c = 0; c < M; c++) {
+    for (size_t c = 0; c < rank; c++) {
         // for all rows of L
-        for (size_t i = 0; i < M; i++) {
+        for (size_t i = 0; i < rank; i++) {
             // for all columns of L
             for (size_t j = 0; j < i; j++) {
                 // for all existing y
@@ -392,14 +392,14 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     //SquareMatrix<Type, M> X = Y;
 
     // for all columns of X
-    for (size_t c = 0; c < M; c++) {
+    for (size_t c = 0; c < rank; c++) {
         // for all rows of U
-        for (size_t k = 0; k < M; k++) {
+        for (size_t k = 0; k < rank; k++) {
             // have to go in reverse order
-            size_t i = M - 1 - k;
+            size_t i = rank - 1 - k;
 
             // for all columns of U
-            for (size_t j = i + 1; j < M; j++) {
+            for (size_t j = i + 1; j < rank; j++) {
                 // for all existing x
                 // subtract the component they
                 // contribute to the solution
@@ -416,8 +416,8 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
     }
 
     //check sanity of results
-    for (size_t i = 0; i < M; i++) {
+    for (size_t i = 0; i < rank; i++) {
-        for (size_t j = 0; j < M; j++) {
+        for (size_t j = 0; j < rank; j++) {
             if (!is_finite(P(i,j))) {
                 return false;
             }

test/pseudoInverse.cpp

@@ -111,22 +111,6 @@ int main()
     Matrix<float, 16, 6> A = geninv(B);
     TEST((A - A_check).abs().max() < 1e-5);
 
-    // Test error case with erroneous rank in internal impl functions
-    Matrix<float, 2, 2> L;
-    Matrix<float, 2, 3> GM;
-    Matrix<float, 3, 2> retM_check;
-    Matrix<float, 3, 2> retM0 = GeninvImpl<float, 2, 3, 0>::genInvUnderdetermined(GM, L, 5);
-    Matrix<float, 3, 2> GN;
-    Matrix<float, 2, 3> retN_check;
-    Matrix<float, 2, 3> retN0 = GeninvImpl<float, 3, 2, 0>::genInvOverdetermined(GN, L, 5);
-    TEST((retM0 - retM_check).abs().max() < 1e-5);
-    TEST((retN0 - retN_check).abs().max() < 1e-5);
-
-    Matrix<float, 3, 2> retM1 = GeninvImpl<float, 2, 3, 1>::genInvUnderdetermined(GM, L, 5);
-    Matrix<float, 2, 3> retN1 = GeninvImpl<float, 3, 2, 1>::genInvOverdetermined(GN, L, 5);
-    TEST((retM1 - retM_check).abs().max() < 1e-5);
-    TEST((retN1 - retN_check).abs().max() < 1e-5);
-
     // Real-world test case
     const float real_alloc[5][6] = {
         { 0.794079,  0.794079,  0.794079,  0.794079,  0.0000,  0.0000},