From fadccfd396730e9f04ee320485d6893e8e542207 Mon Sep 17 00:00:00 2001 From: Samuel Felton Date: Fri, 15 Mar 2024 17:32:10 +0100 Subject: [PATCH] Merge tests for rotations and quaternions, found failing tests --- modules/core/test/math/testQuaternion.cpp | 252 ++++--- modules/core/test/math/testQuaternion2.cpp | 106 --- modules/core/test/math/testRotation.cpp | 727 +++++++++++---------- modules/core/test/math/testRotation2.cpp | 143 ---- 4 files changed, 547 insertions(+), 681 deletions(-) delete mode 100644 modules/core/test/math/testQuaternion2.cpp delete mode 100644 modules/core/test/math/testRotation2.cpp diff --git a/modules/core/test/math/testQuaternion.cpp b/modules/core/test/math/testQuaternion.cpp index a91d278b7f..71d4382578 100644 --- a/modules/core/test/math/testQuaternion.cpp +++ b/modules/core/test/math/testQuaternion.cpp @@ -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 + +#ifdef VISP_HAVE_CATCH2 -#include -#include -#include #include -int main() +#define CATCH_CONFIG_RUNNER +#include + +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::epsilon(); std::cout << "q3=" << q3 << std::endl; - if (!vpMath::equal(q3.x(), 2.6, std::numeric_limits::epsilon()) || - !vpMath::equal(q3.y(), 0.4, std::numeric_limits::epsilon()) || - !vpMath::equal(q3.z(), -3.0, std::numeric_limits::epsilon()) || - !vpMath::equal(q3.w(), 4.0, std::numeric_limits::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::epsilon() * 1e4) || - !vpMath::equal(q4.y(), q2.y(), std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q4.z(), q2.z(), std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q4.w(), q2.w(), std::numeric_limits::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::epsilon() * 1e4) || - !vpMath::equal(q7.y(), 36.7, std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7.z(), -6.6, std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7.w(), 1.3, std::numeric_limits::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::epsilon() * 1e4) || - !vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits::epsilon() * 1e4) || - !vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits::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::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::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 + +int main() { return EXIT_SUCCESS; } +#endif diff --git a/modules/core/test/math/testQuaternion2.cpp b/modules/core/test/math/testQuaternion2.cpp deleted file mode 100644 index 16a73f6a4d..0000000000 --- a/modules/core/test/math/testQuaternion2.cpp +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Description: - * Test quaternion interpolation. - * -*****************************************************************************/ - -/*! - \example testQuaternion2.cpp - - Test quaternion interpolation. -*/ -#include - -#ifdef VISP_HAVE_CATCH2 - -#include - -#define CATCH_CONFIG_RUNNER -#include - -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)); - } -} - -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 - -int main() { return EXIT_SUCCESS; } -#endif diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp index 8f8d0d397a..46bf26d1b5 100644 --- a/modules/core/test/math/testRotation.cpp +++ b/modules/core/test/math/testRotation.cpp @@ -29,30 +29,49 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * Tests transformation from various representations of rotation. + * Test theta.u and quaternion multiplication. * *****************************************************************************/ /*! - \file testRotation.cpp - \brief Tests transformation within various representations of rotation. + \example testRotation2.cpp + + Test theta.u and quaternion multiplication. */ +#include + +#ifdef VISP_HAVE_CATCH2 + +#include +#include -#include -#include -#include -#include +#define CATCH_CONFIG_RUNNER +#include + +namespace +{ +vpThetaUVector generateThetaU(vpUniRand &rng) +{ + return vpThetaUVector( + vpMath::rad(rng.uniform(-180.0, 180.0)) * + vpColVector({ rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0) }).normalize()); +} -#include -#include -#include -#include +vpQuaternionVector generateQuat(vpUniRand &rng) +{ + const double angle = vpMath::rad(rng.uniform(-180.0, 180.0)); + const double ctheta = std::cos(angle); + const double stheta = std::sin(angle); + const double ax = rng.uniform(-1.0, 1.0); + const double ay = rng.uniform(-1.0, 1.0); + const double az = rng.uniform(-1.0, 1.0); + return vpQuaternionVector(stheta * ax, stheta * ay, stheta * az, ctheta); +} +} // namespace -static unsigned int cpt = 0; bool test(const std::string &s, const vpArray2D &v, const std::vector &bench) { - std::cout << "** Test " << ++cpt << std::endl; std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl; if (bench.size() != v.size()) { std::cout << "Test fails: bad size wrt bench" << std::endl; @@ -70,7 +89,6 @@ bool test(const std::string &s, const vpArray2D &v, const std::vector &v, const vpColVector &bench) { - std::cout << "** Test " << ++cpt << std::endl; std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl; if (bench.size() != v.size()) { std::cout << "Test fails: bad size wrt bench" << std::endl; @@ -88,7 +106,6 @@ bool test(const std::string &s, const vpArray2D &v, const vpColVector &b bool test(const std::string &s, const vpRotationVector &v, const double &bench) { - std::cout << "** Test " << ++cpt << std::endl; std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl; for (unsigned int i = 0; i < v.size(); i++) { if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits::epsilon()) { @@ -112,359 +129,377 @@ bool test_matrix_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix return true; } -int main() +TEST_CASE("Common rotation operations", "[rotation]") { - try { - { - vpThetaUVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - std::vector bench1(3, vpMath::rad(10)); - vpColVector bench3(3, vpMath::rad(10)); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - bench1.clear(); - bench1 = r1.toStdVector(); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - r1.buildFrom(bench3); - if (test("r1", r1, bench3) == false) - return EXIT_FAILURE; - - vpThetaUVector r2 = r1; - if (test("r2", r2, bench1) == false) - return EXIT_FAILURE; - - if (test("r2", r2, vpMath::rad(10)) == false) - return EXIT_FAILURE; - - vpThetaUVector r3; - r3 = vpMath::rad(10); - if (test("r3", r3, bench1) == false) - return EXIT_FAILURE; - - std::cout << "** Test " << ++cpt << std::endl; - for (unsigned int i = 0; i < r3.size(); i++) { - if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { - std::cout << "Test fails: bad content" << std::endl; - return EXIT_FAILURE; - } - } + SECTION("Theta u initialization") + { + vpThetaUVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + std::vector bench1(3, vpMath::rad(10)); + vpColVector bench3(3, vpMath::rad(10)); + CHECK(test("r1", r1, bench1)); + + bench1.clear(); + bench1 = r1.toStdVector(); + CHECK(test("r1", r1, bench1)); - vpColVector r4 = 0.5 * r1; - std::vector bench2(3, vpMath::rad(5)); - if (test("r4", r4, bench2) == false) - return EXIT_FAILURE; + r1.buildFrom(bench3); + CHECK(test("r1", r1, bench3)); - vpThetaUVector r5(r3); - if (test("r5", r5, bench1) == false) - return EXIT_FAILURE; + vpThetaUVector r2 = r1; + CHECK(test("r2", r2, bench1)); + CHECK(r2.data != r1.data); + + CHECK(test("r2", r2, vpMath::rad(10))); + + vpThetaUVector r3; + r3 = vpMath::rad(10); + CHECK(test("r3", r3, bench1)); + + for (unsigned int i = 0; i < r3.size(); i++) { + CHECK(std::fabs(r3[i] - bench1[i]) < std::fabs(r3[i]) * std::numeric_limits::epsilon()); } - { - vpRxyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - std::vector bench1(3, vpMath::rad(10)); - vpColVector bench3(3, vpMath::rad(10)); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - bench1.clear(); - bench1 = r1.toStdVector(); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - r1.buildFrom(bench3); - if (test("r1", r1, bench3) == false) - return EXIT_FAILURE; - - vpRxyzVector r2 = r1; - if (test("r2", r2, bench1) == false) - return EXIT_FAILURE; - - if (test("r2", r2, vpMath::rad(10)) == false) - return EXIT_FAILURE; - - vpRxyzVector r3; - r3 = vpMath::rad(10); - if (test("r3", r3, bench1) == false) - return EXIT_FAILURE; - - std::cout << "** Test " << ++cpt << std::endl; - for (unsigned int i = 0; i < r3.size(); i++) { - if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { - std::cout << "Test fails: bad content" << std::endl; - return EXIT_FAILURE; - } - } - vpColVector r4 = 0.5 * r1; - std::vector bench2(3, vpMath::rad(5)); - if (test("r4", r4, bench2) == false) - return EXIT_FAILURE; + const vpColVector r4 = 0.5 * r1; + std::vector bench2(3, vpMath::rad(5)); + CHECK(test("r4", r4, bench2)); + + const vpThetaUVector r5(r3); + CHECK(test("r5", r5, bench1)); + } + SECTION("Rxyz initialization") + { + vpRxyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + std::vector bench1(3, vpMath::rad(10)); + vpColVector bench3(3, vpMath::rad(10)); + CHECK(test("r1", r1, bench1)); + + bench1.clear(); + bench1 = r1.toStdVector(); + CHECK(test("r1", r1, bench1)); + + r1.buildFrom(bench3); + CHECK(test("r1", r1, bench3)); + + vpRxyzVector r2 = r1; + CHECK(test("r2", r2, bench1)); + + CHECK(test("r2", r2, vpMath::rad(10))); - vpRxyzVector r5(r3); - if (test("r5", r5, bench1) == false) - return EXIT_FAILURE; + vpRxyzVector r3; + r3 = vpMath::rad(10); + CHECK(test("r3", r3, bench1)); + + for (unsigned int i = 0; i < r3.size(); i++) { + CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits::epsilon()); } - { - vpRzyxVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - std::vector bench1(3, vpMath::rad(10)); - vpColVector bench3(3, vpMath::rad(10)); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - bench1.clear(); - bench1 = r1.toStdVector(); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - r1.buildFrom(bench3); - if (test("r1", r1, bench3) == false) - return EXIT_FAILURE; - - vpRzyxVector r2 = r1; - if (test("r2", r2, bench1) == false) - return EXIT_FAILURE; - - if (test("r2", r2, vpMath::rad(10)) == false) - return EXIT_FAILURE; - - vpRzyxVector r3; - r3 = vpMath::rad(10); - if (test("r3", r3, bench1) == false) - return EXIT_FAILURE; - - std::cout << "** Test " << ++cpt << std::endl; - for (unsigned int i = 0; i < r3.size(); i++) { - if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { - std::cout << "Test fails: bad content" << std::endl; - return EXIT_FAILURE; - } - } - vpColVector r4 = 0.5 * r1; - std::vector bench2(3, vpMath::rad(5)); - if (test("r4", r4, bench2) == false) - return EXIT_FAILURE; + vpColVector r4 = 0.5 * r1; + std::vector bench2(3, vpMath::rad(5)); + CHECK(test("r4", r4, bench2)); + + vpRxyzVector r5(r3); + CHECK(test("r5", r5, bench1)); + } + SECTION("rzyx initialization") + { + vpRzyxVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + std::vector bench1(3, vpMath::rad(10)); + vpColVector bench3(3, vpMath::rad(10)); + CHECK(test("r1", r1, bench1)); + + bench1.clear(); + bench1 = r1.toStdVector(); + CHECK(test("r1", r1, bench1)); - vpRzyxVector r5(r3); - if (test("r5", r5, bench1) == false) - return EXIT_FAILURE; + r1.buildFrom(bench3); + CHECK(test("r1", r1, bench3)); + + vpRzyxVector r2 = r1; + CHECK(test("r2", r2, bench1)); + + CHECK(test("r2", r2, vpMath::rad(10))); + + vpRzyxVector r3; + r3 = vpMath::rad(10); + CHECK(test("r3", r3, bench1)); + + for (unsigned int i = 0; i < r3.size(); i++) { + CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits::epsilon()); } - { - vpRzyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - std::vector bench1(3, vpMath::rad(10)); - vpColVector bench3(3, vpMath::rad(10)); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - bench1.clear(); - bench1 = r1.toStdVector(); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - r1.buildFrom(bench3); - if (test("r1", r1, bench3) == false) - return EXIT_FAILURE; - - vpRzyzVector r2 = r1; - if (test("r2", r2, bench1) == false) - return EXIT_FAILURE; - - if (test("r2", r2, vpMath::rad(10)) == false) - return EXIT_FAILURE; - - vpRzyzVector r3; - r3 = vpMath::rad(10); - if (test("r3", r3, bench1) == false) - return EXIT_FAILURE; - - std::cout << "** Test " << ++cpt << std::endl; - for (unsigned int i = 0; i < r3.size(); i++) { - if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { - std::cout << "Test fails: bad content" << std::endl; - return EXIT_FAILURE; - } - } - vpColVector r4 = 0.5 * r1; - std::vector bench2(3, vpMath::rad(5)); - if (test("r4", r4, bench2) == false) - return EXIT_FAILURE; + vpColVector r4 = 0.5 * r1; + std::vector bench2(3, vpMath::rad(5)); + CHECK(test("r4", r4, bench2)); - vpRzyzVector r5(r3); - if (test("r5", r5, bench1) == false) - return EXIT_FAILURE; + vpRzyxVector r5(r3); + CHECK(test("r5", r5, bench1)); + } + SECTION("rzyz initialiation") + { + vpRzyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + std::vector bench1(3, vpMath::rad(10)); + vpColVector bench3(3, vpMath::rad(10)); + CHECK(test("r1", r1, bench1)); + + bench1.clear(); + bench1 = r1.toStdVector(); + CHECK(test("r1", r1, bench1)); + + r1.buildFrom(bench3); + CHECK(test("r1", r1, bench3)); + + vpRzyzVector r2 = r1; + CHECK(test("r2", r2, bench1)); + + CHECK(test("r2", r2, vpMath::rad(10))); + + vpRzyzVector r3; + r3 = vpMath::rad(10); + CHECK(test("r3", r3, bench1)); + + for (unsigned int i = 0; i < r3.size(); i++) { + CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits::epsilon()); } - { - vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - std::vector bench1(4, vpMath::rad(10)); - vpColVector bench3(4, vpMath::rad(10)); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - bench1.clear(); - bench1 = r1.toStdVector(); - if (test("r1", r1, bench1) == false) - return EXIT_FAILURE; - - r1.buildFrom(bench3); - if (test("r1", r1, bench3) == false) - return EXIT_FAILURE; - - vpQuaternionVector r2 = r1; - if (test("r2", r2, bench1) == false) - return EXIT_FAILURE; - - if (test("r2", r2, vpMath::rad(10)) == false) - return EXIT_FAILURE; - - vpQuaternionVector r3; - r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); - if (test("r3", r3, bench1) == false) - return EXIT_FAILURE; - - std::cout << "** Test " << ++cpt << std::endl; - for (unsigned int i = 0; i < r3.size(); i++) { - if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits::epsilon()) { - std::cout << "Test fails: bad content" << std::endl; - return EXIT_FAILURE; - } - } - vpColVector r4 = 0.5 * r1; - std::vector bench2(4, vpMath::rad(5)); - if (test("r4", r4, bench2) == false) - return EXIT_FAILURE; + vpColVector r4 = 0.5 * r1; + std::vector bench2(3, vpMath::rad(5)); + CHECK(test("r4", r4, bench2)); + + vpRzyzVector r5(r3); + CHECK(test("r5", r5, bench1)); + } + SECTION("Test quaternion initialization", "[quaternion]") + { + vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + std::vector bench1(4, vpMath::rad(10)); + vpColVector bench3(4, vpMath::rad(10)); + CHECK(test("r1", r1, bench1)); - vpQuaternionVector r5(r3); - if (test("r5", r5, bench1) == false) - return EXIT_FAILURE; + bench1.clear(); + bench1 = r1.toStdVector(); + CHECK(test("r1", r1, bench1)); + + r1.buildFrom(bench3); + CHECK(test("r1", r1, bench3)); + + vpQuaternionVector r2 = r1; + CHECK(test("r2", r2, bench1)); + + CHECK(test("r2", r2, vpMath::rad(10))); + + vpQuaternionVector r3; + r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10)); + CHECK(test("r3", r3, bench1)); + + for (unsigned int i = 0; i < r3.size(); i++) { + CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits::epsilon()); } - { - vpRotationMatrix R; - for (int i = -10; i < 10; i++) { - for (int j = -10; j < 10; j++) { - vpThetaUVector tu(vpMath::rad(90 + i), vpMath::rad(170 + j), vpMath::rad(45)); - tu.buildFrom(vpRotationMatrix(tu)); // put some coherence into rotation convention - - std::cout << "Initialization " << std::endl; - - double theta; - vpColVector u; - tu.extract(theta, u); - - std::cout << "theta=" << vpMath::deg(theta) << std::endl; - std::cout << "u=" << u << std::endl; - - std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl; - R.buildFrom(tu); - - std::cout << "Matrix R"; - if (R.isARotationMatrix() == 1) - std::cout << " is a rotation matrix " << std::endl; - else - std::cout << " is not a rotation matrix " << std::endl; - - std::cout << R << std::endl; - - std::cout << "From vpRotationMatrix to vpQuaternionVector " << std::endl; - vpQuaternionVector q(R); - std::cout << q << std::endl; - - R.buildFrom(q); - std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl; - - std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl; - vpRxyzVector RxyzBuildFromR(R); - std::cout << RxyzBuildFromR << std::endl; - - std::cout << "From vpRxyzVector to vpThetaUVector " << std::endl; - std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl; - std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl; - - vpThetaUVector tuBuildFromEu; - tuBuildFromEu.buildFrom(R); - - std::cout << std::endl; - std::cout << "result : should equivalent to the first one " << std::endl; - - double theta2; - vpColVector u2; - - tuBuildFromEu.extract(theta2, u2); - std::cout << "theta=" << vpMath::deg(theta2) << std::endl; - std::cout << "u=" << u2 << std::endl; - - assert(vpMath::abs(theta2 - theta) < std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[0] - u2[0]) < std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[1] - u2[1]) < std::numeric_limits::epsilon() * 1e10); - assert(vpMath::abs(u[2] - u2[2]) < std::numeric_limits::epsilon() * 1e10); - } - vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)); - std::cout << "Initialization vpRzyzVector " << std::endl; - std::cout << rzyz << std::endl; - std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl; - R.buildFrom(rzyz); - std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl; - vpRzyzVector rzyz_final; - rzyz_final.buildFrom(R); - std::cout << rzyz_final << std::endl; - - vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)); - std::cout << "Initialization vpRzyxVector " << std::endl; - std::cout << rzyx << std::endl; - std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl; - R.buildFrom(rzyx); + + vpColVector r4 = 0.5 * r1; + std::vector bench2(4, vpMath::rad(5)); + CHECK(test("r4", r4, bench2)); + + vpQuaternionVector r5(r3); + CHECK(test("r5", r5, bench1)); + } + SECTION("Conversions") + { + vpRotationMatrix R; + for (int i = -10; i < 10; i++) { + for (int j = -10; j < 10; j++) { + vpThetaUVector tu(vpMath::rad(90 + i), vpMath::rad(170 + j), vpMath::rad(45)); + tu.buildFrom(vpRotationMatrix(tu)); // put some coherence into rotation convention + + std::cout << "Initialization " << std::endl; + + double theta; + vpColVector u; + tu.extract(theta, u); + + std::cout << "theta=" << vpMath::deg(theta) << std::endl; + std::cout << "u=" << u << std::endl; + + std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl; + R.buildFrom(tu); + + std::cout << "Matrix R"; + CHECK(R.isARotationMatrix()); + std::cout << R << std::endl; - std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl; - vpRzyxVector rzyx_final; - rzyx_final.buildFrom(R); - std::cout << rzyx_final << std::endl; + + std::cout << "From vpRotationMatrix to vpQuaternionVector " << std::endl; + vpQuaternionVector q(R); + CHECK(q.magnitude() == Approx(1.0).margin(1e-4)); + std::cout << q << std::endl; + + R.buildFrom(q); + CHECK(R.isARotationMatrix()); + std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl; + + std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl; + vpRxyzVector RxyzBuildFromR(R); + std::cout << RxyzBuildFromR << std::endl; + + std::cout << "From vpRxyzVector to vpThetaUVector " << std::endl; + std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl; + std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl; + + vpThetaUVector tuBuildFromEu; + tuBuildFromEu.buildFrom(R); + + std::cout << std::endl; + std::cout << "result : should equivalent to the first one " << std::endl; + + double theta2; + vpColVector u2; + + tuBuildFromEu.extract(theta2, u2); + std::cout << "theta=" << vpMath::deg(theta2) << std::endl; + std::cout << "u=" << u2 << std::endl; + + CHECK(vpMath::abs(theta2 - theta) < std::numeric_limits::epsilon() * 1e10); + CHECK(vpMath::abs(u[0] - u2[0]) < std::numeric_limits::epsilon() * 1e10); + CHECK(vpMath::abs(u[1] - u2[1]) < std::numeric_limits::epsilon() * 1e10); + CHECK(vpMath::abs(u[2] - u2[2]) < std::numeric_limits::epsilon() * 1e10); } } + SECTION("Conversion from and to rzyz vector") + { + vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)); + std::cout << "Initialization vpRzyzVector " << std::endl; + std::cout << rzyz << std::endl; + std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl; + R.buildFrom(rzyz); + CHECK(R.isARotationMatrix()); + std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl; + vpRzyzVector rzyz_final; + rzyz_final.buildFrom(R); + CHECK(test("rzyz", rzyz_final, rzyz)); + std::cout << rzyz_final << std::endl; + } + SECTION("Conversion from and to rzyx vector") { - // Test rotation_matrix * homogeneous_matrix - vpHomogeneousMatrix _1_M_2_truth; - _1_M_2_truth[0][0] = 0.9835; - _1_M_2_truth[0][1] = -0.0581; - _1_M_2_truth[0][2] = 0.1716; - _1_M_2_truth[0][3] = 0; - _1_M_2_truth[1][0] = -0.0489; - _1_M_2_truth[1][1] = -0.9972; - _1_M_2_truth[1][2] = -0.0571; - _1_M_2_truth[1][3] = 0; - _1_M_2_truth[2][0] = 0.1744; - _1_M_2_truth[2][1] = 0.0478; - _1_M_2_truth[2][2] = -0.9835; - _1_M_2_truth[2][3] = 0; - vpHomogeneousMatrix _2_M_3_; - _2_M_3_[0][0] = 0.9835; - _2_M_3_[0][1] = -0.0581; - _2_M_3_[0][2] = 0.1716; - _2_M_3_[0][3] = 0.0072; - _2_M_3_[1][0] = -0.0489; - _2_M_3_[1][1] = -0.9972; - _2_M_3_[1][2] = -0.0571; - _2_M_3_[1][3] = 0.0352; - _2_M_3_[2][0] = 0.1744; - _2_M_3_[2][1] = 0.0478; - _2_M_3_[2][2] = -0.9835; - _2_M_3_[2][3] = 0.9470; - - vpRotationMatrix _1_R_2_ = _1_M_2_truth.getRotationMatrix(); - vpHomogeneousMatrix _1_M_3_(_1_R_2_* _2_M_3_); - vpHomogeneousMatrix _1_M_3_truth(_1_M_2_truth * _2_M_3_); - bool success = test_matrix_equal(_1_M_3_, _1_M_3_truth); - std::cout << "Test vpHomogeneousMatrix vpRotationMatrix::operator*(vpHomogeneousMatrix) " << (success ? "succeed" : "failed") << std::endl; - if (!success) { - return EXIT_FAILURE; + vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)); + std::cout << "Initialization vpRzyxVector " << std::endl; + std::cout << rzyx << std::endl; + std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl; + R.buildFrom(rzyx); + CHECK(R.isARotationMatrix()); + std::cout << R << std::endl; + std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl; + vpRzyxVector rzyx_final; + rzyx_final.buildFrom(R); + CHECK(test("rzyx", rzyx_final, rzyx)); + std::cout << rzyx_final << std::endl; + } + } + SECTION("Rotation matrix extraction from homogeneous matrix and multiplication") + { + // Test rotation_matrix * homogeneous_matrix + vpHomogeneousMatrix _1_M_2_truth; + _1_M_2_truth[0][0] = 0.9835; + _1_M_2_truth[0][1] = -0.0581; + _1_M_2_truth[0][2] = 0.1716; + _1_M_2_truth[0][3] = 0; + _1_M_2_truth[1][0] = -0.0489; + _1_M_2_truth[1][1] = -0.9972; + _1_M_2_truth[1][2] = -0.0571; + _1_M_2_truth[1][3] = 0; + _1_M_2_truth[2][0] = 0.1744; + _1_M_2_truth[2][1] = 0.0478; + _1_M_2_truth[2][2] = -0.9835; + _1_M_2_truth[2][3] = 0; + vpHomogeneousMatrix _2_M_3_; + _2_M_3_[0][0] = 0.9835; + _2_M_3_[0][1] = -0.0581; + _2_M_3_[0][2] = 0.1716; + _2_M_3_[0][3] = 0.0072; + _2_M_3_[1][0] = -0.0489; + _2_M_3_[1][1] = -0.9972; + _2_M_3_[1][2] = -0.0571; + _2_M_3_[1][3] = 0.0352; + _2_M_3_[2][0] = 0.1744; + _2_M_3_[2][1] = 0.0478; + _2_M_3_[2][2] = -0.9835; + _2_M_3_[2][3] = 0.9470; + + vpRotationMatrix _1_R_2_ = _1_M_2_truth.getRotationMatrix(); + vpHomogeneousMatrix _1_M_3_(_1_R_2_* _2_M_3_); + vpHomogeneousMatrix _1_M_3_truth(_1_M_2_truth * _2_M_3_); + CHECK(test_matrix_equal(_1_M_3_, _1_M_3_truth)); + } + +} + +TEST_CASE("Theta u multiplication", "[theta.u]") +{ + const int nTrials = 100; + const uint64_t seed = 0x123456789; + vpUniRand rng(seed); + for (int iter = 0; iter < nTrials; iter++) { + const vpThetaUVector tu0 = generateThetaU(rng); + const vpThetaUVector tu1 = generateThetaU(rng); + + const vpRotationMatrix c1Rc2(tu0); + const vpRotationMatrix c2Rc3(tu1); + const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3; + const vpThetaUVector c1_tu_c3 = tu0 * tu1; + // two rotation vectors can represent the same rotation, + // that is why we compare the rotation matrices + const vpRotationMatrix c1Rc3(c1_tu_c3); + + const double tolerance = 1e-9; + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance)); } } - std::cout << "All tests succeed" << std::endl; - return EXIT_SUCCESS; } - catch (const vpException &e) { - std::cout << "Catch an exception: " << e << std::endl; - return EXIT_FAILURE; +} + +TEST_CASE("Quaternion multiplication", "[quaternion]") +{ + const int nTrials = 100; + const uint64_t seed = 0x123456789; + vpUniRand rng(seed); + for (int iter = 0; iter < nTrials; iter++) { + const vpQuaternionVector q0 = generateQuat(rng); + const vpQuaternionVector q1 = generateQuat(rng); + + const vpRotationMatrix c1Rc2(q0); + const vpRotationMatrix c2Rc3(q1); + const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3; + + const vpQuaternionVector c1_q_c3 = q0 * q1; + // two quaternions of opposite sign can represent the same rotation, + // that is why we compare the rotation matrices + const vpRotationMatrix c1Rc3(c1_q_c3); + + const double tolerance = 1e-9; + for (unsigned int i = 0; i < 3; i++) { + for (unsigned int j = 0; j < 3; j++) { + CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance)); + } + } } } + +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 + +int main() { return EXIT_SUCCESS; } +#endif diff --git a/modules/core/test/math/testRotation2.cpp b/modules/core/test/math/testRotation2.cpp deleted file mode 100644 index 9292e89a1c..0000000000 --- a/modules/core/test/math/testRotation2.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/**************************************************************************** - * - * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. - * - * This software is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * See the file LICENSE.txt at the root directory of this source - * distribution for additional information about the GNU GPL. - * - * For using ViSP with software that can not be combined with the GNU - * GPL, please contact Inria about acquiring a ViSP Professional - * Edition License. - * - * See https://visp.inria.fr for more information. - * - * This software was developed at: - * Inria Rennes - Bretagne Atlantique - * Campus Universitaire de Beaulieu - * 35042 Rennes Cedex - * France - * - * If you have questions regarding the use of this file, please contact - * Inria at visp@inria.fr - * - * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Description: - * Test theta.u and quaternion multiplication. - * -*****************************************************************************/ - -/*! - \example testRotation2.cpp - - Test theta.u and quaternion multiplication. -*/ -#include - -#ifdef VISP_HAVE_CATCH2 - -#include -#include - -#define CATCH_CONFIG_RUNNER -#include - -namespace -{ -vpThetaUVector generateThetaU(vpUniRand &rng) -{ - return vpThetaUVector( - vpMath::rad(rng.uniform(-180.0, 180.0)) * - vpColVector({rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0)}).normalize()); -} - -vpQuaternionVector generateQuat(vpUniRand &rng) -{ - const double angle = vpMath::rad(rng.uniform(-180.0, 180.0)); - const double ctheta = std::cos(angle); - const double stheta = std::sin(angle); - const double ax = rng.uniform(-1.0, 1.0); - const double ay = rng.uniform(-1.0, 1.0); - const double az = rng.uniform(-1.0, 1.0); - return vpQuaternionVector(stheta * ax, stheta * ay, stheta * az, ctheta); -} -} // namespace - -TEST_CASE("Theta u multiplication", "[theta.u]") -{ - const int nTrials = 100; - const uint64_t seed = 0x123456789; - vpUniRand rng(seed); - for (int iter = 0; iter < nTrials; iter++) { - const vpThetaUVector tu0 = generateThetaU(rng); - const vpThetaUVector tu1 = generateThetaU(rng); - - const vpRotationMatrix c1Rc2(tu0); - const vpRotationMatrix c2Rc3(tu1); - const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3; - const vpThetaUVector c1_tu_c3 = tu0 * tu1; - // two rotation vectors can represent the same rotation, - // that is why we compare the rotation matrices - const vpRotationMatrix c1Rc3(c1_tu_c3); - - const double tolerance = 1e-9; - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance)); - } - } - } -} - -TEST_CASE("Quaternion multiplication", "[quaternion]") -{ - const int nTrials = 100; - const uint64_t seed = 0x123456789; - vpUniRand rng(seed); - for (int iter = 0; iter < nTrials; iter++) { - const vpQuaternionVector q0 = generateQuat(rng); - const vpQuaternionVector q1 = generateQuat(rng); - - const vpRotationMatrix c1Rc2(q0); - const vpRotationMatrix c2Rc3(q1); - const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3; - - const vpQuaternionVector c1_q_c3 = q0 * q1; - // two quaternions of opposite sign can represent the same rotation, - // that is why we compare the rotation matrices - const vpRotationMatrix c1Rc3(c1_q_c3); - - const double tolerance = 1e-9; - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { - CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance)); - } - } - } -} - -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 - -int main() { return EXIT_SUCCESS; } -#endif