tests: Added test_eigen to verify correctness of eigen calculations

4 files changed, 414 insertions, 1 deletions

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..0c9ee810a
--- /dev/null
+++ b/src/systemcmds/tests/test_eigen.cpp
@@ -0,0 +1,410 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+
+//Hacks to make Eigen compile on NuttX
+#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
+
+#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;
+}
+
+#define TEST_OP(_title, _op) { size_t n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (size_t j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); }
+
+/**
+* @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("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("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> * 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}
 };