From 054f8b12f4da79767df6e05bb1d2cc8d679b99f3 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Mon, 4 Jan 2021 14:26:11 +0100 Subject: [PATCH] Use a single inverse implementation for a single matrix size --- matrix/PseudoInverse.hpp | 84 +++++++++++++++++++++----- matrix/PseudoInverse.hxx | 126 --------------------------------------- matrix/SquareMatrix.hpp | 26 ++++---- test/pseudoInverse.cpp | 16 ----- 4 files changed, 83 insertions(+), 169 deletions(-) delete mode 100644 matrix/PseudoInverse.hxx diff --git a/matrix/PseudoInverse.hpp b/matrix/PseudoInverse.hpp index 469e358100..2463529640 100644 --- a/matrix/PseudoInverse.hpp +++ b/matrix/PseudoInverse.hpp @@ -4,6 +4,7 @@ * Implementation of matrix pseudo inverse * * @author Julien Lecoeur + * @author Julian Kent */ #pragma once @@ -13,17 +14,6 @@ namespace matrix { -/** - * Full rank Cholesky factorization of A - */ -template -SquareMatrix fullRankCholesky(const SquareMatrix & A, size_t& rank); - -/** - * Geninv implementation detail - */ -template class GeninvImpl; - /** * Geninv * Fast pseudoinverse based on full rank cholesky factorisation @@ -38,18 +28,84 @@ Matrix geninv(const Matrix & G) SquareMatrix A = G * G.transpose(); SquareMatrix L = fullRankCholesky(A, rank); - return GeninvImpl::genInvUnderdetermined(G, L, rank); + SquareMatrix LtL = L.transpose() * L; + SquareMatrix X; + if (!inv(LtL, X, rank)) { + return Matrix(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues + } + return G.transpose() * L * X * X * L.transpose(); } else { SquareMatrix A = G.transpose() * G; SquareMatrix L = fullRankCholesky(A, rank); - return GeninvImpl::genInvOverdetermined(G, L, rank); + SquareMatrix LtL = L.transpose() * L; + SquareMatrix X; + if(!inv(LtL, X, rank)) { + return Matrix(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues + } + return L * X * X * L.transpose() * G.transpose(); } } -#include "PseudoInverse.hxx" +template +Type typeEpsilon(); + +template<> inline +float typeEpsilon() +{ + return FLT_EPSILON; +} + +/** + * Full rank Cholesky factorization of A + */ +template +SquareMatrix fullRankCholesky(const SquareMatrix & A, + size_t& rank) +{ + // Loses one ulp accuracy per row of diag, relative to largest magnitude + const Type tol = N * typeEpsilon() * A.diag().max(); + + Matrix 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 diff --git a/matrix/PseudoInverse.hxx b/matrix/PseudoInverse.hxx deleted file mode 100644 index bd5ae6bbb4..0000000000 --- a/matrix/PseudoInverse.hxx +++ /dev/null @@ -1,126 +0,0 @@ -/** - * @file pseudoinverse.hxx - * - * Implementation of matrix pseudo inverse - * - * @author Julien Lecoeur - */ - - - -/** - * Full rank Cholesky factorization of A - */ -template -Type typeEpsilon(); - -template<> inline -float typeEpsilon() -{ - return FLT_EPSILON; -} - -template -SquareMatrix fullRankCholesky(const SquareMatrix & A, - size_t& rank) -{ - // Loses one ulp accuracy per row of diag, relative to largest magnitude - const Type tol = N * typeEpsilon() * A.diag().max(); - - Matrix 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 genInvUnderdetermined(const Matrix & G, const Matrix & L, size_t rank) - { - if (rank < R) { - // Recursive call - return GeninvImpl::genInvUnderdetermined(G, L, rank); - - } else if (rank > R) { - // Error - return Matrix(); - - } else { - // R == rank - Matrix LL = L. template slice(0, 0); - SquareMatrix X = inv(SquareMatrix(LL.transpose() * LL)); - return G.transpose() * LL * X * X * LL.transpose(); - - } - } - - static Matrix genInvOverdetermined(const Matrix & G, const Matrix & L, size_t rank) - { - if (rank < R) { - // Recursive call - return GeninvImpl::genInvOverdetermined(G, L, rank); - - } else if (rank > R) { - // Error - return Matrix(); - - } else { - // R == rank - Matrix LL = L. template slice(0, 0); - SquareMatrix X = inv(SquareMatrix(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 -{ -public: - static Matrix genInvUnderdetermined(const Matrix & G, const Matrix & L, size_t rank) - { - return Matrix(); - } - - static Matrix genInvOverdetermined(const Matrix & G, const Matrix & L, size_t rank) - { - return Matrix(); - } -}; diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp index 8c35351664..ba2721879b 100644 --- a/matrix/SquareMatrix.hpp +++ b/matrix/SquareMatrix.hpp @@ -305,7 +305,7 @@ SquareMatrix expm(const Matrix & A, size_t order=5) * inverse based on LU factorization with partial pivotting */ template -bool inv(const SquareMatrix & A, SquareMatrix & inv) +bool inv(const SquareMatrix & A, SquareMatrix & inv, size_t rank = M) { SquareMatrix L; L.setIdentity(); @@ -316,12 +316,12 @@ bool inv(const SquareMatrix & A, SquareMatrix & 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(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(U(i, n))) > 1e-8f) { @@ -349,12 +349,12 @@ bool inv(const SquareMatrix & A, SquareMatrix & 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 & A, SquareMatrix & inv) //SquareMatrix 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 & A, SquareMatrix & inv) //SquareMatrix 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 & A, SquareMatrix & inv) } //check sanity of results - for (size_t i = 0; i < M; i++) { - for (size_t j = 0; j < M; j++) { + for (size_t i = 0; i < rank; i++) { + for (size_t j = 0; j < rank; j++) { if (!is_finite(P(i,j))) { return false; } diff --git a/test/pseudoInverse.cpp b/test/pseudoInverse.cpp index 58a934fd83..e7d8b10187 100644 --- a/test/pseudoInverse.cpp +++ b/test/pseudoInverse.cpp @@ -111,22 +111,6 @@ int main() Matrix A = geninv(B); TEST((A - A_check).abs().max() < 1e-5); - // Test error case with erroneous rank in internal impl functions - Matrix L; - Matrix GM; - Matrix retM_check; - Matrix retM0 = GeninvImpl::genInvUnderdetermined(GM, L, 5); - Matrix GN; - Matrix retN_check; - Matrix retN0 = GeninvImpl::genInvOverdetermined(GN, L, 5); - TEST((retM0 - retM_check).abs().max() < 1e-5); - TEST((retN0 - retN_check).abs().max() < 1e-5); - - Matrix retM1 = GeninvImpl::genInvUnderdetermined(GM, L, 5); - Matrix retN1 = GeninvImpl::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},