add tests for Bezier and Golden Section Search libraries

src/systemcmds/tests/CMakeLists.txt

@@ -34,6 +34,7 @@
 set(srcs
 	test_adc.c
 	test_autodeclination.cpp
+	test_bezierQuad.cpp
 	test_bson.c
 	test_conv.cpp
 	test_dataman.c
@@ -60,6 +61,7 @@ set(srcs
 	test_perf.c
 	test_ppm_loopback.c
 	test_rc.c
+	test_search_min.cpp
 	test_sensors.c
 	test_servo.c
 	test_sleep.c

src/systemcmds/tests/test_bezierQuad.cpp

+#include <unit_test.h>
+#include <float.h>
+#include <stdlib.h>
+#include <time.h>
+
+#include "../../lib/bezier/BezierQuad.hpp"
+
+class BezierQuadTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+
+	bool _get_states_from_time();
+	bool _get_arc_length();
+	bool _set_bez_from_vel();
+
+	float random(float min, float max);
+
+};
+
+
+bool BezierQuadTest::run_tests()
+{
+	ut_run_test(_get_states_from_time);
+	ut_run_test(_get_arc_length);
+	ut_run_test(_set_bez_from_vel);
+
+	return (_tests_failed == 0);
+}
+
+bool BezierQuadTest::_get_states_from_time()
+{
+	// symmetric around 0
+	matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f);
+	matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f);
+	matrix::Vector3f pt1(0.5f, 0.0f, 0.0f);
+
+	// create bezier with default t = [0,1]
+	bezier::BezierQuadf bz(pt0, ctrl, pt1);
+
+	matrix::Vector3f pos, vel, acc;
+	float precision = 0.00001;
+
+	// states at time = 0
+	bz.getStates(pos, vel, acc, 0.0f);
+
+	ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision);
+	ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision);
+	ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision);
+
+	ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
+	ut_compare_float("slope not equal 1", vel(1), 1.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
+	ut_compare_float("acc not equal 1", acc(1), -2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	// states at time = 1
+	bz.getStates(pos, vel, acc, 1.0f);
+
+	ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision);
+	ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision);
+	ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision);
+
+	ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
+	ut_compare_float("slope not equal -1", vel(1), -1.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
+	ut_compare_float("acc not equal 1", acc(1), -2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	// states at time = 0.5
+	bz.getStates(pos, vel, acc, 0.50f);
+
+	// pos must be equal to ctrl(0) and lower than ctrl(1)
+	ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision);
+	ut_assert_true(pos(1) < ctrl(1));
+
+	ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
+	ut_compare_float("slope not equal -1", vel(1), 0.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 0", acc(0), 0.0f, precision);
+	ut_compare_float("acc not equal -2", acc(1), -2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	// acceleration
+	pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f);
+	ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f);
+	pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f);
+
+	// create bezier with default t = [0,1]
+	bz.setBezier(pt0, ctrl, pt1, 1.0f);
+
+	// states at time = 0.0
+	bz.getStates(pos, vel, acc, 0.0f);
+
+	ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision);
+	ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision);
+	ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision);
+
+	ut_compare_float("slope not equal 0", vel(0), 0.0f, precision);
+	ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	// states at time = 1.0
+	bz.getStates(pos, vel, acc, 1.0f);
+
+	ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision);
+	ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision);
+	ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision);
+
+	ut_compare_float("slope not equal 2", vel(0), 2.0f, precision);
+	ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	// states at time = 0.5
+	bz.getStates(pos, vel, acc, 0.5f);
+
+	ut_compare_float("slope not equal 1", vel(0), 1.0f, precision);
+	ut_compare_float("slope not equal 0", vel(1), 0.0f, precision);
+	ut_compare_float("slope not equal 0", vel(2), 0.0f, precision);
+
+	ut_compare_float("acc not equal 2", acc(0), 2.0f, precision);
+	ut_compare_float("acc not equal 0", acc(1), 0.0f, precision);
+	ut_compare_float("acc not equal 0", acc(2), 0.0f, precision);
+
+	return true;
+
+}
+
+bool BezierQuadTest::_get_arc_length()
+{
+	// create random numbers
+	srand(0); // choose a constant to make it deterministic
+
+	float min = -50.f;
+	float max = 50.f;
+	float resolution = 0.1f;
+
+	matrix::Vector3f pt0, pt1, ctrl;
+	float duration, arc_length, triangle_length, straigth_length;
+	float T = 100.0f;
+
+	// loop trough different control points 100x and check if arc_length is in the expected range
+	for (int i = 0; i < 100 ; i++) {
+		// random bezier point
+		pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max));
+		pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max));
+		ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max));
+
+		// use for each test a new duration
+		duration = random(0.0f, T);
+
+		// create bezier
+		bezier::BezierQuadf bz(pt0, ctrl, pt1, duration);
+
+		// compute arc length, triangle length and straigh length
+		arc_length = bz.getArcLength(resolution);
+		triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length();
+		straigth_length = (pt1 - pt0).length();
+
+		// we also compute length from going point to point and add segment
+		float time_increment = duration / T;
+		float t = 0.0f + time_increment;
+		matrix::Vector3f p0 = pt0;
+		float sum_segments = 0.0f;
+
+		for (int s = 0; s < (int)T; s++) {
+			matrix::Vector3f nextpt = bz.getPoint(t);
+			sum_segments = (nextpt - p0).length() + sum_segments;
+			p0 = bz.getPoint(t);
+			t = t + time_increment;
+		}
+
+		// test comparisons
+		ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length)
+			       && (fabsf(arc_length - sum_segments) < 1.f));
+	}
+
+
+	return true;
+}
+
+bool BezierQuadTest::_set_bez_from_vel()
+{
+	// create random numbers
+	srand(100); // choose a constant to make it deterministic
+
+	float low = -50.0f;
+	float max = 50.0f;
+	float precision = 0.001f;
+
+	for (int i = 0; i < 20; i++) {
+
+		// set velocity
+		matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max));
+		matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max));
+		matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max));
+		float duration = random(0.0f, 100.0f);
+
+		bezier::BezierQuadf bz;;
+		bz.setBezFromVel(ctrl, vel0, vel1, duration);
+
+		// get velocity back
+		matrix::Vector3f v0 = bz.getVelocity(0.0f);
+		matrix::Vector3f v1 = bz.getVelocity(duration);
+		ut_compare_float("", vel0(0), v0(0), precision);
+		ut_compare_float("", vel1(0), v1(0), precision);
+
+		ut_compare_float("", vel0(1), v0(1), precision);
+		ut_compare_float("", vel1(1), v1(1), precision);
+
+		ut_compare_float("", vel0(2), v0(2), precision);
+		ut_compare_float("", vel1(2), v1(2), precision);
+	}
+
+	return true;
+}
+
+float BezierQuadTest::random(float min, float max)
+{
+	float s = rand() / (float)RAND_MAX;
+	return (min + s * (max - min));
+
+}
+
+ut_declare_test_c(test_bezierQuad, BezierQuadTest)

