From c0c3e153ec5b8fdea6a69a97e17ff149e483c8dd Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 17 Mar 2015 14:51:18 +0100 Subject: [PATCH] Eigen: Add verify macro to check if math operations are correct --- src/systemcmds/tests/test_eigen.cpp | 33 ++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 767ea2d87a..de5630cc3e 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -56,7 +57,7 @@ namespace Eigen static constexpr size_t OPERATOR_ITERATIONS = 60000; -#define TEST_OP(_title, _op) \ +#define TEST_OP(_title, _op) \ { \ const hrt_abstime t0 = hrt_absolute_time(); \ for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \ @@ -65,6 +66,18 @@ static constexpr size_t OPERATOR_ITERATIONS = 60000; printf(_title ": %.6fus", static_cast(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 @@ -110,15 +123,15 @@ int test_eigen(int argc, char *argv[]) Eigen::Vector2f v2(1.0f, -1.0f); float data[2] = {1.0f, 2.0f}; TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3); - TEST_OP("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1)); - TEST_OP("Constructor Vector2f(float[])", Eigen::Vector2f v3(data)); - TEST_OP("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f)); - TEST_OP("Vector2f = Vector2f", v = v1); - TEST_OP("Vector2f + Vector2f", v + v1); - TEST_OP("Vector2f - Vector2f", v - v1); - TEST_OP("Vector2f += Vector2f", v += v1); - TEST_OP("Vector2f -= Vector2f", v -= v1); - TEST_OP("Vector2f dot Vector2f", v.dot(v1)); + 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? }