Eigen: Add verify macro to check if math operations are correct

This commit is contained in:
Johan Jansen 2015-03-17 14:51:18 +01:00
parent 3451a4686a
commit c0c3e153ec
1 changed files with 23 additions and 10 deletions

View File

@ -39,6 +39,7 @@
*/
#include <px4_eigen.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
@ -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<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
@ -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?
}