fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 14:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

geninv(): pass result as argument

Browse Source 
depending on usage, this reduces stack usage a bit.
This commit is contained in:
Beat Küng
2021-09-03 12:07:26 +02:00
parent dc7f119b99
commit 7c1860f286
2 changed files with 28 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/PseudoInverse.hpp
+8 -5
View File
@@ -21,7 +21,7 @@ namespace matrix
* 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)
bool geninv(const Matrix<Type, M, N> & G, Matrix<Type, N, M>& res)
{
size_t rank;
if (M <= N) {
@@ -31,11 +31,12 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
A = L.transpose() * L;
SquareMatrix<Type, M> X;
if (!inv(A, X, rank)) {
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
res = Matrix<Type, N, M>();
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
}
// doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
return G.transpose() * (L * A);
res = G.transpose() * (L * A);
} else {
SquareMatrix<Type, N> A = G.transpose() * G;
@@ -44,12 +45,14 @@ Matrix<Type, N, M> geninv(const Matrix<Type, M, N> & G)
A = L.transpose() * L;
SquareMatrix<Type, N> X;
if(!inv(A, X, rank)) {
return Matrix<Type, N, M>(); // LCOV_EXCL_LINE -- this can only be hit from numerical issues
res = Matrix<Type, N, M>();
return false; // LCOV_EXCL_LINE -- this can only be hit from numerical issues
}
// doing an intermediate assignment reduces stack usage
A = X * X * L.transpose();
return (L * A) * G.transpose();
res = (L * A) * G.transpose();
}
return true;
}
test/pseudoInverse.cpp
+20 -7
View File
@@ -22,7 +22,9 @@ int main()
};
Matrix<float, 3, 4> A0(data0);
Matrix<float, 4, 3> A0_I = geninv(A0);
Matrix<float, 4, 3> A0_I;
bool ret = geninv(A0, A0_I);
TEST(ret);
Matrix<float, 4, 3> A0_I_check(data0_check);
TEST((A0_I - A0_I_check).abs().max() < 1e-5);
@@ -42,7 +44,9 @@ int main()
};
Matrix<float, 4, 3> A1(data1);
Matrix<float, 3, 4> A1_I = geninv(A1);
Matrix<float, 3, 4> A1_I;
ret = geninv(A1, A1_I);
TEST(ret);
Matrix<float, 3, 4> A1_I_check(data1_check);
TEST((A1_I - A1_I_check).abs().max() < 1e-5);
@@ -53,7 +57,8 @@ int main()
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);
ret = geninv(A_large, A_large_I);
TEST(ret);
TEST(isEqual(A_large, A_large_I.T()));
}
@@ -69,13 +74,17 @@ int main()
};
SquareMatrix<float, 3> A2(data2);
SquareMatrix<float, 3> A2_I = geninv(A2);
SquareMatrix<float, 3> A2_I;
ret = geninv(A2, A2_I);
TEST(ret);
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;
ret = geninv(A3, A3_I);
TEST(ret);
Matrix<float, 16, 6> A3_I_check;
TEST((A3_I - A3_I_check).abs().max() < 1e-5);
@@ -108,7 +117,9 @@ int main()
{ 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);
Matrix<float, 16, 6> A;
ret = geninv(B, A);
TEST(ret);
TEST((A - A_check).abs().max() < 1e-5);
// Real-world test case
@@ -120,7 +131,9 @@ int main()
{ 0.607814, -0.607814, 0.607814, -0.607814, 1.0000, 1.0000}
};
Matrix<float, 5, 6> real ( real_alloc);
Matrix<float, 6, 5> real_pinv = geninv(real);
Matrix<float, 6, 5> real_pinv;
ret = geninv(real, real_pinv);
TEST(ret);
// from SVD-based inverse
const float real_pinv_expected_alloc[6][5] = {