Add implementation of pseudo-inverse (PX4#102)

* 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 <[email protected]> * Adapt fullRankCholesky tolerance to type * Add pseudoinverse test with effectiveness matrix * Fix coverage * Fix rebase issue * Fix SquareMatrix test, add null Matrix test
dagar · Nov 18, 2019 · a172c3c · a172c3c
1 parent cd185c9
commit a172c3c
Show file tree

Hide file tree

Showing 7 changed files with 338 additions and 1 deletion.
diff --git 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
@@ -65,7 +65,7 @@ class Dcm : public SquareMatrix<Type, 3>
      *
      * @param _data pointer to array
      */
-    explicit Dcm(const Type data_[3*3]) : SquareMatrix<Type, 3>(data_)
+    explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
     {
     }
 

diff --git a/matrix/PseudoInverse.hpp b/matrix/PseudoInverse.hpp
@@ -0,0 +1,56 @@
+/**
+ * @file PseudoInverse.hpp
+ *
+ * Implementation of matrix pseudo inverse
+ *
+ * @author Julien Lecoeur <[email protected]>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+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
+ *
+ * Courrieu, P. (2008). Fast Computation of Moore-Penrose Inverse Matrices, 8(2), 25–29. http://arxiv.org/abs/0804.4809
+ */
+template<typename Type, size_t M, size_t N>
+Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
+{
+    size_t rank;
+    if (M <= N) {
+        SquareMatrix<Type, M> A = G * G.transpose();
+        SquareMatrix<Type, M> L = fullRankCholesky(A, rank);
+
+        return GeninvImpl<Type, M, N, M>::genInvUnderdetermined(G, L, rank);
+
+    } 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);
+    }
+}
+
+
+#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
@@ -0,0 +1,139 @@
+/**
+ * @file pseudoinverse.hxx
+ *
+ * Implementation of matrix pseudo inverse
+ *
+ * @author Julien Lecoeur <[email protected]>
+ */
+
+
+
+/**
+ * Full rank Cholesky factorization of A
+ */
+template<typename Type>
+void fullRankCholeskyTolerance(Type &tol)
+{
+    tol /= 1000000000;
+}
+
+template<> inline
+void fullRankCholeskyTolerance<double>(double &tol)
+{
+    tol /= 1000000000000000000.0;
+}
+
+template<typename Type, size_t N>
+SquareMatrix<Type, N> fullRankCholesky(const SquareMatrix<Type, N> & A,
+                               size_t& rank)
+{
+    // Compute
+    // dA = np.diag(A)
+    // tol = np.min(dA[dA > 0]) * 1e-9
+    Vector<Type, N> 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<Type>(tol);
+
+    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>();
+    }
+};
diff --git 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
@@ -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
@@ -0,0 +1,139 @@
+#include "test_macros.hpp"
+#include <matrix/PseudoInverse.hpp>
+
+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<float, 3, 4> A0(data0);
+    Matrix<float, 4, 3> A0_I = geninv(A0);
+    Matrix<float, 4, 3> 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<float, 4, 3> A1(data1);
+    Matrix<float, 3, 4> A1_I = geninv(A1);
+    Matrix<float, 3, 4> A1_I_check(data1_check);
+
+    TEST((A1_I - A1_I_check).abs().max() < 1e-5);
+
+    // Stess test
+    Matrix<float, n_large, n_large - 1> A_large;
+    A_large.setIdentity();
+    Matrix<float, n_large - 1, n_large> 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<float, 3> A2(data2);
+    SquareMatrix<float, 3> A2_I = geninv(A2);
+    SquareMatrix<float, 3> A2_I_check(data2_check);
+    TEST((A2_I - A2_I_check).abs().max() < 1e-3);
+
+    // Null matrix test
+    Matrix<float, 6, 16> A3;
+    Matrix<float, 16, 6> A3_I = geninv(A3);
+    Matrix<float, 16, 6> 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<float, 6, 16> B = Matrix<float, 6, 16>(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<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
+    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);
+
+    float float_scale = 1.f;
+    fullRankCholeskyTolerance(float_scale);
+    double double_scale = 1.;
+    fullRankCholeskyTolerance(double_scale);
+    TEST(static_cast<double>(float_scale) > double_scale);
+
+    return 0;
+}
+
+/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
-Original file line number
+Diff line change
@@ Expand Up / @@ -65,7 +65,7 @@ class Dcm : public SquareMatrix<Type, 3> @@
          *
          * @param _data pointer to array
          */
-        explicit Dcm(const Type data_[3*3]) : SquareMatrix<Type, 3>(data_)
+        explicit Dcm(const Type data_[9]) : SquareMatrix<Type, 3>(data_)
         {
         }
@@ Expand Down @@