From a172c3cdac9260369fb5805fcce19067121566e5 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Mon, 18 Nov 2019 23:36:30 +0100 Subject: [PATCH] Add implementation of pseudo-inverse (#102) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix compilation error * Add implementation of pseudo-inverse The implementation is based on this publication: Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809 It is a fully templated implementation to guaranty type correctness. * Add tests for pseudoinverse * Apply suggestions from code review Co-Authored-By: Mathieu Bresciani * Adapt fullRankCholesky tolerance to type * Add pseudoinverse test with effectiveness matrix * Fix coverage * Fix rebase issue * Fix SquareMatrix test, add null Matrix test --- .gitignore | 1 + matrix/Dcm.hpp | 2 +- matrix/PseudoInverse.hpp | 56 ++++++++++++++++ matrix/PseudoInverse.hxx | 139 +++++++++++++++++++++++++++++++++++++++ matrix/math.hpp | 1 + test/CMakeLists.txt | 1 + test/pseudoInverse.cpp | 139 +++++++++++++++++++++++++++++++++++++++ 7 files changed, 338 insertions(+), 1 deletion(-) create mode 100644 matrix/PseudoInverse.hpp create mode 100644 matrix/PseudoInverse.hxx create mode 100644 test/pseudoInverse.cpp diff --git a/.gitignore b/.gitignore index 09e562fc8c..1edf5f525d 100644 --- a/.gitignore +++ b/.gitignore @@ -27,6 +27,7 @@ test/matrixAssignment test/matrixMult test/matrixScalarMult test/out/ +test/pseudoinverse test/setIdentity test/slice test/squareMatrix diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 9a1c6524b2..9b14868707 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -65,7 +65,7 @@ public: * * @param _data pointer to array */ - explicit Dcm(const Type data_[3*3]) : SquareMatrix(data_) + explicit Dcm(const Type data_[9]) : SquareMatrix(data_) { } diff --git a/matrix/PseudoInverse.hpp b/matrix/PseudoInverse.hpp new file mode 100644 index 0000000000..469e358100 --- /dev/null +++ b/matrix/PseudoInverse.hpp @@ -0,0 +1,56 @@ +/** + * @file PseudoInverse.hpp + * + * Implementation of matrix pseudo inverse + * + * @author Julien Lecoeur + */ + +#pragma once + +#include "math.hpp" + +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 + * + * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809 + */ +template +Matrix geninv(const Matrix & G) +{ + size_t rank; + if (M <= N) { + SquareMatrix A = G * G.transpose(); + SquareMatrix L = fullRankCholesky(A, rank); + + return GeninvImpl::genInvUnderdetermined(G, L, rank); + + } else { + SquareMatrix A = G.transpose() * G; + SquareMatrix L = fullRankCholesky(A, rank); + + return GeninvImpl::genInvOverdetermined(G, L, rank); + } +} + + +#include "PseudoInverse.hxx" + +} // namespace matrix + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/matrix/PseudoInverse.hxx b/matrix/PseudoInverse.hxx new file mode 100644 index 0000000000..99408f5e2f --- /dev/null +++ b/matrix/PseudoInverse.hxx @@ -0,0 +1,139 @@ +/** + * @file pseudoinverse.hxx + * + * Implementation of matrix pseudo inverse + * + * @author Julien Lecoeur + */ + + + +/** + * Full rank Cholesky factorization of A + */ +template +void fullRankCholeskyTolerance(Type &tol) +{ + tol /= 1000000000; +} + +template<> inline +void fullRankCholeskyTolerance(double &tol) +{ + tol /= 1000000000000000000.0; +} + +template +SquareMatrix fullRankCholesky(const SquareMatrix & A, + size_t& rank) +{ + // Compute + // dA = np.diag(A) + // tol = np.min(dA[dA > 0]) * 1e-9 + Vector d = A.diag(); + Type tol = d.max(); + for (size_t k = 0; k < N; k++) { + if ((d(k) > 0) && (d(k) < tol)) { + tol = d(k); + } + } + fullRankCholeskyTolerance(tol); + + 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/math.hpp b/matrix/math.hpp index 1aa4b0ade4..1bb9d67243 100644 --- a/matrix/math.hpp +++ b/matrix/math.hpp @@ -18,3 +18,4 @@ #include "AxisAngle.hpp" #include "LeastSquaresSolver.hpp" #include "Dual.hpp" +#include "PseudoInverse.hpp" diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8b0a0b3931..c1f8e7a024 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,7 @@ set(tests least_squares upperRightTriangle dual + pseudoInverse ) add_custom_target(test_build) diff --git a/test/pseudoInverse.cpp b/test/pseudoInverse.cpp new file mode 100644 index 0000000000..476e45dd37 --- /dev/null +++ b/test/pseudoInverse.cpp @@ -0,0 +1,139 @@ +#include "test_macros.hpp" +#include + +using namespace matrix; + +static const size_t n_large = 20; + +int main() +{ + // 3x4 Matrix test + float data0[12] = { + 0.f, 1.f, 2.f, 3.f, + 4.f, 5.f, 6.f, 7.f, + 8.f, 9.f, 10.f, 11.f + }; + + float data0_check[12] = { + -0.3375f, -0.1f, 0.1375f, + -0.13333333f, -0.03333333f, 0.06666667f, + 0.07083333f, 0.03333333f, -0.00416667f, + 0.275f, 0.1f, -0.075f + }; + + Matrix A0(data0); + Matrix A0_I = geninv(A0); + Matrix A0_I_check(data0_check); + + TEST((A0_I - A0_I_check).abs().max() < 1e-5); + + // 4x3 Matrix test + float data1[12] = { + 0.f, 4.f, 8.f, + 1.f, 5.f, 9.f, + 2.f, 6.f, 10.f, + 3.f, 7.f, 11.f + }; + + float data1_check[12] = { + -0.3375f, -0.13333333f, 0.07083333f, 0.275f, + -0.1f, -0.03333333f, 0.03333333f, 0.1f, + 0.1375f, 0.06666667f, -0.00416667f, -0.075f + }; + + Matrix A1(data1); + Matrix A1_I = geninv(A1); + Matrix A1_I_check(data1_check); + + TEST((A1_I - A1_I_check).abs().max() < 1e-5); + + // Stess test + Matrix A_large; + A_large.setIdentity(); + Matrix A_large_I; + + for (size_t i = 0; i < n_large; i++) { + A_large_I = geninv(A_large); + TEST(isEqual(A_large, A_large_I.T())); + } + + // Square matrix test + float data2[9] = {0, 2, 3, + 4, 5, 6, + 7, 8, 10 + }; + float data2_check[9] = { + -0.4f, -0.8f, 0.6f, + -0.4f, 4.2f, -2.4f, + 0.6f, -2.8f, 1.6f + }; + + SquareMatrix A2(data2); + SquareMatrix A2_I = geninv(A2); + SquareMatrix A2_I_check(data2_check); + TEST((A2_I - A2_I_check).abs().max() < 1e-3); + + // Null matrix test + Matrix A3; + Matrix A3_I = geninv(A3); + Matrix A3_I_check; + TEST((A3_I - A3_I_check).abs().max() < 1e-5); + + // Mock-up effectiveness matrix + const float B_quad_w[6][16] = { + {-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}, + {-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f} + }; + Matrix B = Matrix(B_quad_w); + const float A_quad_w[16][6] = { + { -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f }, + { -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f }, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, + { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} + }; + Matrix A_check = Matrix(A_quad_w); + 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); + + float float_scale = 1.f; + fullRankCholeskyTolerance(float_scale); + double double_scale = 1.; + fullRankCholeskyTolerance(double_scale); + TEST(static_cast(float_scale) > double_scale); + + return 0; +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */