Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testQuaternion.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  * Tests quaternion operations.
33  *
34 *****************************************************************************/
35 
41 #include <limits>
42 #include <visp3/core/vpException.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpQuaternionVector.h>
45 
46 int main()
47 {
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;
53  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  }
60 
61  // Test subtraction of two quaternions
62  vpQuaternionVector q4 = q3 - q1;
63  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
118  vpQuaternionVector q_copy1 = vpQuaternionVector(0, 0, 1, 1);
119  std::cout << "q_copy1=" << q_copy1 << std::endl;
120  vpQuaternionVector q_copy2 = q_copy1;
121  q_copy1.set(1, 0, 1, 10);
122  std::cout << "q_copy1 after set=" << q_copy1 << std::endl;
123  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;
132  }
133  catch (const vpException &e) {
134  std::cerr << "Catch an exception: " << e << std::endl;
135  return EXIT_FAILURE;
136  }
137 }
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:449
Implementation of a rotation vector as quaternion angle minimal representation.
const double & z() const
Returns the z-component of the quaternion.
vpQuaternionVector conjugate() const
vpQuaternionVector inverse() const
void set(double x, double y, double z, double w)
const double & x() const
Returns the x-component of the quaternion.
const double & y() const
Returns the y-component of the quaternion.
const double & w() const
Returns the w-component of the quaternion.