fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

define FLT_EPSILON; be descriptive about upper_right_triangle() method

Browse Source
This commit is contained in:
TSC21 2019-02-19 14:44:46 +00:00 committed by Nuno Marques
parent 7355a29a2a
commit ec436d5aee
11 changed files with 43 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
matrix/SquareMatrix.hpp
View File

@ -66,8 +66,8 @@ public:
return res;
}
// get matrix upper right triangle
Vector<Type, M * (M + 1) / 2> urt() const
// get matrix upper right triangle in a row-major vector format
Vector<Type, M * (M + 1) / 2> upper_right_triangle() const
{
Vector<Type, M * (M + 1) / 2> res;
const SquareMatrix<Type, M> &self = *this;
@ -149,7 +149,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
for (size_t n = 0; n < M; n++) {
// if diagonal is zero, swap with row below
if (fabs(static_cast<float>(U(n, n))) < __FLT_EPSILON__) {
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++) {
@ -174,7 +174,7 @@ bool inv(const SquareMatrix<Type, M> & A, SquareMatrix<Type, M> & inv)
#endif
// failsafe, return zero matrix
if (fabs(static_cast<float>(U(n, n))) < __FLT_EPSILON__) {
if (fabs(static_cast<float>(U(n, n))) < FLT_EPSILON) {
return false;
}

4
matrix/stdlib_imports.hpp
View File

@ -15,6 +15,10 @@
namespace matrix {
#if !defined(FLT_EPSILON)
#define FLT_EPSILON __FLT_EPSILON__
#endif
#if defined(__PX4_NUTTX)
/*
* NuttX has no usable C++ math library, so we need to provide the needed definitions here manually.

2
test/CMakeLists.txt
View File

@ -18,7 +18,7 @@ set(tests
hatvee
copyto
least_squares
urt
upperRightTriangle
)
add_custom_target(test_build)

10
test/helper.cpp
View File

@ -5,15 +5,15 @@ using namespace matrix;
int main()
{
TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < __FLT_EPSILON__);
TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < __FLT_EPSILON__);
TEST(fabs(wrap_pi(3.0) - (3.0)) < __FLT_EPSILON__);
TEST(fabs(wrap_pi(4.0) - (4.0 - 2*M_PI)) < FLT_EPSILON);
TEST(fabs(wrap_pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON);
TEST(fabs(wrap_pi(3.0) - (3.0)) < FLT_EPSILON);
TEST(!is_finite(wrap_pi(1000.0f)));
TEST(!is_finite(wrap_pi(-1000.0f)));
wrap_pi(NAN);
TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < __FLT_EPSILON__);
TEST(fabs(wrap_2pi(3.0) - (3.0)) < __FLT_EPSILON__);
TEST(fabs(wrap_2pi(-4.0) - (-4.0 + 2*M_PI)) < FLT_EPSILON);
TEST(fabs(wrap_2pi(3.0) - (3.0)) < FLT_EPSILON);
TEST(!is_finite(wrap_2pi(1000.0f)));
TEST(!is_finite(wrap_2pi(-1000.0f)));
wrap_2pi(NAN);

14
test/matrixAssignment.cpp
View File

@ -22,7 +22,7 @@ int main()
Matrix3f m2(data);
for(int i=0; i<9; i++) {
TEST(fabs(data[i] - m2.data()[i]) < __FLT_EPSILON__);
TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON);
}
Matrix3f m_nan;
@ -38,7 +38,7 @@ int main()
};
m2 = Matrix3f(data2d);
for(int i=0; i<9; i++) {
TEST(fabs(data[i] - m2.data()[i]) < __FLT_EPSILON__);
TEST(fabs(data[i] - m2.data()[i]) < FLT_EPSILON);
}
float data_times_2[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
@ -100,16 +100,16 @@ int main()
m4.swapCols(2, 2);
TEST(isEqual(m4, Matrix3f(data)));
TEST(fabs(m4.min() - 1) < __FLT_EPSILON__);
TEST(fabs((-m4).min() + 9) < __FLT_EPSILON__);
TEST(fabs(m4.min() - 1) < FLT_EPSILON);
TEST(fabs((-m4).min() + 9) < FLT_EPSILON);
Scalar<float> s = 1;
const Vector<float, 1> & s_vect = s;
TEST(fabs(s - 1) < __FLT_EPSILON__);
TEST(fabs(s_vect(0) - 1.0f) < __FLT_EPSILON__);
TEST(fabs(s - 1) < FLT_EPSILON);
TEST(fabs(s_vect(0) - 1.0f) < FLT_EPSILON);
Matrix<float, 1, 1> m5 = s;
TEST(fabs(m5(0,0) - s) < __FLT_EPSILON__);
TEST(fabs(m5(0,0) - s) < FLT_EPSILON);
Matrix<float, 2, 2> m6;
m6.setRow(0, Vector2f(1, 2));

8
test/setIdentity.cpp
View File

@ -11,10 +11,10 @@ int main()
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST(fabs(A(i, j) - 1) < __FLT_EPSILON__);
TEST(fabs(A(i, j) - 1) < FLT_EPSILON);
} else {
TEST(fabs(A(i, j) - 0) < __FLT_EPSILON__);
TEST(fabs(A(i, j) - 0) < FLT_EPSILON);
}
}
}
@ -25,10 +25,10 @@ int main()
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (i == j) {
TEST(fabs(B(i, j) - 1) < __FLT_EPSILON__);
TEST(fabs(B(i, j) - 1) < FLT_EPSILON);
} else {
TEST(fabs(B(i, j) - 0) < __FLT_EPSILON__);
TEST(fabs(B(i, j) - 0) < FLT_EPSILON);
}
}
}

2
test/squareMatrix.cpp
View File

@ -14,7 +14,7 @@ int main()
Vector3<float> diag_check(1, 5, 10);
TEST(isEqual(A.diag(), diag_check));
TEST(A.trace() - 16 < __FLT_EPSILON__);
TEST(A.trace() - 16 < FLT_EPSILON);
float data_check[9] = {
1.01158503f, 0.02190432f, 0.03238144f,

10
test/test_data.py
View File

@ -75,9 +75,9 @@ psi = 0.3
print('euler', phi, theta, psi)
q = euler_to_quat(phi, theta, psi)
assert(abs(norm(q) - 1) < __FLT_EPSILON__)
assert(abs(norm(q) - 1) < __FLT_EPSILON__)
assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < __FLT_EPSILON__)
assert(abs(norm(q) - 1) < FLT_EPSILON)
assert(abs(norm(q) - 1) < FLT_EPSILON)
assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < FLT_EPSILON)
print('\nq:')
pprint(q)
@ -85,8 +85,8 @@ dcm = euler_to_dcm(phi, theta, psi)
assert(norm(dcm[:,0]) == 1)
assert(norm(dcm[:,1]) == 1)
assert(norm(dcm[:,2]) == 1)
assert(abs(dcm[:,0].dot(dcm[:,1])) < __FLT_EPSILON__)
assert(abs(dcm[:,0].dot(dcm[:,2])) < __FLT_EPSILON__)
assert(abs(dcm[:,0].dot(dcm[:,1])) < FLT_EPSILON)
assert(abs(dcm[:,0].dot(dcm[:,2])) < FLT_EPSILON)
print('\ndcm:')
pprint(dcm)

1
test/test_macros.hpp
View File

@ -10,7 +10,6 @@
#include <cstdio>
#include <cmath> // cmath has to be introduced BEFORE we poison the C library identifiers
#include "float.h"
#define TEST(X) if(!(X)) { fprintf(stderr, "test failed on %s:%d\n", __FILE__, __LINE__); return -1;}

2
test/urt.cpp → test/upperRightTriangle.cpp
View File

@ -14,7 +14,7 @@ int main()
SquareMatrix<float, 3> A(data);
for(int i=0; i<6; i++) {
TEST(fabs(urt[i] - A.urt().data()[i]) < __FLT_EPSILON__);
TEST(fabs(urt[i] - A.upper_right_triangle().data()[i]) < FLT_EPSILON);
}
return 0;

22
test/vector2.cpp
View File

@ -10,29 +10,29 @@ int main()
{
Vector2f a(1, 0);
Vector2f b(0, 1);
TEST(fabs(a % b - 1.0f) < __FLT_EPSILON__);
TEST(fabs(a % b - 1.0f) < FLT_EPSILON);
Vector2f c;
TEST(fabs(c(0) - 0) < __FLT_EPSILON__);
TEST(fabs(c(1) - 0) < __FLT_EPSILON__);
TEST(fabs(c(0) - 0) < FLT_EPSILON);
TEST(fabs(c(1) - 0) < FLT_EPSILON);
Matrix<float, 2, 1> d(a);
TEST(fabs(d(0,0) - 1) < __FLT_EPSILON__);
TEST(fabs(d(1,0) - 0) < __FLT_EPSILON__);
TEST(fabs(d(0,0) - 1) < FLT_EPSILON);
TEST(fabs(d(1,0) - 0) < FLT_EPSILON);
Vector2f e(d);
TEST(fabs(e(0) - 1) < __FLT_EPSILON__);
TEST(fabs(e(1) - 0) < __FLT_EPSILON__);
TEST(fabs(e(0) - 1) < FLT_EPSILON);
TEST(fabs(e(1) - 0) < FLT_EPSILON);
float data[] = {4,5};
Vector2f f(data);
TEST(fabs(f(0) - 4) < __FLT_EPSILON__);
TEST(fabs(f(1) - 5) < __FLT_EPSILON__);
TEST(fabs(f(0) - 4) < FLT_EPSILON);
TEST(fabs(f(1) - 5) < FLT_EPSILON);
Vector3f g(1.23f, 423.4f, 3221.f);
Vector2f h(g);
TEST(fabs(h(0) - 1.23f) < __FLT_EPSILON__);
TEST(fabs(h(1) - 423.4f) < __FLT_EPSILON__);
TEST(fabs(h(0) - 1.23f) < FLT_EPSILON);
TEST(fabs(h(1) - 423.4f) < FLT_EPSILON);
return 0;
}