Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merge tests for rotations and quaternions, found failing tests #1351

Merged
merged 2 commits into from
Mar 19, 2024
Merged

Merge tests for rotations and quaternions, found failing tests #1351

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
fadccfd
Merge tests for rotations and quaternions, found failing tests
SamFlt Mar 15, 2024
7a38878
Fix vpRzyxVector test and improve rotation matrix to rzyx vector conv…
fspindle Mar 18, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion modules/core/src/math/transformation/vpRzyxVector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,15 @@ vpRzyxVector vpRzyxVector::buildFrom(const vpRotationMatrix &R)
double nx = R[0][0];
double ny = R[1][0];

double phi = atan2(ny, nx);
double COEF_MIN_ROT = 1e-6;
double phi;

if ((fabs(nx) < COEF_MIN_ROT) && (fabs(ny) < COEF_MIN_ROT)) {
phi = 0;
}
else {
phi = atan2(ny, nx);
}
double si = sin(phi);
double co = cos(phi);

Expand Down
252 changes: 166 additions & 86 deletions modules/core/test/math/testQuaternion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,109 +29,189 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Tests quaternion operations.
* Test quaternion interpolation.
*
*****************************************************************************/

/*!
\file testQuaternion.cpp
\brief Tests quaternion operations.
\example testQuaternion2.cpp

Test quaternion interpolation.
*/
#include <visp3/core/vpConfig.h>

#ifdef VISP_HAVE_CATCH2

#include <limits>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpQuaternionVector.h>

int main()
#define CATCH_CONFIG_RUNNER
#include <catch.hpp>

TEST_CASE("Quaternion interpolation", "[quaternion]")
{
const double angle0 = vpMath::rad(-37.14);
const double angle1 = vpMath::rad(57.96);
vpColVector axis({ 1.2, 6.4, -3.7 });
axis.normalize();
const vpThetaUVector tu0(angle0 * axis);
const vpThetaUVector tu1(angle1 * axis);
const vpQuaternionVector q0(tu0);
const vpQuaternionVector q1(tu1);
const double t = 0.5;

const double ref_angle_middle = t * (angle0 + angle1);
const double margin = 1e-3;
const double marginLerp = 1e-1;

// From:
// https://github.com/google/mathfu/blob/a75f852f2d76f6f14d5697e0d09ce509a2e3bfc6/unit_tests/quaternion_test/quaternion_test.cpp#L319-L329
// This will verify that interpolating two quaternions corresponds to interpolating the angle.
SECTION("LERP")
{
vpQuaternionVector qLerp = vpQuaternionVector::lerp(q0, q1, t);
CHECK(vpThetaUVector(qLerp).getTheta() == Approx(ref_angle_middle).margin(marginLerp));
}

SECTION("NLERP")
{
vpQuaternionVector qNlerp = vpQuaternionVector::nlerp(q0, q1, t);
CHECK(vpThetaUVector(qNlerp).getTheta() == Approx(ref_angle_middle).margin(margin));
}

SECTION("SERP")
{
vpQuaternionVector qSlerp = vpQuaternionVector::slerp(q0, q1, t);
CHECK(vpThetaUVector(qSlerp).getTheta() == Approx(ref_angle_middle).margin(margin));
}
}

