mirror of https://github.com/ArduPilot/ardupilot
AP_Math: Implement tests for the Quaternion class
These tests cover Quaternion multiplication, conversion to rotation matrix, and the optimized rotation operator
This commit is contained in:
hs293go
Peter Barker
parent
8bc02ab00b
commit
93a13bbf0e
|
|
@ -0,0 +1,166 @@
|
|||
#include <AP_gtest.h>
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
// Tests that quaternion multiplication obeys Hamilton's quaternion multiplication convention
|
||||
// i*i == j*j == k*k == i*j*k == -1
|
||||
TEST(QuaternionTest, QuaternionMultiplicationOfBases) {
|
||||
const Quaternion unit(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
const Quaternion i(0.0f, 1.0f, 0.0f, 0.0f);
|
||||
const Quaternion j(0.0f, 0.0f, 1.0f, 0.0f);
|
||||
const Quaternion k(0.0f, 0.0f, 0.0f, 1.0f);
|
||||
|
||||
Quaternion ii, ij, ik, ji, jj, jk, ki, kj, kk, ijk;
|
||||
ii = i * i;
|
||||
ij = i * j;
|
||||
ik = i * k;
|
||||
ji = j * i;
|
||||
jj = j * j;
|
||||
jk = j * k;
|
||||
ki = k * i;
|
||||
kj = k * j;
|
||||
kk = k * k;
|
||||
ijk = i * j * k;
|
||||
|
||||
for (int a = 0; a < 4; ++a) {
|
||||
EXPECT_FLOAT_EQ(ii[a], jj[a]);
|
||||
EXPECT_FLOAT_EQ(jj[a], kk[a]);
|
||||
EXPECT_FLOAT_EQ(kk[a], ijk[a]);
|
||||
EXPECT_FLOAT_EQ(ijk[a], -unit[a]);
|
||||
EXPECT_FLOAT_EQ(ij[a], k[a]);
|
||||
EXPECT_FLOAT_EQ(ii[a], -unit[a]);
|
||||
EXPECT_FLOAT_EQ(ik[a], -j[a]);
|
||||
EXPECT_FLOAT_EQ(ji[a], -k[a]);
|
||||
EXPECT_FLOAT_EQ(jj[a], -unit[a]);
|
||||
EXPECT_FLOAT_EQ(jk[a], i[a]);
|
||||
EXPECT_FLOAT_EQ(ki[a], j[a]);
|
||||
EXPECT_FLOAT_EQ(kj[a], -i[a]);
|
||||
EXPECT_FLOAT_EQ(kk[a], -unit[a]);
|
||||
EXPECT_FLOAT_EQ(ijk[a], -unit[a]);
|
||||
}
|
||||
}
|
||||
|
||||
// Tests that the quaternion to rotation matrix conversion formula is correctly derived from the Hamilton's quaternion
|
||||
// multiplication convention. This specific example is taken from "Why and How to Avoid the Flipped Quaternion
|
||||
// Multiplication" (https://arxiv.org/pdf/1801.07478.pdf)
|
||||
TEST(QuaternionTest, QuaternionToRotationMatrix) {
|
||||
Matrix3f res;
|
||||
Quaternion(0.5f * sqrtf(2.0f), 0.0f, 0.0f, 0.5f * sqrtf(2.0f)).rotation_matrix(res);
|
||||
|
||||
EXPECT_NEAR(res.a.x, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.a.y, -1.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.a.z, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.b.x, 1.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.b.y, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.b.z, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.c.x, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.c.y, 0.0f, 1e-6f);
|
||||
EXPECT_NEAR(res.c.z, 1.0f, 1e-6f);
|
||||
}
|
||||
|
||||
// Tests that quaternion multiplication is homomorphic with rotation matrix
|
||||
// multiplication, or C(q0 * q1) = C(q0) * C(q1)
|
||||
TEST(QuaternionTest, QuaternionMultiplicationIsHomomorphism) {
|
||||
Quaternion l_quat(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f);
|
||||
Quaternion r_quat(0.9576622f, 0.03378266f, 0.12607862f, 0.25660481f);
|
||||
|
||||
Matrix3f res_mat_0;
|
||||
(l_quat * r_quat).rotation_matrix(res_mat_0);
|
||||
|
||||
Matrix3f res_mat_1, l_mat, r_mat;
|
||||
l_quat.rotation_matrix(l_mat);
|
||||
r_quat.rotation_matrix(r_mat);
|
||||
res_mat_1 = l_mat * r_mat;
|
||||
|
||||
EXPECT_NEAR(res_mat_0.a.x, res_mat_1.a.x, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.a.y, res_mat_1.a.y, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.a.z, res_mat_1.a.z, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.b.x, res_mat_1.b.x, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.b.y, res_mat_1.b.y, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.b.z, res_mat_1.b.z, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.c.x, res_mat_1.c.x, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.c.y, res_mat_1.c.y, 1e-6f);
|
||||
EXPECT_NEAR(res_mat_0.c.z, res_mat_1.c.z, 1e-6f);
|
||||
}
|
||||
|
||||
// Tests that applying a rotation by a unit quaternion does nothing
|
||||
TEST(QuaternionTest, QuatenionRotationByUnitQuaternion) {
|
||||
Vector3f v(1.0f, 2.0f, 3.0f);
|
||||
Quaternion q(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
|
||||
Vector3f res = q * v;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
EXPECT_FLOAT_EQ(res[i], v[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Tests that applying a rotation by a quaternion whose axis is parallel to the vector does nothing
|
||||
TEST(QuaternionTest, QuatenionRotationByParallelQuaternion) {
|
||||
Vector3f v(1.0f, 2.0f, 3.0f);
|
||||
Quaternion q(0.730296743340221, 0.182574185835055, 0.365148371670111, 0.547722557505166f);
|
||||
|
||||
Vector3f res = q * v;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
EXPECT_FLOAT_EQ(res[i], v[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Tests that applying a rotation by a unit quaternion does not change the vector's length
|
||||
TEST(QuaternionTest, QuatenionRotationLengthPreserving) {
|
||||
Vector3f v(1.0f, 2.0f, 3.0f);
|
||||
Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f);
|
||||
|
||||
Vector3f res = q * v;
|
||||
|
||||
EXPECT_FLOAT_EQ(res.length(), v.length());
|
||||
}
|
||||
|
||||
// Tests that calling the quaternion rotation operator is equivalent to the formula q * v * q.inverse(), and to
|
||||
// converting to rotation matrix followed by matrix multiplication
|
||||
TEST(QuaternionTest, QuatenionRotationFormulaEquivalence) {
|
||||
Vector3f res_1, res_0, res_2;
|
||||
Vector3f v(1.0f, 2.0f, 3.0f);
|
||||
Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f);
|
||||
|
||||
res_0 = q * v;
|
||||
|
||||
Quaternion qv(0.0f, v.x, v.y, v.z);
|
||||
Quaternion res_qv = q * qv * q.inverse();
|
||||
res_1 = Vector3f(res_qv.q2, res_qv.q3, res_qv.q4);
|
||||
|
||||
Matrix3f q_equiv_mat;
|
||||
q.rotation_matrix(q_equiv_mat);
|
||||
res_2 = q_equiv_mat * v;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
EXPECT_FLOAT_EQ(res_0[i], res_1[i]);
|
||||
EXPECT_FLOAT_EQ(res_0[i], res_2[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Tests that the calling the rotation operator on a inverted quaternion is equivalent to q.inverse() * v * q, and to
|
||||
// converting to rotation matrix, taking transpose, followed by matrix multiplication
|
||||
TEST(QuaternionTest, QuatenionInverseRotationFormulaEquivalence) {
|
||||
Vector3f res_0, res_1, res_2;
|
||||
Vector3f v(1.0f, 2.0f, 3.0f);
|
||||
Quaternion q(0.8365163f, 0.48296291f, 0.22414387f, -0.12940952f);
|
||||
|
||||
res_0 = q.inverse() * v;
|
||||
|
||||
Quaternion qv(0.0f, v.x, v.y, v.z);
|
||||
Quaternion res_qv = q.inverse() * qv * q;
|
||||
res_1 = Vector3f(res_qv.q2, res_qv.q3, res_qv.q4);
|
||||
|
||||
Matrix3f q_equiv_mat;
|
||||
q.rotation_matrix(q_equiv_mat);
|
||||
res_2 = q_equiv_mat.transposed() * v;
|
||||
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
EXPECT_FLOAT_EQ(res_0[i], res_1[i]);
|
||||
EXPECT_FLOAT_EQ(res_0[i], res_2[i]);
|
||||
}
|
||||
}
|
||||
|
||||
AP_GTEST_MAIN()
|
||||
Loading…
Reference in New Issue