diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-03-19 23:45:55 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-03-19 23:45:55 +0100 |
commit | fb040f91174a5369aa7dd5b272015995ba01acba (patch) | |
tree | 161cab04b158bcdf6ec81e3e4e83b461b44ad9af | |
parent | 310a6b421293b42850835384a1c7f9cc70beaf57 (diff) | |
parent | c0c3e153ec5b8fdea6a69a97e17ff149e483c8dd (diff) | |
download | px4-firmware-fb040f91174a5369aa7dd5b272015995ba01acba.tar.gz px4-firmware-fb040f91174a5369aa7dd5b272015995ba01acba.tar.bz2 px4-firmware-fb040f91174a5369aa7dd5b272015995ba01acba.zip |
Merge pull request #1931 from Zefz/eigen-tests
Added test_eigen to verify correctness of eigen calculations
-rw-r--r-- | src/lib/px4_eigen.h | 51 | ||||
-rw-r--r-- | src/systemcmds/tests/module.mk | 3 | ||||
-rw-r--r-- | src/systemcmds/tests/test_eigen.cpp | 422 | ||||
-rw-r--r-- | src/systemcmds/tests/tests.h | 1 | ||||
-rw-r--r-- | src/systemcmds/tests/tests_main.c | 1 |
5 files changed, 477 insertions, 1 deletions
diff --git a/src/lib/px4_eigen.h b/src/lib/px4_eigen.h new file mode 100644 index 000000000..2cd98e59a --- /dev/null +++ b/src/lib/px4_eigen.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_eigen.h + * + * Compatability header to make Eigen compile on the PX4 stack + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#pragma once + +#pragma GCC diagnostic push +#define RAND_MAX __RAND_MAX +#pragma GCC diagnostic ignored "-Wshadow" +#pragma GCC diagnostic ignored "-Wfloat-equal" +#define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1 + +#include <eigen/Eigen/Core> +#include <eigen/Eigen/Geometry> +#pragma GCC diagnostic pop diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1d728e857..4b7b73ac6 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -32,7 +32,8 @@ SRCS = test_adc.c \ test_ppm_loopback.c \ test_rc.c \ test_conv.cpp \ - test_mount.c + test_mount.c \ + test_eigen.cpp EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp new file mode 100644 index 000000000..de5630cc3 --- /dev/null +++ b/src/systemcmds/tests/test_eigen.cpp @@ -0,0 +1,422 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_eigen.cpp + * + * Eigen test (based of mathlib test) + * @author Johan Jansen <jnsn.johan@gmail.com> + */ + +#include <px4_eigen.h> +#include <float.h> +#include <stdio.h> +#include <stdlib.h> +#include <unistd.h> +#include <string.h> +#include <time.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> + +#include "tests.h" + +namespace Eigen +{ + 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); \ +} + +#define VERIFY_OP(_title, _op, __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__) \ + TEST_OP(_title, _op) + +/** +* @brief +* Prints an Eigen::Matrix to stdout +**/ +template<typename T> +void printEigen(const Eigen::MatrixBase<T>& b) +{ + for(int i = 0; i < b.rows(); ++i) { + printf("("); + 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() ? "," : ""); + } +} + +/** +* @brief +* Construct new Eigen::Quaternion from euler angles +**/ +template<typename T> +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; +} + + +int test_eigen(int argc, char *argv[]) +{ + int rc = 0; + warnx("testing eigen"); + + { + Eigen::Vector2f v; + Eigen::Vector2f v1(1.0f, 2.0f); + Eigen::Vector2f v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); + TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1)); + 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)); + 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? + } + + { + Eigen::Vector3f v; + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + Eigen::Vector3f v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3); + TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1)); + TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data)); + TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector3f = Vector3f", v = v1); + TEST_OP("Vector3f + Vector3f", v + v1); + TEST_OP("Vector3f - Vector3f", v - v1); + TEST_OP("Vector3f += Vector3f", v += v1); + TEST_OP("Vector3f -= Vector3f", v -= v1); + TEST_OP("Vector3f * float", v1 * 2.0f); + TEST_OP("Vector3f / float", v1 / 2.0f); + TEST_OP("Vector3f *= float", v1 *= 2.0f); + TEST_OP("Vector3f /= float", v1 /= 2.0f); + TEST_OP("Vector3f dot Vector3f", v.dot(v1)); + TEST_OP("Vector3f cross Vector3f", v1.cross(v2)); + TEST_OP("Vector3f length", v1.norm()); + TEST_OP("Vector3f length squared", v1.squaredNorm()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" + // Need pragma here intead 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]); +#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); + } + + { + Eigen::Vector4f v; + Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f); + Eigen::Vector4f 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>()", Eigen::Vector4f v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f 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> dot Vector<4>", v.dot(v1)); + } + + { + Eigen::Vector10f v1; + v1.Zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data)); + } + + { + Eigen::Matrix3f m1; + m1.setIdentity(); + Eigen::Matrix3f m2; + m2.setIdentity(); + Eigen::Vector3f v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix3f * Vector3f", m1 * v1); + TEST_OP("Matrix3f + Matrix3f", m1 + m2); + TEST_OP("Matrix3f * Matrix3f", m1 * m2); + } + + { + Eigen::Matrix<float, 10, 10> m1; + m1.setIdentity(); + Eigen::Matrix<float, 10, 10> m2; + m2.setIdentity(); + Eigen::Vector10f 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); + } + + { + warnx("Nonsymmetric matrix operations test"); + // 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> m1(m1_orig); + + Eigen::Matrix<float, 2, 3> m2; + m2 << 2, 4, 6, + 8, 10, 12; + + Eigen::Matrix<float, 2, 3> m3; + m3 << 3, 6, 9, + 12, 15, 18; + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + printEigen(m1 + m2); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + printEigen(m3 - m2); + printf("!=\n"); + printEigen(m1); + rc = 1; + } + + m1 += m2; + + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m3); + rc = 1; + } + + m1 -= m2; + + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + printEigen(m1); + printf("!=\n"); + printEigen(m1_orig); + rc = 1; + } + + } + + { + // test conversion rotation matrix to quaternion and back + Eigen::Matrix3f R_orig; + Eigen::Matrix3f R; + Eigen::Quaternionf q; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion transformation methods 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_orig.eulerAngles(roll, pitch, yaw); + q = R_orig; //from_dcm + R = q.toRotationMatrix(); + + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + if (fabsf(R_orig(i, j) - R(i, j)) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f}; + R_orig.setIdentity(); + q = R_orig; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(0.3f, 0.2f, 0.1f); + q = {0.9833f, 0.1436f, 0.1060f, 0.0343f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(-1.5f, -0.2f, 0.5f); + q = {0.7222f, -0.6391f, -0.2386f, 0.1142f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3); + q = {0.6830f, 0.1830f, -0.6830f, 0.1830f}; + + for (size_t i = 0; i < 4; i++) { + if (!q.isApprox(q_true, tol)) { + warnx("Quaternion method 'eulerAngles()' outside tolerance!"); + rc = 1; + } + } + + } + + { + // test quaternion method "rotate" (rotate vector by quaternion) + Eigen::Vector3f vector = {1.0f,1.0f,1.0f}; + Eigen::Vector3f vector_q; + Eigen::Vector3f vector_r; + Eigen::Quaternionf q; + Eigen::Matrix3f R; + float diff = 0.1f; + float tol = 0.00001f; + + warnx("Quaternion 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.eulerAngles(roll, pitch, yaw); + 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) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + } + } + + // test some values calculated with matlab + tol = 0.0001f; + 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) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + 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) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + 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) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + + 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) { + warnx("Quaternion method 'rotate' outside tolerance"); + rc = 1; + } + } + } + return rc; +}
\ No newline at end of file diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index 69fb0e57b..9d13d50d7 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -113,6 +113,7 @@ extern int test_rc(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_mount(int argc, char *argv[]); extern int test_mathlib(int argc, char *argv[]); +extern int test_eigen(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 0768c1d5a..bad8d9cc3 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -112,6 +112,7 @@ const struct { #ifndef TESTS_MATHLIB_DISABLE {"mathlib", test_mathlib, 0}, #endif + {"eigen", test_eigen, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; |