dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Catch quaternion canonical corner cases (#116)

This commit is contained in:
kritz 2019-12-18 14:12:47 +01:00 committed by Julian Kent
parent 4f3565da94
commit e81483a808
4 changed files with 62 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
matrix/Quaternion.hpp
View File

@ -367,11 +367,13 @@ public:
Quaternion canonical() const
{
const Quaternion &q = *this;
if (q(0) < Type(0)) {
return Quaternion(-q(0),-q(1),-q(2),-q(3));
} else {
return Quaternion(q(0),q(1),q(2),q(3));
for (size_t i = 0; i < 4; i++) {
if (fabs(q(i)) > FLT_EPSILON) {
return q * Type(matrix::sign(q(i)));
}
}
return q;
}
/**

6
matrix/helper_functions.hpp
View File

@ -78,4 +78,10 @@ Type wrap_2pi(Type x)
return wrap(x, Type(0), Type(M_TWOPI));
}
template<typename T>
int sign(T val)
{
return (T(FLT_EPSILON) < val) - (val < T(FLT_EPSILON));
}
} // namespace matrix

58
test/attitude.cpp
View File

@ -255,15 +255,55 @@ int main()
TEST(fabs(q_check(3) + q(3)) < eps);
// quaternion canonical
Quatf q_non_canonical(-0.7f,0.4f, 0.3f, -0.3f);
Quatf q_canonical(0.7f,-0.4f, -0.3f, 0.3f);
Quatf q_canonical_ref(0.7f,-0.4f, -0.3f, 0.3f);
TEST(isEqual(q_non_canonical.canonical(),q_canonical_ref));
TEST(isEqual(q_canonical.canonical(),q_canonical_ref));
q_non_canonical.canonicalize();
q_canonical.canonicalize();
TEST(isEqual(q_non_canonical,q_canonical_ref));
TEST(isEqual(q_canonical,q_canonical_ref));
Quatf q_non_canonical_1(-0.7f,0.4f, 0.3f, -0.3f);
Quatf q_canonical_1(0.7f,-0.4f, -0.3f, 0.3f);
Quatf q_canonical_ref_1(0.7f,-0.4f, -0.3f, 0.3f);
TEST(isEqual(q_non_canonical_1.canonical(),q_canonical_ref_1));
TEST(isEqual(q_canonical_1.canonical(),q_canonical_ref_1));
q_non_canonical_1.canonicalize();
q_canonical_1.canonicalize();
TEST(isEqual(q_non_canonical_1,q_canonical_ref_1));
TEST(isEqual(q_canonical_1,q_canonical_ref_1));
Quatf q_non_canonical_2(0.0f, -1.0f, 0.0f, 0.0f);
Quatf q_canonical_2(0.0f, 1.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_2(0.0f, 1.0f, 0.0f, 0.0f);
TEST(isEqual(q_non_canonical_2.canonical(),q_canonical_ref_2));
TEST(isEqual(q_canonical_2.canonical(),q_canonical_ref_2));
q_non_canonical_2.canonicalize();
q_canonical_2.canonicalize();
TEST(isEqual(q_non_canonical_2,q_canonical_ref_2));
TEST(isEqual(q_canonical_2,q_canonical_ref_2));
Quatf q_non_canonical_3(0.0f, 0.0f, -1.0f, 0.0f);
Quatf q_canonical_3(0.0f, 0.0f, 1.0f, 0.0f);
Quatf q_canonical_ref_3(0.0f, 0.0f, 1.0f, 0.0f);
TEST(isEqual(q_non_canonical_3.canonical(),q_canonical_ref_3));
TEST(isEqual(q_canonical_3.canonical(),q_canonical_ref_3));
q_non_canonical_3.canonicalize();
q_canonical_3.canonicalize();
TEST(isEqual(q_non_canonical_3,q_canonical_ref_3));
TEST(isEqual(q_canonical_3,q_canonical_ref_3));
Quatf q_non_canonical_4(0.0f, 0.0f, 0.0f, -1.0f);
Quatf q_canonical_4(0.0f, 0.0f, 0.0f, 1.0f);
Quatf q_canonical_ref_4(0.0f, 0.0f, 0.0f, 1.0f);
TEST(isEqual(q_non_canonical_4.canonical(),q_canonical_ref_4));
TEST(isEqual(q_canonical_4.canonical(),q_canonical_ref_4));
q_non_canonical_4.canonicalize();
q_canonical_4.canonicalize();
TEST(isEqual(q_non_canonical_4,q_canonical_ref_4));
TEST(isEqual(q_canonical_4,q_canonical_ref_4));
Quatf q_non_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
Quatf q_canonical_5(0.0f, 0.0f, 0.0f, 0.0f);
Quatf q_canonical_ref_5(0.0f, 0.0f, 0.0f, 0.0f);
TEST(isEqual(q_non_canonical_5.canonical(),q_canonical_ref_5));
TEST(isEqual(q_canonical_5.canonical(),q_canonical_ref_5));
q_non_canonical_5.canonicalize();
q_canonical_5.canonicalize();
TEST(isEqual(q_non_canonical_5,q_canonical_ref_5));
TEST(isEqual(q_canonical_5,q_canonical_ref_5));
// quaternion setIdentity
Quatf q_nonIdentity(-0.7f, 0.4f, 0.5f, -0.3f);

2
test/copyto.cpp
View File

@ -15,7 +15,7 @@ int main()
float eps = 1e-6f;
// Vector3 copyTo
Vector3f v(1, 2, 3);
const Vector3f v(1, 2, 3);
float dst3[3] = {};
v.copyTo(dst3);
for (size_t i = 0; i < 3; i++) {