src/systemcmds/tests/test_search_min.cpp

+#include <unit_test.h>
+#include <float.h>
+
+#include "../../lib/mathlib/math/SearchMin.hpp"
+
+// linear function
+float _linear_function(float x)
+{
+	float slope = 2.0f;
+	return slope * x - 1.4f;
+
+}
+
+//linear function without slope
+float _linear_function_flat(float x)
+{
+	return 1.4f;
+}
+
+// quadratic function with min at 2
+float _quadratic_function(float x)
+{
+	return ((x - 2.0f) * (x - 2.0f) + 3.0f);
+}
+
+class SearchMinTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+private:
+	bool _init_inputs();
+	bool _init_inputs_flipped();
+	bool _init_inputs_negative();
+	bool _init_tol_larger_than_range();
+	bool _init_tol_larger_than_range_flipped();
+	bool _no_extremum();
+
+};
+
+
+bool SearchMinTest::run_tests()
+{
+	ut_run_test(_init_inputs);
+	ut_run_test(_init_inputs_flipped);
+	ut_run_test(_init_inputs_negative);
+	ut_run_test(_init_tol_larger_than_range);
+	ut_run_test(_init_tol_larger_than_range_flipped);
+	ut_run_test(_no_extremum);
+
+	return (_tests_failed == 0);
+}
+
+bool SearchMinTest::_init_inputs()
+{
+	float a = 1.0f;
+	float b = 4.0f;
+	float tol = 0.001f;
+	float (*fun)(float);
+	float (*fun2)(float);
+
+	fun = &_linear_function;
+	fun2 = &_quadratic_function;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	float opt2 = math::goldensection(a, b, fun2, tol);
+	PX4_INFO("opt2: %.5f", (double)opt2);
+	ut_assert("linear function opt not equal min ", fabsf(opt - a) <= (tol * 2.0f));
+	ut_assert("quad function opt not equal min ", fabsf(opt2 - 2.0f) <= (tol * 2.0f));
+
+	return true;
+}
+
+bool SearchMinTest::_init_inputs_flipped()
+{
+	float a = 4.0f;
+	float b = 1.0f;
+	float tol = 0.001f;
+	float (*fun)(float);
+	float (*fun2)(float);
+
+	fun = &_linear_function;
+	fun2 = &_quadratic_function;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	float opt2 = math::goldensection(a, b, fun2, tol);
+
+	ut_assert("linear function opt not equal min", fabsf(opt - b) <= (tol * 2.0f));
+	ut_assert("quad function opt not equal min ", fabsf(opt2 - 2.0f) <= (tol * 2.0f));
+
+	return true;
+}
+
+bool SearchMinTest::_init_inputs_negative()
+{
+	float a = -4.0f;
+	float b = -2.0f;
+	float tol = 0.001f;
+	float (*fun)(float);
+	float (*fun2)(float);
+
+	fun = &_linear_function;
+	fun2 = &_quadratic_function;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	float opt2 = math::goldensection(a, b, fun2, tol);
+
+	ut_assert("linear function opt not equal min", fabsf(opt - a) <= (tol * 2.0f));
+	ut_assert("quad function opt not equal min ", fabsf(opt2 - b) <= (tol * 2.0f));
+
+
+	return true;
+}
+
+bool SearchMinTest::_init_tol_larger_than_range()
+{
+	float a = 1.0f;
+	float b = 4.0f;
+	float tol = 6.0f;
+	float (*fun)(float);
+	float (*fun2)(float);
+
+	fun = &_linear_function;
+	fun2 = &_quadratic_function;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	float opt2 = math::goldensection(a, b, fun2, tol);
+
+	ut_assert("linear function opt not equal min", fabsf(opt - (b + a) / 2.0f) <= (0.001f * 2.0f));
+	ut_assert("quad function opt not equal min ", fabsf(opt2 - (b + a) / 2.0f) <= (0.001f * 2.0f));
+
+
+	return true;
+}
+
+bool SearchMinTest::_init_tol_larger_than_range_flipped()
+{
+	float a = 4.0f;
+	float b = 1.0f;
+	float tol = 6.0f;
+	float (*fun)(float);
+	float (*fun2)(float);
+
+	fun = &_linear_function;
+	fun2 = &_quadratic_function;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	float opt2 = math::goldensection(a, b, fun2, tol);
+
+	ut_assert("linear function opt not equal min", fabsf(opt - (b + a) / 2.0f) <= (0.001f * 2.0f));
+	ut_assert("quad function opt not equal min ", fabsf(opt2 - (b + a) / 2.0f) <= (0.001f * 2.0f));
+
+
+	return true;
+}
+
+bool SearchMinTest::_no_extremum()
+{
+	float a = 1.f;
+	float b = 4.0f;
+	float tol = 0.001f;
+	float (*fun)(float);
+	fun = &_linear_function_flat;
+
+	float opt = math::goldensection(a, b, fun, tol);
+	ut_assert("linear function function opt not equal min", fabsf(fun(opt) - fun(b)) <= (tol));
+
+	return true;
+}
+
+ut_declare_test_c(test_search_min, SearchMinTest)

