mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 14:47:44 +08:00
Add implementation of pseudo-inverse (#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 <brescianimathieu@gmail.com> * Adapt fullRankCholesky tolerance to type * Add pseudoinverse test with effectiveness matrix * Fix coverage * Fix rebase issue * Fix SquareMatrix test, add null Matrix test
This commit is contained in:
Julien Lecoeur
committed by
Julian Kent
Julian Kent
parent
cd185c995b
commit
a172c3cdac
@@ -27,6 +27,7 @@ test/matrixAssignment
|
||||
test/matrixMult
|
||||
test/matrixScalarMult
|
||||
test/out/
|
||||
test/pseudoinverse
|
||||
test/setIdentity
|
||||
test/slice
|
||||
test/squareMatrix
|
||||
|
||||
+1
-1
@@ -65,7 +65,7 @@ public:
|
||||
*
|
||||
* @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_)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
/**
|
||||
* @file PseudoInverse.hpp
|
||||
*
|
||||
* Implementation of matrix pseudo inverse
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#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 : */
|
||||
@@ -0,0 +1,139 @@
|
||||
/**
|
||||
* @file pseudoinverse.hxx
|
||||
*
|
||||
* Implementation of matrix pseudo inverse
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* 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>();
|
||||
}
|
||||
};
|
||||
@@ -18,3 +18,4 @@
|
||||
#include "AxisAngle.hpp"
|
||||
#include "LeastSquaresSolver.hpp"
|
||||
#include "Dual.hpp"
|
||||
#include "PseudoInverse.hpp"
|
||||
|
||||
@@ -20,6 +20,7 @@ set(tests
|
||||
least_squares
|
||||
upperRightTriangle
|
||||
dual
|
||||
pseudoInverse
|
||||
)
|
||||
|
||||
add_custom_target(test_build)
|
||||
|
||||
@@ -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 : */
|
||||
Reference in New Issue
Block a user