Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testRotation2.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test theta.u and quaternion multiplication.
33  *
34 *****************************************************************************/
35 
41 #include <visp3/core/vpConfig.h>
42 
43 #ifdef VISP_HAVE_CATCH2
44 
45 #include <visp3/core/vpThetaUVector.h>
46 #include <visp3/core/vpUniRand.h>
47 
48 #define CATCH_CONFIG_RUNNER
49 #include <catch.hpp>
50 
51 namespace
52 {
53 vpThetaUVector generateThetaU(vpUniRand &rng)
54 {
55  return vpThetaUVector(
56  vpMath::rad(rng.uniform(-180.0, 180.0)) *
57  vpColVector({rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0), rng.uniform(-1.0, 1.0)}).normalize());
58 }
59 
60 vpQuaternionVector generateQuat(vpUniRand &rng)
61 {
62  const double angle = vpMath::rad(rng.uniform(-180.0, 180.0));
63  const double ctheta = std::cos(angle);
64  const double stheta = std::sin(angle);
65  const double ax = rng.uniform(-1.0, 1.0);
66  const double ay = rng.uniform(-1.0, 1.0);
67  const double az = rng.uniform(-1.0, 1.0);
68  return vpQuaternionVector(stheta * ax, stheta * ay, stheta * az, ctheta);
69 }
70 } // namespace
71 
72 TEST_CASE("Theta u multiplication", "[theta.u]")
73 {
74  const int nTrials = 100;
75  const uint64_t seed = 0x123456789;
76  vpUniRand rng(seed);
77  for (int iter = 0; iter < nTrials; iter++) {
78  const vpThetaUVector tu0 = generateThetaU(rng);
79  const vpThetaUVector tu1 = generateThetaU(rng);
80 
81  const vpRotationMatrix c1Rc2(tu0);
82  const vpRotationMatrix c2Rc3(tu1);
83  const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3;
84  const vpThetaUVector c1_tu_c3 = tu0 * tu1;
85  // two rotation vectors can represent the same rotation,
86  // that is why we compare the rotation matrices
87  const vpRotationMatrix c1Rc3(c1_tu_c3);
88 
89  const double tolerance = 1e-9;
90  for (unsigned int i = 0; i < 3; i++) {
91  for (unsigned int j = 0; j < 3; j++) {
92  CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance));
93  }
94  }
95  }
96 }
97 
98 TEST_CASE("Quaternion multiplication", "[quaternion]")
99 {
100  const int nTrials = 100;
101  const uint64_t seed = 0x123456789;
102  vpUniRand rng(seed);
103  for (int iter = 0; iter < nTrials; iter++) {
104  const vpQuaternionVector q0 = generateQuat(rng);
105  const vpQuaternionVector q1 = generateQuat(rng);
106 
107  const vpRotationMatrix c1Rc2(q0);
108  const vpRotationMatrix c2Rc3(q1);
109  const vpRotationMatrix c1Rc3_ref = c1Rc2 * c2Rc3;
110 
111  const vpQuaternionVector c1_q_c3 = q0 * q1;
112  // two quaternions of opposite sign can represent the same rotation,
113  // that is why we compare the rotation matrices
114  const vpRotationMatrix c1Rc3(c1_q_c3);
115 
116  const double tolerance = 1e-9;
117  for (unsigned int i = 0; i < 3; i++) {
118  for (unsigned int j = 0; j < 3; j++) {
119  CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance));
120  }
121  }
122  }
123 }
124 
125 int main(int argc, char *argv[])
126 {
127  Catch::Session session; // There must be exactly one instance
128 
129  // Let Catch (using Clara) parse the command line
130  session.applyCommandLine(argc, argv);
131 
132  int numFailed = session.run();
133 
134  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
135  // This clamping has already been applied, so just return it here
136  // You can also do any post run clean-up here
137  return numFailed;
138 }
139 #else
140 #include <iostream>
141 
142 int main() { return EXIT_SUCCESS; }
143 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
static double rad(double deg)
Definition: vpMath.h:127
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Class for generating random numbers with uniform probability density.
Definition: vpUniRand.h:123
int uniform(int a, int b)
Definition: vpUniRand.cpp:160