fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 14:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib delete Matrix, Quaternion, Vector

Browse Source
This commit is contained in:
Daniel Agar
2018-03-28 17:05:42 -04:00
parent 0d7b5c4a4e
commit 222a91c6be
32 changed files with 160 additions and 1746 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/test_mathlib.cpp
+88 -144
View File
@@ -53,6 +53,7 @@
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <matrix/math.hpp>
#include "tests_main.h"
@@ -64,7 +65,6 @@ public:
private:
bool testVector2();
bool testVector3();
bool testVector4();
bool testVector10();
bool testMatrix3x3();
bool testMatrix10x10();
@@ -82,21 +82,21 @@ using namespace math;
bool MathlibTest::testVector2()
{
{
Vector<2> v;
Vector<2> v1(1.0f, 2.0f);
Vector<2> v2(1.0f, -1.0f);
matrix::Vector2f v;
matrix::Vector2f v1(1.0f, 2.0f);
matrix::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector<2>()", Vector<2> v3);
TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
TEST_OP("Vector<2> = Vector<2>", v = v1);
TEST_OP("Vector<2> + Vector<2>", v + v1);
TEST_OP("Vector<2> - Vector<2>", v - v1);
TEST_OP("Vector<2> += Vector<2>", v += v1);
TEST_OP("Vector<2> -= Vector<2>", v -= v1);
TEST_OP("Vector<2> * Vector<2>", v * v1);
TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
TEST_OP("Constructor matrix::Vector2f()", matrix::Vector2f v3);
TEST_OP("Constructor matrix::Vector2f(matrix::Vector2f)", matrix::Vector2f v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor matrix::Vector2f(float[])", matrix::Vector2f v3(data));
TEST_OP("Constructor matrix::Vector2f(float, float)", matrix::Vector2f v3(1.0f, 2.0f));
TEST_OP("matrix::Vector2f = matrix::Vector2f", v = v1);
TEST_OP("matrix::Vector2f + matrix::Vector2f", v + v1);
TEST_OP("matrix::Vector2f - matrix::Vector2f", v - v1);
TEST_OP("matrix::Vector2f += matrix::Vector2f", v += v1);
TEST_OP("matrix::Vector2f -= matrix::Vector2f", v -= v1);
TEST_OP("matrix::Vector2f * matrix::Vector2f", v * v1);
TEST_OP("matrix::Vector2f %% matrix::Vector2f", v1 % v2);
}
return true;
}
@@ -105,70 +105,33 @@ bool MathlibTest::testVector3()
{
{
Vector<3> v;
Vector<3> v1(1.0f, 2.0f, 0.0f);
Vector<3> v2(1.0f, -1.0f, 2.0f);
matrix::Vector3f v;
matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
matrix::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector<3>()", Vector<3> v3);
TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector<3> = Vector<3>", v = v1);
TEST_OP("Vector<3> + Vector<3>", v + v1);
TEST_OP("Vector<3> - Vector<3>", v - v1);
TEST_OP("Vector<3> += Vector<3>", v += v1);
TEST_OP("Vector<3> -= Vector<3>", v -= v1);
TEST_OP("Vector<3> * float", v1 * 2.0f);
TEST_OP("Vector<3> / float", v1 / 2.0f);
TEST_OP("Vector<3> *= float", v1 *= 2.0f);
TEST_OP("Vector<3> /= float", v1 /= 2.0f);
TEST_OP("Vector<3> * Vector<3>", v * v1);
TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
TEST_OP("Vector<3> length", v1.length());
TEST_OP("Vector<3> length squared", v1.length_squared());
TEST_OP("Constructor matrix::Vector3f()", matrix::Vector3f v3);
TEST_OP("Constructor matrix::Vector3f(matrix::Vector3f)", matrix::Vector3f v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor matrix::Vector3f(float[])", matrix::Vector3f v3(data));
TEST_OP("Constructor matrix::Vector3f(float, float, float)", matrix::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("matrix::Vector3f = matrix::Vector3f", v = v1);
TEST_OP("matrix::Vector3f + matrix::Vector3f", v + v1);
TEST_OP("matrix::Vector3f - matrix::Vector3f", v - v1);
TEST_OP("matrix::Vector3f += matrix::Vector3f", v += v1);
TEST_OP("matrix::Vector3f -= matrix::Vector3f", v -= v1);
TEST_OP("matrix::Vector3f * float", v1 * 2.0f);
TEST_OP("matrix::Vector3f / float", v1 / 2.0f);
TEST_OP("matrix::Vector3f *= float", v1 *= 2.0f);
TEST_OP("matrix::Vector3f /= float", v1 /= 2.0f);
TEST_OP("matrix::Vector3f * matrix::Vector3f", v * v1);
TEST_OP("matrix::Vector3f %% matrix::Vector3f", v1 % v2);
TEST_OP("matrix::Vector3f length", v1.length());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here instead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
TEST_OP("matrix::Vector3f element read", volatile float a = v1(0));
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
}
return true;
}
bool MathlibTest::testVector4()
{
{
Vector<4> v;
Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Vector<4> v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> * Vector<4>", v * v1);
}
return true;
}
bool MathlibTest::testVector10()
{
{
Vector<10> v1;
v1.zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Vector<10> v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1); ut_assert_true(v3 == v1); v3.zero());
TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
TEST_OP("matrix::Vector3f element write", v1(0) = 1.0f);
}
return true;
}
@@ -176,30 +139,14 @@ bool MathlibTest::testVector10()
bool MathlibTest::testMatrix3x3()
{
{
Matrix<3, 3> m1;
matrix::Matrix3f m1;
m1.identity();
Matrix<3, 3> m2;
matrix::Matrix3f m2;
m2.identity();
Vector<3> v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
}
return true;
}
bool MathlibTest::testMatrix10x10()
{
{
Matrix<10, 10> m1;
m1.identity();
Matrix<10, 10> m2;
m2.identity();
Vector<10> v1;
v1.zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
matrix::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("matrix::Matrix3f * matrix::Vector3f", m1 * v1);
TEST_OP("matrix::Matrix3f + matrix::Matrix3f", m1 + m2);
TEST_OP("matrix::Matrix3f * matrix::Matrix3f", m1 * m2);
}
return true;
}
@@ -215,12 +162,12 @@ bool MathlibTest::testMatrixNonsymmetric()
float data2[2][3] = {{2, 4, 6}, {8, 10, 12}};
float data3[2][3] = {{3, 6, 9}, {12, 15, 18}};
Matrix<2, 3> m1(data1);
Matrix<2, 3> m2(data2);
Matrix<2, 3> m3(data3);
matrix::Matrix<float, 2, 3> m1(data1);
matrix::Matrix<float, 2, 3> m2(data2);
matrix::Matrix<float, 2, 3> m3(data3);
if (m1 + m2 != m3) {
PX4_ERR("Matrix<2, 3> + Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> + matrix::Matrix<float, 2, 3> failed!");
(m1 + m2).print();
printf("!=\n");
m3.print();
@@ -230,7 +177,7 @@ bool MathlibTest::testMatrixNonsymmetric()
ut_assert("m1 + m2 == m3", m1 + m2 == m3);
if (m3 - m2 != m1) {
PX4_ERR("Matrix<2, 3> - Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> - matrix::Matrix<float, 2, 3> failed!");
(m3 - m2).print();
printf("!=\n");
m1.print();
@@ -242,7 +189,7 @@ bool MathlibTest::testMatrixNonsymmetric()
m1 += m2;
if (m1 != m3) {
PX4_ERR("Matrix<2, 3> += Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> += matrix::Matrix<float, 2, 3> failed!");
m1.print();
printf("!=\n");
m3.print();
@@ -252,10 +199,10 @@ bool MathlibTest::testMatrixNonsymmetric()
ut_assert("m1 == m3", m1 == m3);
m1 -= m2;
Matrix<2, 3> m1_orig(data1);
matrix::Matrix<float, 2, 3> m1_orig(data1);
if (m1 != m1_orig) {
PX4_ERR("Matrix<2, 3> -= Matrix<2, 3> failed!");
PX4_ERR("matrix::Matrix<float, 2, 3> -= matrix::Matrix<float, 2, 3> failed!");
m1.print();
printf("!=\n");
m1_orig.print();
@@ -271,9 +218,9 @@ bool MathlibTest::testMatrixNonsymmetric()
bool MathlibTest::testRotationMatrixQuaternion()
{
// test conversion rotation matrix to quaternion and back
math::Matrix<3, 3> R_orig;
math::Matrix<3, 3> R;
math::Quaternion q;
matrix::Dcmf R_orig;
matrix::Dcmf R;
matrix::Quatf q;
float diff = 0.2f;
float tol = 0.00001f;
@@ -282,13 +229,13 @@ bool MathlibTest::testRotationMatrixQuaternion()
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.from_euler(roll, pitch, yaw);
q.from_dcm(R_orig);
R = q.to_dcm();
R_orig = matrix::Eulerf(roll, pitch, yaw);
q = R_orig;
R = q;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
ut_assert("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig.data[i][j] - R.data[i][j]) < tol);
ut_assert("matrix::Quatf method 'from_dcm' or 'to_dcm' outside tolerance!", fabsf(R_orig(i, j) - R(i, j)) < tol);
}
}
}
@@ -303,16 +250,16 @@ bool MathlibTest::testQuaternionfrom_dcm()
{
// test against some known values
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
matrix::Matrix3f R_orig;
R_orig.identity();
math::Quaternion q;
matrix::Quatf q;
q.from_dcm(R_orig);
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_dcm()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_dcm()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
return true;
@@ -321,33 +268,33 @@ bool MathlibTest::testQuaternionfrom_dcm()
bool MathlibTest::testQuaternionfrom_euler()
{
float tol = 0.0001f;
math::Quaternion q_true = {1.0f, 0.0f, 0.0f, 0.0f};
matrix::Quatf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
math::Matrix<3, 3> R_orig;
matrix::Matrix3f R_orig;
R_orig.identity();
math::Quaternion q;
matrix::Quatf q;
q.from_dcm(R_orig);
q_true.from_euler(0.3f, 0.2f, 0.1f);
q_true = matrix::Eulerf(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
q_true.from_euler(-1.5f, -0.2f, 0.5f);
q_true = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
q_true.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q_true = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (unsigned i = 0; i < 4; i++) {
ut_assert("Quaternion method 'from_euler()' outside tolerance!", fabsf(q.data[i] - q_true.data[i]) < tol);
ut_assert("matrix::Quatf method 'from_euler()' outside tolerance!", fabsf(q(i) - q_true(i)) < tol);
}
return true;
@@ -356,26 +303,26 @@ bool MathlibTest::testQuaternionfrom_euler()
bool MathlibTest::testQuaternionRotate()
{
// test quaternion method "rotate" (rotate vector by quaternion)
Vector<3> vector = {1.0f, 1.0f, 1.0f};
Vector<3> vector_q;
Vector<3> vector_r;
Quaternion q;
Matrix<3, 3> R;
matrix::Vector3f vector = {1.0f, 1.0f, 1.0f};
matrix::Vector3f vector_q;
matrix::Vector3f vector_r;
matrix::Quatf q;
matrix::Dcmf R;
float diff = 0.2f;
float tol = 0.00001f;
//PX4_INFO("Quaternion vector rotation method test.");
//PX4_INFO("matrix::Quatf vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.from_euler(roll, pitch, yaw);
q.from_euler(roll, pitch, yaw);
R = matrix::Eulerf(roll, pitch, yaw);
q = matrix::Eulerf(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q.conjugate(vector);
for (int i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_r(i) - vector_q(i)) < tol);
}
}
}
@@ -384,36 +331,36 @@ bool MathlibTest::testQuaternionRotate()
// test some values calculated with matlab
tol = 0.0001f;
q.from_euler(M_PI_2_F, 0.0f, 0.0f);
q = matrix::Eulerf(M_PI_2_F, 0.0f, 0.0f);
vector_q = q.conjugate(vector);
Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
matrix::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(0.3f, 0.2f, 0.1f);
q = matrix::Eulerf(0.3f, 0.2f, 0.1f);
vector_q = q.conjugate(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(-1.5f, -0.2f, 0.5f);
q = matrix::Eulerf(-1.5f, -0.2f, 0.5f);
vector_q = q.conjugate(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
q = matrix::Eulerf(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q.conjugate(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (unsigned i = 0; i < 3; i++) {
ut_assert("Quaternion method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
ut_assert("matrix::Quatf method 'rotate' outside tolerance", fabsf(vector_true(i) - vector_q(i)) < tol);
}
return true;
@@ -423,10 +370,7 @@ bool MathlibTest::run_tests()
{
ut_run_test(testVector2);
ut_run_test(testVector3);
ut_run_test(testVector4);
ut_run_test(testVector10);
ut_run_test(testMatrix3x3);
ut_run_test(testMatrix10x10);
ut_run_test(testMatrixNonsymmetric);
ut_run_test(testRotationMatrixQuaternion);
ut_run_test(testQuaternionfrom_dcm);