src/systemcmds/tests/tests_main.c

@@ -102,6 +102,7 @@ const struct {
 
 	{"mixer",		test_mixer,	OPT_NOJIGTEST},
 	{"autodeclination",	test_autodeclination,	0},
+	{"bezier",		test_bezierQuad, 0},
 	{"bson",		test_bson,	0},
 	{"conv",		test_conv, 0},
 	{"dataman",		test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST},
@@ -125,6 +126,7 @@ const struct {
 	{"ppm",			test_ppm,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"ppm_loopback",	test_ppm_loopback,	OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"search_min",	test_search_min, 0},
 	{"servo",		test_servo,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"sleep",		test_sleep,	OPT_NOJIGTEST},
 	{"tone",		test_tone,	0},

src/systemcmds/tests/tests_main.h

@@ -55,6 +55,7 @@ __BEGIN_DECLS
 extern int	test_adc(int argc, char *argv[]);
 extern int	test_autodeclination(int argc, char *argv[]);
 extern int	test_hysteresis(int argc, char *argv[]);
+extern int	test_bezierQuad(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);
 extern int	test_dataman(int argc, char *argv[]);
@@ -80,6 +81,7 @@ extern int	test_perf(int argc, char *argv[]);
 extern int	test_ppm(int argc, char *argv[]);
 extern int	test_ppm_loopback(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
+extern int	test_search_min(int argc, char *argv[]);
 extern int	test_sensors(int argc, char *argv[]);
 extern int	test_servo(int argc, char *argv[]);
 extern int	test_sleep(int argc, char *argv[]);