zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Use a single inverse implementation for a single matrix size

master
Julian Kent 4 years ago committed by Julian Kent
parent
15e54ceda1
commit
054f8b12f4
4 changed files with 83 additions and 169 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 84
      matrix/PseudoInverse.hpp
  2. 126
      matrix/PseudoInverse.hxx
  3. 26
      matrix/SquareMatrix.hpp
  4. 16
      test/pseudoInverse.cpp

84
matrix/PseudoInverse.hpp
Unescape View File

@ -4,6 +4,7 @@ @@ -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 @@ @@ -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) @@ -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

126
matrix/PseudoInverse.hxx
Unescape View File

@ -1,126 +0,0 @@ @@ -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>();
}
};

26
matrix/SquareMatrix.hpp
Unescape View File

@ -305,7 +305,7 @@ SquareMatrix<Type, M> expm(const Matrix<Type, M, M> & A, size_t order=5) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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 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;
}

16
test/pseudoInverse.cpp
Unescape View File

@ -111,22 +111,6 @@ int main() @@ -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},

Write Preview
Loading…
Cancel
Save