aboutsummaryrefslogtreecommitdiff
path: root/src/systemcmds/tests/test_eigen.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/systemcmds/tests/test_eigen.cpp')
-rw-r--r--src/systemcmds/tests/test_eigen.cpp101
1 files changed, 54 insertions, 47 deletions
diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp
index de5630cc3..fb50e0d7a 100644
--- a/src/systemcmds/tests/test_eigen.cpp
+++ b/src/systemcmds/tests/test_eigen.cpp
@@ -52,27 +52,27 @@
namespace Eigen
{
- typedef Matrix<float, 10, 1> Vector10f;
+typedef Matrix<float, 10, 1> Vector10f;
}
static constexpr size_t OPERATOR_ITERATIONS = 60000;
#define TEST_OP(_title, _op) \
-{ \
- const hrt_abstime t0 = hrt_absolute_time(); \
- for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
- _op; \
- } \
- printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
-}
+ { \
+ const hrt_abstime t0 = hrt_absolute_time(); \
+ for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
+ _op; \
+ } \
+ printf(_title ": %.6fus", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
+ }
#define VERIFY_OP(_title, _op, __OP_TEST__) \
-{ \
- _op; \
- if(!(__OP_TEST__)) { \
- printf(_title " Failed! ("#__OP_TEST__")"); \
- } \
-}
+ { \
+ _op; \
+ if(!(__OP_TEST__)) { \
+ printf(_title " Failed! ("#__OP_TEST__")"); \
+ } \
+ }
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
VERIFY_OP(_title, _op, __OP_TEST__) \
@@ -83,18 +83,20 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000;
* Prints an Eigen::Matrix to stdout
**/
template<typename T>
-void printEigen(const Eigen::MatrixBase<T>& b)
+void printEigen(const Eigen::MatrixBase<T> &b)
{
- for(int i = 0; i < b.rows(); ++i) {
+ for (int i = 0; i < b.rows(); ++i) {
printf("(");
- for(int j = 0; j < b.cols(); ++i) {
- if(j > 0) {
+
+ for (int j = 0; j < b.cols(); ++i) {
+ if (j > 0) {
printf(",");
}
printf("%.3f", static_cast<double>(b(i, j)));
}
- printf(")%s\n", i+1 < b.rows() ? "," : "");
+
+ printf(")%s\n", i + 1 < b.rows() ? "," : "");
}
}
@@ -108,7 +110,7 @@ Eigen::Quaternion<T> quatFromEuler(const T roll, const T pitch, const T yaw)
Eigen::AngleAxis<T> rollAngle(roll, Eigen::Matrix<T, 3, 1>::UnitZ());
Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Matrix<T, 3, 1>::UnitY());
Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Matrix<T, 3, 1>::UnitX());
- return rollAngle * yawAngle * pitchAngle;
+ return rollAngle * yawAngle * pitchAngle;
}
@@ -127,9 +129,9 @@ int test_eigen(int argc, char *argv[])
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
- TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1+v1));
+ TEST_OP_VERIFY("Vector2f + Vector2f", v + v1, v.isApprox(v1 + v1));
TEST_OP_VERIFY("Vector2f - Vector2f", v - v1, v.isApprox(v1));
- TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1+v1));
+ TEST_OP_VERIFY("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
TEST_OP_VERIFY("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
@@ -185,7 +187,7 @@ int test_eigen(int argc, char *argv[])
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
}
- {
+ {
Eigen::Vector10f v1;
v1.Zero();
float data[10];
@@ -222,18 +224,18 @@ int test_eigen(int argc, char *argv[])
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
- (Eigen::Matrix<float, 2, 3>() << 1, 3, 3,
- 4, 6, 6).finished();
+ (Eigen::Matrix<float, 2, 3>() << 1, 3, 3,
+ 4, 6, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
Eigen::Matrix<float, 2, 3> m2;
m2 << 2, 4, 6,
- 8, 10, 12;
+ 8, 10, 12;
Eigen::Matrix<float, 2, 3> m3;
m3 << 3, 6, 9,
- 12, 15, 18;
+ 12, 15, 18;
if (m1 + m2 != m3) {
warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
@@ -349,7 +351,7 @@ int test_eigen(int argc, char *argv[])
{
// test quaternion method "rotate" (rotate vector by quaternion)
- Eigen::Vector3f vector = {1.0f,1.0f,1.0f};
+ Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
Eigen::Vector3f vector_q;
Eigen::Vector3f vector_r;
Eigen::Quaternionf q;
@@ -363,11 +365,12 @@ int test_eigen(int argc, char *argv[])
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.eulerAngles(roll, pitch, yaw);
- q = quatFromEuler(roll,pitch,yaw);
- vector_r = R*vector;
+ q = quatFromEuler(roll, pitch, yaw);
+ vector_r = R * vector;
vector_q = q._transformVector(vector);
+
for (int i = 0; i < 3; i++) {
- if(fabsf(vector_r(i) - vector_q(i)) > tol) {
+ if (fabsf(vector_r(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
@@ -378,41 +381,45 @@ int test_eigen(int argc, char *argv[])
// test some values calculated with matlab
tol = 0.0001f;
- q = quatFromEuler(M_PI_2_F,0.0f,0.0f);
+ q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q._transformVector(vector);
- Eigen::Vector3f vector_true = {1.00f,-1.00f,1.00f};
- for(size_t i = 0;i<3;i++) {
- if(fabsf(vector_true(i) - vector_q(i)) > tol) {
+ Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
+
+ for (size_t i = 0; i < 3; i++) {
+ if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
- q = quatFromEuler(0.3f,0.2f,0.1f);
+ q = quatFromEuler(0.3f, 0.2f, 0.1f);
vector_q = q._transformVector(vector);
- vector_true = {1.1566,0.7792,1.0273};
- for(size_t i = 0;i<3;i++) {
- if(fabsf(vector_true(i) - vector_q(i)) > tol) {
+ vector_true = {1.1566, 0.7792, 1.0273};
+
+ for (size_t i = 0; i < 3; i++) {
+ if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
- q = quatFromEuler(-1.5f,-0.2f,0.5f);
+ q = quatFromEuler(-1.5f, -0.2f, 0.5f);
vector_q = q._transformVector(vector);
- vector_true = {0.5095,1.4956,-0.7096};
- for(size_t i = 0;i<3;i++) {
- if(fabsf(vector_true(i) - vector_q(i)) > tol) {
+ vector_true = {0.5095, 1.4956, -0.7096};
+
+ for (size_t i = 0; i < 3; i++) {
+ if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
- q = quatFromEuler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3.0f);
+ q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q._transformVector(vector);
- vector_true = {-1.3660,0.3660,1.0000};
- for(size_t i = 0;i<3;i++) {
- if(fabsf(vector_true(i) - vector_q(i)) > tol) {
+ vector_true = { -1.3660, 0.3660, 1.0000};
+
+ for (size_t i = 0; i < 3; i++) {
+ if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-10 23:11:07 +0000