TEST_CASE("Quaternion operators", "[quaternion]")
{
try {
// Test addition of two quaternions
vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
vpQuaternionVector q3 = q1 + q2;

SECTION("Addition and subtraction")
{
const vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
const vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
const vpQuaternionVector q3 = q1 + q2;
const double margin = std::numeric_limits<double>::epsilon();
std::cout << "q3=" << q3 << std::endl;
if (!vpMath::equal(q3.x(), 2.6, std::numeric_limits<double>::epsilon()) ||
!vpMath::equal(q3.y(), 0.4, std::numeric_limits<double>::epsilon()) ||
!vpMath::equal(q3.z(), -3.0, std::numeric_limits<double>::epsilon()) ||
!vpMath::equal(q3.w(), 4.0, std::numeric_limits<double>::epsilon())) {
throw vpException(vpException::fatalError, "Problem with addition of two quaternions !");
}
CHECK(q3.x() == Approx(2.6).margin(margin));
CHECK(q3.y() == Approx(0.4).margin(margin));
CHECK(q3.z() == Approx(-3.0).margin(margin));
CHECK(q3.w() == Approx(4.0).margin(margin));


// Test subtraction of two quaternions
vpQuaternionVector q4 = q3 - q1;
const vpQuaternionVector q4 = q3 - q1;
std::cout << "q4=" << q4 << std::endl;
if (!vpMath::equal(q4.x(), q2.x(), std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q4.y(), q2.y(), std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q4.z(), q2.z(), std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q4.w(), q2.w(), std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with subtraction of two quaternions !");
}

// Test multiplication of two quaternions
// https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
vpQuaternionVector q5(3.0, 4.0, 3.0, -sin(M_PI));
vpQuaternionVector q6(3.9, -1.0, -3.0, 4.0);
vpQuaternionVector q7 = q5 * q6;
std::cout << "q7=" << q7 << std::endl;
if (!vpMath::equal(q7.x(), 3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7.y(), 36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7.z(), -6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with multiplication of two quaternions !");
}

// Test quaternion conjugate
vpQuaternionVector q7_conj = q7.conjugate();
std::cout << "q7_conj=" << q7_conj << std::endl;
if (!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
!vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with quaternion conjugate !");
}

// Test quaternion inverse
vpQuaternionVector q7_inv = q7.inverse();
std::cout << "q7_inv=" << q7_inv << std::endl;
if (!vpMath::equal(q7_inv.x(), -0.00214111, 0.000001) || !vpMath::equal(q7_inv.y(), -0.026193, 0.000001) ||
!vpMath::equal(q7_inv.z(), 0.00471045, 0.000001) || !vpMath::equal(q7_inv.w(), 0.000927816, 0.000001)) {
throw vpException(vpException::fatalError, "Problem with quaternion inverse !");
}

// Test quaternion norm
double q7_norm = q7.magnitude();
std::cout << "q7_norm=" << q7_norm << std::endl;
if (!vpMath::equal(q7_norm, 37.4318, 0.0001)) {
throw vpException(vpException::fatalError, "Problem with quaternion magnitude !");
}

// Test quaternion normalization
q7.normalize();
std::cout << "q7_unit=" << q7 << std::endl;
if (!vpMath::equal(q7.x(), 0.0801457, 0.00001) || !vpMath::equal(q7.y(), 0.98045, 0.00001) ||
!vpMath::equal(q7.z(), -0.176321, 0.00001) || !vpMath::equal(q7.w(), 0.0347298, 0.00001)) {
throw vpException(vpException::fatalError, "Problem with quaternion normalization !");
}

// Test copy constructor
CHECK(q4.x() == Approx(q2.x()).margin(margin));
CHECK(q4.y() == Approx(q2.y()).margin(margin));
CHECK(q4.z() == Approx(q2.z()).margin(margin));
CHECK(q4.w() == Approx(q2.w()).margin(margin));
}

SECTION("Multiplication")
{
//// https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
const vpQuaternionVector q1(3.0, 4.0, 3.0, -sin(M_PI));
const vpQuaternionVector q2(3.9, -1.0, -3.0, 4.0);
const vpQuaternionVector q3 = q1 * q2;
const double margin = std::numeric_limits<double>::epsilon() * 1e4;
CHECK(q3.x() == Approx(3.0).margin(margin));
CHECK(q3.y() == Approx(36.7).margin(margin));
CHECK(q3.z() == Approx(-6.6).margin(margin));
CHECK(q3.w() == Approx(1.3).margin(margin));
}

SECTION("Conjugate")
{
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
const vpQuaternionVector q1_conj = q1.conjugate();
const double margin = std::numeric_limits<double>::epsilon();
CHECK(q1_conj.x() == Approx(-q1.x()).margin(margin));
CHECK(q1_conj.y() == Approx(-q1.y()).margin(margin));
CHECK(q1_conj.z() == Approx(-q1.z()).margin(margin));
CHECK(q1_conj.w() == Approx(q1.w()).margin(margin));
}

SECTION("Inverse")
{
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
const vpQuaternionVector q1_inv = q1.inverse();
const double margin = 1e-6;
CHECK(q1_inv.x() == Approx(-0.00214111).margin(margin));
CHECK(q1_inv.y() == Approx(-0.026193).margin(margin));
CHECK(q1_inv.z() == Approx(0.00471045).margin(margin));
CHECK(q1_inv.w() == Approx(0.000927816).margin(margin));
}

SECTION("Norm")
{
const vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
const double norm = q1.magnitude();
CHECK(norm == Approx(37.4318).margin(1e-4));
}

SECTION("Normalization")
{
vpQuaternionVector q1(3.0, 36.7, -6.6, 1.3);
q1.normalize();
const double margin = 1e-6;
const double norm = q1.magnitude();
CHECK(norm == Approx(1.0).margin(1e-4));
CHECK(q1.x() == Approx(0.0801457).margin(margin));
CHECK(q1.y() == Approx(0.98045).margin(margin));
CHECK(q1.z() == Approx(-0.176321).margin(margin));
CHECK(q1.w() == Approx(0.0347298).margin(margin));
}

SECTION("Copy constructor")
{
vpQuaternionVector q_copy1 = vpQuaternionVector(0, 0, 1, 1);
std::cout << "q_copy1=" << q_copy1 << std::endl;
vpQuaternionVector q_copy2 = q_copy1;
const vpQuaternionVector q_copy2 = q_copy1;
CHECK_FALSE((q_copy2.x() != q_copy1.x() || q_copy2.y() != q_copy1.y() ||
q_copy2.z() != q_copy1.z() || q_copy2.w() != q_copy1.w()));

// compare data pointers: verify that they're not the same
CHECK(q_copy2.data != q_copy1.data);
q_copy1.set(1, 0, 1, 10);
std::cout << "q_copy1 after set=" << q_copy1 << std::endl;
CHECK_FALSE((q_copy2.x() == q_copy1.x() && q_copy2.y() == q_copy1.y() &&
q_copy2.z() == q_copy1.z() && q_copy2.w() == q_copy1.w()));
std::cout << "q_copy1 after set = " << q_copy1 << std::endl;
std::cout << "q_copy2=" << q_copy2 << std::endl;

// Test assignment operator
vpQuaternionVector q_copy3(10, 10, 10, 10);
q_copy3 = q_copy1;
std::cout << "q_copy3=" << q_copy3 << std::endl;

std::cout << "vpQuaternion operations are ok !" << std::endl;
return EXIT_SUCCESS;
}
catch (const vpException &e) {
std::cerr << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;

SECTION("operator=")
{
const vpQuaternionVector q1 = vpQuaternionVector(0, 0, 1, 1);
vpQuaternionVector q_same(10, 10, 10, 10);
q_same = q1;
CHECK_FALSE((q_same.x() != q1.x() || q_same.y() != q1.y() ||
q_same.z() != q1.z() || q_same.w() != q1.w()));
// compare data pointers: verify that they're not the same
CHECK(q_same.data != q1.data);
}

}


int main(int argc, char *argv[])
{
Catch::Session session; // There must be exactly one instance

// Let Catch (using Clara) parse the command line
session.applyCommandLine(argc, argv);

int numFailed = session.run();

// numFailed is clamped to 255 as some unices only use the lower 8 bits.
// This clamping has already been applied, so just return it here
// You can also do any post run clean-up here
return numFailed;
}
#else
#include <iostream>

int main() { return EXIT_SUCCESS; }
#endif
106 changes: 0 additions & 106 deletions modules/core/test/math/testQuaternion2.cpp
View file
Open in desktop

This file was deleted.

Loading
Loading