Skip to content

Commit fadccfd

Browse files
SamFltSamFlt
committed
Merge tests for rotations and quaternions, found failing tests
1 parent ac958c9 commit fadccfd

File tree

4 files changed

+547
-681
lines changed

4 files changed

+547
-681
lines changed

‎modules/core/test/math/testQuaternion.cpp

+166-86
Original file line numberDiff line numberDiff line change
@@ -29,109 +29,189 @@
2929
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
3030
*
3131
* Description:
32-
* Tests quaternion operations.
32+
* Test quaternion interpolation.
3333
*
3434
*****************************************************************************/
3535

3636
/*!
37-
\file testQuaternion.cpp
38-
\brief Tests quaternion operations.
37+
\example testQuaternion2.cpp
38+
39+
Test quaternion interpolation.
3940
*/
41+
#include <visp3/core/vpConfig.h>
42+
43+
#ifdef VISP_HAVE_CATCH2
4044

41-
#include <limits>
42-
#include <visp3/core/vpException.h>
43-
#include <visp3/core/vpMath.h>
4445
#include <visp3/core/vpQuaternionVector.h>
4546

46-
int main()
47+
#define CATCH_CONFIG_RUNNER
48+
#include <catch.hpp>
49+
50+
TEST_CASE("Quaternion interpolation", "[quaternion]")
51+
{
52+
const double angle0 = vpMath::rad(-37.14);
53+
const double angle1 = vpMath::rad(57.96);
54+
vpColVector axis({ 1.2, 6.4, -3.7 });
55+
axis.normalize();
56+
const vpThetaUVector tu0(angle0 * axis);
57+
const vpThetaUVector tu1(angle1 * axis);
58+
const vpQuaternionVector q0(tu0);
59+
const vpQuaternionVector q1(tu1);
60+
const double t = 0.5;
61+
62+
const double ref_angle_middle = t * (angle0 + angle1);
63+
const double margin = 1e-3;
64+
const double marginLerp = 1e-1;
65+
66+
// From:
67+
// https://github.com/google/mathfu/blob/a75f852f2d76f6f14d5697e0d09ce509a2e3bfc6/unit_tests/quaternion_test/quaternion_test.cpp#L319-L329
68+
// This will verify that interpolating two quaternions corresponds to interpolating the angle.
69+
SECTION("LERP")
70+
{
71+
vpQuaternionVector qLerp = vpQuaternionVector::lerp(q0, q1, t);
72+
CHECK(vpThetaUVector(qLerp).getTheta() == Approx(ref_angle_middle).margin(marginLerp));
73+
}
74+
75+
SECTION("NLERP")
76+
{
77+
vpQuaternionVector qNlerp = vpQuaternionVector::nlerp(q0, q1, t);
78+
CHECK(vpThetaUVector(qNlerp).getTheta() == Approx(ref_angle_middle).margin(margin));
79+
}
80+
81+
SECTION("SERP")
82+
{
83+
vpQuaternionVector qSlerp = vpQuaternionVector::slerp(q0, q1, t);
84+
CHECK(vpThetaUVector(qSlerp).getTheta() == Approx(ref_angle_middle).margin(margin));
85+
}
86+
}
87+
88+
TEST_CASE("Quaternion operators", "[quaternion]")
4789
{
48-
try {
49-
// Test addition of two quaternions
50-
vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
51-
vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
52-
vpQuaternionVector q3 = q1 + q2;
90+
91+
SECTION("Addition and subtraction")
92+
{
93+
const vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
94+
const vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
95+
const vpQuaternionVector q3 = q1 + q2;
96+
const double margin = std::numeric_limits<double>::epsilon();
5397
std::cout << "q3=" << q3 << std::endl;
54-
if (!vpMath::equal(q3.x(), 2.6, std::numeric_limits<double>::epsilon()) ||
55-
!vpMath::equal(q3.y(), 0.4, std::numeric_limits<double>::epsilon()) ||
56-
!vpMath::equal(q3.z(), -3.0, std::numeric_limits<double>::epsilon()) ||
57-
!vpMath::equal(q3.w(), 4.0, std::numeric_limits<double>::epsilon())) {
58-
throw vpException(vpException::fatalError, "Problem with addition of two quaternions !");
59-
}
98+
CHECK(q3.x() == Approx(2.6).margin(margin));
99+
CHECK(q3.y() == Approx(0.4).margin(margin));
100+
CHECK(q3.z() == Approx(-3.0).margin(margin));
101+
CHECK(q3.w() == Approx(4.0).margin(margin));
102+
60103

61104
// Test subtraction of two quaternions
62-
vpQuaternionVector q4 = q3 - q1;
105+
const vpQuaternionVector q4 = q3 - q1;
63106
std::cout << "q4=" << q4 << std::endl;
64-
if (!vpMath::equal(q4.x(), q2.x(), std::numeric_limits<double>::epsilon() * 1e4) ||
65-
!vpMath::equal(q4.y(), q2.y(), std::numeric_limits<double>::epsilon() * 1e4) ||
66-
!vpMath::equal(q4.z(), q2.z(), std::numeric_limits<double>::epsilon() * 1e4) ||
67-
!vpMath::equal(q4.w(), q2.w(), std::numeric_limits<double>::epsilon() * 1e4)) {
68-
throw vpException(vpException::fatalError, "Problem with subtraction of two quaternions !");
69-
}
70-
71-
// Test multiplication of two quaternions
72-
// https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
73-
vpQuaternionVector q5(3.0, 4.0, 3.0, -sin(M_PI));
74-
vpQuaternionVector q6(3.9, -1.0, -3.0, 4.0);
75-
vpQuaternionVector q7 = q5 * q6;
76-
std::cout << "q7=" << q7 << std::endl;
77-
if (!vpMath::equal(q7.x(), 3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
78-
!vpMath::equal(q7.y(), 36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
79-
!vpMath::equal(q7.z(), -6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
80-
!vpMath::equal(q7.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
81-
throw vpException(vpException::fatalError, "Problem with multiplication of two quaternions !");
82-
}
83-
84-
// Test quaternion conjugate
85-
vpQuaternionVector q7_conj = q7.conjugate();
86-
std::cout << "q7_conj=" << q7_conj << std::endl;
87-
if (!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
88-
!vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
89-
!vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
90-
!vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
91-
throw vpException(vpException::fatalError, "Problem with quaternion conjugate !");
92-
}
93-
94-
// Test quaternion inverse
95-
vpQuaternionVector q7_inv = q7.inverse();
96-
std::cout << "q7_inv=" << q7_inv << std::endl;
97-
if (!vpMath::equal(q7_inv.x(), -0.00214111, 0.000001) || !vpMath::equal(q7_inv.y(), -0.026193, 0.000001) ||
98-
!vpMath::equal(q7_inv.z(), 0.00471045, 0.000001) || !vpMath::equal(q7_inv.w(), 0.000927816, 0.000001)) {
99-
throw vpException(vpException::fatalError, "Problem with quaternion inverse !");
100-
}
101-
102-
// Test quaternion norm
103-
double q7_norm = q7.magnitude();
104-
std::cout << "q7_norm=" << q7_norm << std::endl;
105-
if (!vpMath::equal(q7_norm, 37.4318, 0.0001)) {
106-
throw vpException(vpException::fatalError, "Problem with quaternion magnitude !");
107-
}
108-
109-
// Test quaternion normalization
110-
q7.normalize();
111-
std::cout << "q7_unit=" << q7 << std::endl;
112-
if (!vpMath::equal(q7.x(), 0.0801457, 0.00001) || !vpMath::equal(q7.y(), 0.98045, 0.00001) ||
113-
!vpMath::equal(q7.z(), -0.176321, 0.00001) || !vpMath::equal(q7.w(), 0.0347298, 0.00001)) {
114-
throw vpException(vpException::fatalError, "Problem with quaternion normalization !");
115-
}
116-
117-
// Test copy constructor
107+
CHECK(q4.x() == Approx(q2.x()).margin(margin));
108+
CHECK(q4.y() == Approx(q2.y()).margin(margin));
109+
CHECK(q4.z() == Approx(q2.z()).margin(margin));
110+
CHECK(q4.w() == Approx(q2.w()).margin(margin));
111+
}
112+
113+
SECTION("Multiplication")
114+
{
115+
//// https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
116+
const vpQuaternionVector q1(3.0, 4.0, 3.0, -sin(M_PI));
117+
const vpQuaternionVector q2(3.9, -1.0, -3.0, 4.0);
118+
const vpQuaternionVector q3 = q1 * q2;
119+
const double margin = std::numeric_limits<double>::epsilon() * 1e4;
120+
CHECK(q3.x() == Approx(3.0).margin(margin));
121+
CHECK(q3.y() == Approx(36.7).margin(margin));
122+
CHECK(q3.z() == Approx(-6.6).margin(margin));
123+
CHECK(q3.w() == Approx(1.3).margin(margin));
124+
}
125+
126+
SECTION("Conjugate")
127+
{
128+
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
129+
const vpQuaternionVector q1_conj = q1.conjugate();
130+
const double margin = std::numeric_limits<double>::epsilon();
131+
CHECK(q1_conj.x() == Approx(-q1.x()).margin(margin));
132+
CHECK(q1_conj.y() == Approx(-q1.y()).margin(margin));
133+
CHECK(q1_conj.z() == Approx(-q1.z()).margin(margin));
134+
CHECK(q1_conj.w() == Approx(q1.w()).margin(margin));
135+
}
136+
137+
SECTION("Inverse")
138+
{
139+
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
140+
const vpQuaternionVector q1_inv = q1.inverse();
141+
const double margin = 1e-6;
142+
CHECK(q1_inv.x() == Approx(-0.00214111).margin(margin));
143+
CHECK(q1_inv.y() == Approx(-0.026193).margin(margin));
144+
CHECK(q1_inv.z() == Approx(0.00471045).margin(margin));
145+
CHECK(q1_inv.w() == Approx(0.000927816).margin(margin));
146+
}
147+
148+
SECTION("Norm")
149+
{
150+
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
151+
const double norm = q1.magnitude();
152+
CHECK(norm == Approx(37.4318).margin(1e-4));
153+
}
154+
155+
SECTION("Normalization")
156+
{
157+
vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
158+
q1.normalize();
159+
const double margin = 1e-6;
160+
const double norm = q1.magnitude();
161+
CHECK(norm == Approx(1.0).margin(1e-4));
162+
CHECK(q1.x() == Approx(0.0801457).margin(margin));
163+
CHECK(q1.y() == Approx(0.98045).margin(margin));
164+
CHECK(q1.z() == Approx(-0.176321).margin(margin));
165+
CHECK(q1.w() == Approx(0.0347298).margin(margin));
166+
}
167+
168+
SECTION("Copy constructor")
169+
{
118170
vpQuaternionVector q_copy1 = vpQuaternionVector(0, 0, 1, 1);
119171
std::cout << "q_copy1=" << q_copy1 << std::endl;
120-
vpQuaternionVector q_copy2 = q_copy1;
172+
const vpQuaternionVector q_copy2 = q_copy1;
173+
CHECK_FALSE((q_copy2.x() != q_copy1.x() || q_copy2.y() != q_copy1.y() ||
174+
q_copy2.z() != q_copy1.z() || q_copy2.w() != q_copy1.w()));
175+
176+
// compare data pointers: verify that they're not the same
177+
CHECK(q_copy2.data != q_copy1.data);
121178
q_copy1.set(1, 0, 1, 10);
122-
std::cout << "q_copy1 after set=" << q_copy1 << std::endl;
179+
CHECK_FALSE((q_copy2.x() == q_copy1.x() && q_copy2.y() == q_copy1.y() &&
180+
q_copy2.z() == q_copy1.z() && q_copy2.w() == q_copy1.w()));
181+
std::cout << "q_copy1 after set = " << q_copy1 << std::endl;
123182
std::cout << "q_copy2=" << q_copy2 << std::endl;
124-
125-
// Test assignment operator
126-
vpQuaternionVector q_copy3(10, 10, 10, 10);
127-
q_copy3 = q_copy1;
128-
std::cout << "q_copy3=" << q_copy3 << std::endl;
129-
130-
std::cout << "vpQuaternion operations are ok !" << std::endl;
131-
return EXIT_SUCCESS;
132183
}
133-
catch (const vpException &e) {
134-
std::cerr << "Catch an exception: " << e << std::endl;
135-
return EXIT_FAILURE;
184+
185+
SECTION("operator=")
186+
{
187+
const vpQuaternionVector q1 = vpQuaternionVector(0, 0, 1, 1);
188+
vpQuaternionVector q_same(10, 10, 10, 10);
189+
q_same = q1;
190+
CHECK_FALSE((q_same.x() != q1.x() || q_same.y() != q1.y() ||
191+
q_same.z() != q1.z() || q_same.w() != q1.w()));
192+
// compare data pointers: verify that they're not the same
193+
CHECK(q_same.data != q1.data);
136194
}
195+
196+
}
197+
198+
199+
int main(int argc, char *argv[])
200+
{
201+
Catch::Session session; // There must be exactly one instance
202+
203+
// Let Catch (using Clara) parse the command line
204+
session.applyCommandLine(argc, argv);
205+
206+
int numFailed = session.run();
207+
208+
// numFailed is clamped to 255 as some unices only use the lower 8 bits.
209+
// This clamping has already been applied, so just return it here
210+
// You can also do any post run clean-up here
211+
return numFailed;
137212
}
213+
#else
214+
#include <iostream>
215+
216+
int main() { return EXIT_SUCCESS; }
217+
#endif

‎modules/core/test/math/testQuaternion2.cpp

-106
This file was deleted.

0 commit comments

Comments
 (0)