mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
Use a single inverse implementation for a single matrix size
This commit is contained in:
Julian Kent
Julian Kent
parent
15e54ceda1
commit
054f8b12f4
@ -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
|
||||
|
||||
|
||||
@ -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>();
|
||||
}
|
||||
};
|
||||
@ -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 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;
|
||||
}
|
||||
|
||||
@ -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},
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user