fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 22:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

test_eigen: reformulate and enhance eigen math tests on firmware

Browse Source
This commit is contained in:
TSC21
2015-08-16 18:02:31 +01:00
committed by Lorenz Meier
parent 90ecc942ce
commit c5dfff1e6f
1 changed files with 255 additions and 213 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/tests/test_eigen.cpp
+255 -213
View File
@@ -1,41 +1,42 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
*
* 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>
* @author Nuno Marques <n.marques21@hotmail.com>
*/
#include <cmath>
@@ -52,38 +53,53 @@
#include "tests.h"
namespace Eigen
{
using namespace Eigen;
typedef Matrix<float, 10, 1> Vector10f;
}
static constexpr size_t OPERATOR_ITERATIONS = 30000;
#define TEST_OP(_title, _op) \
{ \
const hrt_abstime t0 = hrt_absolute_time(); \
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
_op; \
} \
printf("-O0 " _title ": %.6fus\n", static_cast<double>(hrt_absolute_time() - t0) / OPERATOR_ITERATIONS); \
const float min = -M_PI_F;
const float max = M_PI_F;
const float step = M_PI_F / 12;
const float epsilon_f = 1E-4;
uint8_t err_num;
#define TEST_OP(_title, _op) \
{ \
const hrt_abstime t0 = hrt_absolute_time(); \
for (size_t j = 0; j < OPERATOR_ITERATIONS; j++) { \
_op; \
} \
printf("%llu: %s: finished in: %.6fus\n", (unsigned long long)t0, _title, 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__")\n"); \
#define VERIFY_OP(_title, _op, __OP_TEST__) \
{ \
_op; \
if (!(__OP_TEST__)) { \
printf(_title ": Failed! (" # __OP_TEST__ ")\n"); \
++err_num; \
} \
}
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
VERIFY_OP(_title, _op, __OP_TEST__) \
#define TEST_OP_VERIFY(_title, _op, __OP_TEST__) \
VERIFY_OP(_title, _op, __OP_TEST__) \
TEST_OP(_title, _op)
#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \
(fabsf(q_exp.x() - q_act.x()) <= epsilon && \
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
#define EXPECT_NEAR(expected, actual, epsilon) \
(fabsf(expected - actual) <= epsilon)
/**
* @brief
* Prints an Eigen::Matrix to stdout
**/
* @brief
* Prints an Eigen::Matrix to stdout
**/
template<typename T>
void printEigen(const Eigen::MatrixBase<T> &b)
{
@@ -102,48 +118,66 @@ void printEigen(const Eigen::MatrixBase<T> &b)
}
}
// Methods definition
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy);
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q);
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy);
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q);
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
/**
* @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 yawAngle * pitchAngle * rollAngle;
// }
Eigen::Quaternionf quatFromEuler(const float roll, const float pitch, const float yaw);
Eigen::Quaternionf quatFromEuler(const float roll, const float pitch, const float yaw)
{
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitZ());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitX());
return yawAngle * pitchAngle * rollAngle;
* @brief
* Construct new Eigen::Quaternion from euler angles
* Right order is YPR.
**/
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
return Eigen::Quaternionf(
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
}
/**
* @brief
* Construct new Eigen::Matrix3f from euler angles
**/
template<typename T>
Eigen::Matrix3f matrixFromEuler(const T roll, const T pitch, const T yaw)
{
Eigen::AngleAxis<T> rollAngle(roll, Eigen::Vector3f::UnitZ());
Eigen::AngleAxis<T> yawAngle(yaw, Eigen::Vector3f::UnitY());
Eigen::AngleAxis<T> pitchAngle(pitch, Eigen::Vector3f::UnitX());
Eigen::Quaternionf q = yawAngle * pitchAngle * rollAngle;
return q.toRotationMatrix();
* @brief
* Construct new Eigen::Vector3f of euler angles from quaternion
* Right order is YPR.
**/
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
/**
* @brief
* Construct new Eigen::Matrix3f from euler angles
**/
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
return quatFromEuler(rpy).toRotationMatrix();
}
int test_eigen(int argc, char *argv[])
{
/**
* @brief
* Adjust PX4 math::quaternion to Eigen::Quaternionf
**/
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
}
/**
* @brief
* Adjust Eigen::Quaternionf to PX4 math::quaternion
**/
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
}
/**
* @brief
* Testing main routine
**/
int test_eigen(int argc, char *argv[]) {
int rc = 0;
warnx("testing eigen");
warnx("Testing Eigen math...");
{
Eigen::Vector2f v;
@@ -160,7 +194,6 @@ int test_eigen(int argc, char *argv[])
VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
VERIFY_OP("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?
}
{
@@ -215,12 +248,12 @@ int test_eigen(int argc, char *argv[])
}
{
Eigen::Vector10f v1;
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));
TEST_OP("Constructor Vector<10>()", Vector10f v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Vector10f v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Vector10f v3(data));
}
{
@@ -239,7 +272,7 @@ int test_eigen(int argc, char *argv[])
m1.setIdentity();
Eigen::Matrix<float, 10, 10> m2;
m2.setIdentity();
Eigen::Vector10f v1;
Vector10f v1;
v1.Zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
@@ -247,12 +280,12 @@ int test_eigen(int argc, char *argv[])
}
{
warnx("Nonsymmetric matrix operations test");
printf("%llu: Nonsymmetric matrix operations test...\n", (unsigned long long)hrt_absolute_time());
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
@@ -265,139 +298,142 @@ int test_eigen(int argc, char *argv[])
12, 15, 18;
if (m1 + m2 != m3) {
warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
printf("%llu: Matrix<2, 3> + Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1 + m2);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
if (m3 - m2 != m1) {
warnx("Matrix<2, 3> - Matrix<2, 3> failed!");
printf("%llu: Matrix<2, 3> - Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m3 - m2);
printf("!=\n");
printEigen(m1);
++err_num;
rc = 1;
}
m1 += m2;
if (m1 != m3) {
warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
printf("%llu: Matrix<2, 3> += Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m3);
++err_num;
rc = 1;
}
m1 -= m2;
if (m1 != m1_orig) {
warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
printf("%llu: Matrix<2, 3> -= Matrix<2, 3> failed!\n", (unsigned long long)hrt_absolute_time());
printEigen(m1);
printf("!=\n");
printEigen(m1_orig);
++err_num;
rc = 1;
}
}
warnx("pre-quat");
usleep(500000);
/* QUATERNION TESTS */
{
// test conversion rotation matrix to quaternion and back
// Test conversion rotation matrix to quaternion and back
// against existing PX4 mathlib
math::Matrix<3, 3> R_orig;
Eigen::Matrix3f R;
Eigen::Quaternionf q_true;
Eigen::Quaternionf q;
float diff = 0.1f;
float tol;
Eigen::Matrix3f R;
warnx("Quaternion transformation methods test.");
printf("%llu: Conversion method: Quaternion transformation methods test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Conversion method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
warnx("testing known values..");
/******************************************** TEST 1 ****************************************************/
q_true = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q_px4 = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q_eigen(eigenqFromPx4q(q_px4));
// test against some known values
tol = 0.001f;
Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion 1.0f, 0.0f, 0.0f, 0.0f error: w: %8.4f", q.w());
if (!EXPECT_QUATERNION(q_true, q_eigen, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion1 [1.0, 0.0, 0.0, 0.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_eigen.x(), q_eigen.y(), q_eigen.z(), q_eigen.w());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
Eigen::Quaternionf q2_eigen = {0.0f, 0.0f, 0.0f, 1.0f};
math::Quaternion q2_px4(px4qFromEigenq(q2_eigen));
Eigen::Quaternionf q2_eigen_(q2_px4.data[3], q2_px4.data[0], q2_px4.data[1], q2_px4.data[2]);
usleep(510000);
warnx("post-first");
q_true = quatFromEuler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
usleep(510000);
warnx("post-2");
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion 0.9833f, 0.1436f, 0.1060f, 0.0343f error: w: %8.6f, %8.6f", q.w(), q_true.w());
if (!EXPECT_QUATERNION(q_true, q2_eigen_, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion2 [0.0, 0.0, 0.0, 1.0]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q2_px4.data[0], q2_px4.data[1], q2_px4.data[2], q2_px4.data[3]);
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.x(), q_true.y(), q_true.z(), q_true.w());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
q = {0.9833474432563558f, 0.14357217502739184f, 0.10602051106179561f, 0.0342707985504821f};
usleep(510000);
warnx("post-3");
Eigen::Quaternionf q_true2 = quatFromEuler(-1.5f, -0.2f, 0.5f);
Eigen::Quaternionf q2 = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
usleep(510000);
warnx("post-4");
if (!q2.isApprox(q_true2, tol)) {
warnx("Quaternion 0.9833f, 0.1436f, 0.1060f, 0.0343f error: w: %8.6f, %8.6f", q2.w(), q_true2.w());
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.9833, 0.1436, 0.1060, 0.0343]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
q = {0.7222365948153096f, -0.6390766544101811f, -0.2385737751841646f, 0.11418355701173476f};
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
usleep(510000);
warnx("post-5");
q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
usleep(510000);
warnx("post-6");
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: Value of: Quaternion [0.7222, -0.6391, -0.2386, 0.1142]\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
q = {0.6830127018922193f, 0.18301270189221933f, -0.6830127018922193f, 0.18301270189221933f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
warnx("%8.4f, %8.4f, %8.4f, %8.4f, w: %8.4f",
q.vec()(1), q.vec()(2), q.vec()(3), q.vec()(4), q.w());
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", (unsigned long long)hrt_absolute_time(), i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 6 ****************************************************/
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
usleep(510000);
warnx("post-7");
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
warnx("testing transformation range (this will take a while)");
tol = 0.00001f;
q = Eigen::Quaternionf(quatFromEuler(Eigen::Vector3f(roll, pitch, yaw)));
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) {
Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
R_orig.from_euler(roll, pitch, yaw);
q = yawAngle * pitchAngle * rollAngle;
R = q.toRotationMatrix();
R_orig.from_euler(roll, pitch, yaw);
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)) > tol) {
warnx("Quaternion constructor or 'toRotationMatrix' outside tolerance!\n %d, %d: %8.4f vs. %8.4f", i, j, R_orig(i, j), R(i, j));
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
printf("(%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", (unsigned long long)hrt_absolute_time(), i, j);
printf("Actual: \t%8.5f\n", R(i, j));
printf("Expected: \t%8.5f\n", R_orig(i, j));
++err_num;
rc = 1;
}
}
@@ -408,91 +444,97 @@ int test_eigen(int argc, char *argv[])
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
// Test rotation method (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;
warnx("Quaternion vector rotation method test.");
printf("%llu: Rotation method: Quaternion vector rotation method test...\n", (unsigned long long)hrt_absolute_time());
printf("%llu: Rotation method: Testing known values...\n", (unsigned long long)hrt_absolute_time());
// test some values calculated with matlab
tol = 0.0001f;
q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
/******************************************** TEST 1 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
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");
warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f",
(double)vector_q(1), (double)vector_q(2), (double)vector_q(3),
(double)vector_true(1), (double)vector_true(2), (double)vector_true(3));
if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
printf("%llu: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", vector_q(0), vector_q(1), vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", vector_true(0), vector_true(1), vector_true(2));
++err_num;
rc = 1;
}
}
q = quatFromEuler(0.3f, 0.2f, 0.1f);
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
vector_q = q._transformVector(vector);
vector_true = {1.1566, 0.7792, 1.0273};
vector_true = {0.8795481794122900f, 1.2090975499501229f, 0.874344391414010f};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f",
(double)vector_q(1), (double)vector_q(2), (double)vector_q(3),
(double)vector_true(1), (double)vector_true(2), (double)vector_true(3));
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
q = quatFromEuler(-1.5f, -0.2f, 0.5f);
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
vector_q = q._transformVector(vector);
vector_true = {0.5095, 1.4956, -0.7096};
vector_true = {0.447416342848463f, -0.6805264343934600f, 1.528627615949624f};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f",
(double)vector_q(1), (double)vector_q(2), (double)vector_q(3),
(double)vector_true(1), (double)vector_true(2), (double)vector_true(3));
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
vector_q = q._transformVector(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
vector_true = {-1.366030f, 0.366025f, 1.000000f};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
warnx("%8.4f, %8.4f, %8.4f, true: %8.4f, %8.4f, %8.4f",
(double)vector_q(1), (double)vector_q(2), (double)vector_q(3),
(double)vector_true(1), (double)vector_true(2), (double)vector_true(3));
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
printf("%llu: It[%d]: Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time());
printf("Actual: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_q(0), (double)vector_q(1), (double)vector_q(2));
printf("Expected: \t[%8.5f, %8.5f, %8.5f]\n", (double)vector_true(0), (double)vector_true(1), (double)vector_true(2));
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
warnx("testing transformation range (this will take a while)");
tol = 0.00001f;
Eigen::Vector3f vectorR(1.0f, 1.0f, 1.0f);
Eigen::Vector3f vectorR = {1.0f, 1.0f, 1.0f};
for (float roll = min; roll <= max; roll += step) {
for (float pitch = min; pitch <= max; pitch += step) {
for (float yaw = min; yaw <= max; yaw += step) {
R = matrixFromEuler(Eigen::Vector3f(roll, pitch, yaw));
q = quatFromEuler(Eigen::Vector3f(roll, pitch, yaw));
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 = matrixFromEuler(roll, pitch, yaw);
q = quatFromEuler(roll, pitch, yaw);
vector_r = R * vectorR;
vector_q = q._transformVector(vectorR);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
if (!EXPECT_NEAR(vector_r(i), vector_q(i), epsilon_f)) {
printf("%llu: (%d) Value of: Quaternion method 'rotate'\n", (unsigned long long)hrt_absolute_time(), i);
printf("Actual: \t%8.5f\n", vector_q(i));
printf("Expected: \t%8.5f\n", vector_r(i));
++err_num;
rc = 1;
}
}
@@ -500,6 +542,6 @@ int test_eigen(int argc, char *argv[])
}
}
}
printf("%llu: Finished Eigen math tests with %d error(s)...\n", (unsigned long long)hrt_absolute_time(), err_num);
return rc;
}