Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
testRotation.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 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 http://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 transformation from various representations of rotation.
33  *
34  * Authors:
35  * Eric Marchand
36  * Fabien Spindler
37  *
38  *****************************************************************************/
39 
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpQuaternionVector.h>
47 #include <visp3/core/vpRotationMatrix.h>
48 #include <visp3/io/vpParseArgv.h>
49 
50 #include <cassert>
51 #include <limits>
52 #include <stdio.h>
53 #include <stdlib.h>
54 
55 static unsigned int cpt = 0;
56 
57 bool test(const std::string &s, const vpArray2D<double> &v, const std::vector<double> &bench)
58 {
59  std::cout << "** Test " << ++cpt << std::endl;
60  std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
61  if (bench.size() != v.size()) {
62  std::cout << "Test fails: bad size wrt bench" << std::endl;
63  return false;
64  }
65  for (unsigned int i = 0; i < v.size(); i++) {
66  if (std::fabs(v.data[i] - bench[i]) > std::fabs(v.data[i]) * std::numeric_limits<double>::epsilon()) {
67  std::cout << "Test fails: bad content" << std::endl;
68  return false;
69  }
70  }
71 
72  return true;
73 }
74 
75 bool test(const std::string &s, const vpArray2D<double> &v, const vpColVector &bench)
76 {
77  std::cout << "** Test " << ++cpt << std::endl;
78  std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
79  if (bench.size() != v.size()) {
80  std::cout << "Test fails: bad size wrt bench" << std::endl;
81  return false;
82  }
83  for (unsigned int i = 0; i < v.size(); i++) {
84  if (std::fabs(v.data[i] - bench[i]) > std::fabs(v.data[i]) * std::numeric_limits<double>::epsilon()) {
85  std::cout << "Test fails: bad content" << std::endl;
86  return false;
87  }
88  }
89 
90  return true;
91 }
92 
93 bool test(const std::string &s, const vpRotationVector &v, const double &bench)
94 {
95  std::cout << "** Test " << ++cpt << std::endl;
96  std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
97  for (unsigned int i = 0; i < v.size(); i++) {
98  if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
99  std::cout << "Test fails: bad content" << std::endl;
100  return false;
101  }
102  }
103 
104  return true;
105 }
106 
107 int main()
108 {
109  try {
110  {
112  std::vector<double> bench1(3, vpMath::rad(10));
113  vpColVector bench3(3, vpMath::rad(10));
114  if (test("r1", r1, bench1) == false)
115  return EXIT_FAILURE;
116 
117  bench1.clear();
118  bench1 = r1.toStdVector();
119  if (test("r1", r1, bench1) == false)
120  return EXIT_FAILURE;
121 
122  r1.buildFrom(bench3);
123  if (test("r1", r1, bench3) == false)
124  return EXIT_FAILURE;
125 
126  vpThetaUVector r2 = r1;
127  if (test("r2", r2, bench1) == false)
128  return EXIT_FAILURE;
129 
130  if (test("r2", r2, vpMath::rad(10)) == false)
131  return EXIT_FAILURE;
132 
133  vpThetaUVector r3;
134  r3 = vpMath::rad(10);
135  if (test("r3", r3, bench1) == false)
136  return EXIT_FAILURE;
137 
138  std::cout << "** Test " << ++cpt << std::endl;
139  for (unsigned int i = 0; i < r3.size(); i++) {
140  if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
141  std::cout << "Test fails: bad content" << std::endl;
142  return EXIT_FAILURE;
143  }
144  }
145 
146  vpColVector r4 = 0.5 * r1;
147  std::vector<double> bench2(3, vpMath::rad(5));
148  if (test("r4", r4, bench2) == false)
149  return EXIT_FAILURE;
150 
151  vpThetaUVector r5(r3);
152  if (test("r5", r5, bench1) == false)
153  return EXIT_FAILURE;
154  }
155  {
157  std::vector<double> bench1(3, vpMath::rad(10));
158  vpColVector bench3(3, vpMath::rad(10));
159  if (test("r1", r1, bench1) == false)
160  return EXIT_FAILURE;
161 
162  bench1.clear();
163  bench1 = r1.toStdVector();
164  if (test("r1", r1, bench1) == false)
165  return EXIT_FAILURE;
166 
167  r1.buildFrom(bench3);
168  if (test("r1", r1, bench3) == false)
169  return EXIT_FAILURE;
170 
171  vpRxyzVector r2 = r1;
172  if (test("r2", r2, bench1) == false)
173  return EXIT_FAILURE;
174 
175  if (test("r2", r2, vpMath::rad(10)) == false)
176  return EXIT_FAILURE;
177 
178  vpRxyzVector r3;
179  r3 = vpMath::rad(10);
180  if (test("r3", r3, bench1) == false)
181  return EXIT_FAILURE;
182 
183  std::cout << "** Test " << ++cpt << std::endl;
184  for (unsigned int i = 0; i < r3.size(); i++) {
185  if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
186  std::cout << "Test fails: bad content" << std::endl;
187  return EXIT_FAILURE;
188  }
189  }
190 
191  vpColVector r4 = 0.5 * r1;
192  std::vector<double> bench2(3, vpMath::rad(5));
193  if (test("r4", r4, bench2) == false)
194  return EXIT_FAILURE;
195 
196  vpRxyzVector r5(r3);
197  if (test("r5", r5, bench1) == false)
198  return EXIT_FAILURE;
199  }
200  {
202  std::vector<double> bench1(3, vpMath::rad(10));
203  vpColVector bench3(3, vpMath::rad(10));
204  if (test("r1", r1, bench1) == false)
205  return EXIT_FAILURE;
206 
207  bench1.clear();
208  bench1 = r1.toStdVector();
209  if (test("r1", r1, bench1) == false)
210  return EXIT_FAILURE;
211 
212  r1.buildFrom(bench3);
213  if (test("r1", r1, bench3) == false)
214  return EXIT_FAILURE;
215 
216  vpRzyxVector r2 = r1;
217  if (test("r2", r2, bench1) == false)
218  return EXIT_FAILURE;
219 
220  if (test("r2", r2, vpMath::rad(10)) == false)
221  return EXIT_FAILURE;
222 
223  vpRzyxVector r3;
224  r3 = vpMath::rad(10);
225  if (test("r3", r3, bench1) == false)
226  return EXIT_FAILURE;
227 
228  std::cout << "** Test " << ++cpt << std::endl;
229  for (unsigned int i = 0; i < r3.size(); i++) {
230  if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
231  std::cout << "Test fails: bad content" << std::endl;
232  return EXIT_FAILURE;
233  }
234  }
235 
236  vpColVector r4 = 0.5 * r1;
237  std::vector<double> bench2(3, vpMath::rad(5));
238  if (test("r4", r4, bench2) == false)
239  return EXIT_FAILURE;
240 
241  vpRzyxVector r5(r3);
242  if (test("r5", r5, bench1) == false)
243  return EXIT_FAILURE;
244  }
245  {
247  std::vector<double> bench1(3, vpMath::rad(10));
248  vpColVector bench3(3, vpMath::rad(10));
249  if (test("r1", r1, bench1) == false)
250  return EXIT_FAILURE;
251 
252  bench1.clear();
253  bench1 = r1.toStdVector();
254  if (test("r1", r1, bench1) == false)
255  return EXIT_FAILURE;
256 
257  r1.buildFrom(bench3);
258  if (test("r1", r1, bench3) == false)
259  return EXIT_FAILURE;
260 
261  vpRzyzVector r2 = r1;
262  if (test("r2", r2, bench1) == false)
263  return EXIT_FAILURE;
264 
265  if (test("r2", r2, vpMath::rad(10)) == false)
266  return EXIT_FAILURE;
267 
268  vpRzyzVector r3;
269  r3 = vpMath::rad(10);
270  if (test("r3", r3, bench1) == false)
271  return EXIT_FAILURE;
272 
273  std::cout << "** Test " << ++cpt << std::endl;
274  for (unsigned int i = 0; i < r3.size(); i++) {
275  if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
276  std::cout << "Test fails: bad content" << std::endl;
277  return EXIT_FAILURE;
278  }
279  }
280 
281  vpColVector r4 = 0.5 * r1;
282  std::vector<double> bench2(3, vpMath::rad(5));
283  if (test("r4", r4, bench2) == false)
284  return EXIT_FAILURE;
285 
286  vpRzyzVector r5(r3);
287  if (test("r5", r5, bench1) == false)
288  return EXIT_FAILURE;
289  }
290  {
292  std::vector<double> bench1(4, vpMath::rad(10));
293  vpColVector bench3(4, vpMath::rad(10));
294  if (test("r1", r1, bench1) == false)
295  return EXIT_FAILURE;
296 
297  bench1.clear();
298  bench1 = r1.toStdVector();
299  if (test("r1", r1, bench1) == false)
300  return EXIT_FAILURE;
301 
302  r1.buildFrom(bench3);
303  if (test("r1", r1, bench3) == false)
304  return EXIT_FAILURE;
305 
306  vpQuaternionVector r2 = r1;
307  if (test("r2", r2, bench1) == false)
308  return EXIT_FAILURE;
309 
310  if (test("r2", r2, vpMath::rad(10)) == false)
311  return EXIT_FAILURE;
312 
314  r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
315  if (test("r3", r3, bench1) == false)
316  return EXIT_FAILURE;
317 
318  std::cout << "** Test " << ++cpt << std::endl;
319  for (unsigned int i = 0; i < r3.size(); i++) {
320  if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
321  std::cout << "Test fails: bad content" << std::endl;
322  return EXIT_FAILURE;
323  }
324  }
325 
326  vpColVector r4 = 0.5 * r1;
327  std::vector<double> bench2(4, vpMath::rad(5));
328  if (test("r4", r4, bench2) == false)
329  return EXIT_FAILURE;
330 
331  vpQuaternionVector r5(r3);
332  if (test("r5", r5, bench1) == false)
333  return EXIT_FAILURE;
334  }
335  {
337  for (int i = -10; i < 10; i++) {
338  for (int j = -10; j < 10; j++) {
339  vpThetaUVector tu(vpMath::rad(90 + i), vpMath::rad(170 + j), vpMath::rad(45));
340  tu.buildFrom(vpRotationMatrix(tu)); // put some coherence into rotation convention
341 
342  std::cout << "Initialization " << std::endl;
343 
344  double theta;
345  vpColVector u;
346  tu.extract(theta, u);
347 
348  std::cout << "theta=" << vpMath::deg(theta) << std::endl;
349  std::cout << "u=" << u << std::endl;
350 
351  std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
352  R.buildFrom(tu);
353 
354  std::cout << "Matrix R";
355  if (R.isARotationMatrix() == 1)
356  std::cout << " is a rotation matrix " << std::endl;
357  else
358  std::cout << " is not a rotation matrix " << std::endl;
359 
360  std::cout << R << std::endl;
361 
362  std::cout << "From vpRotationMatrix to vpQuaternionVector " << std::endl;
363  vpQuaternionVector q(R);
364  std::cout << q << std::endl;
365 
366  R.buildFrom(q);
367  std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl;
368 
369  std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl;
370  vpRxyzVector RxyzBuildFromR(R);
371  std::cout << RxyzBuildFromR << std::endl;
372 
373  std::cout << "From vpRxyzVector to vpThetaUVector " << std::endl;
374  std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl;
375  std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl;
376 
377  vpThetaUVector tuBuildFromEu;
378  tuBuildFromEu.buildFrom(R);
379 
380  std::cout << std::endl;
381  std::cout << "result : should equivalent to the first one " << std::endl;
382 
383  double theta2;
384  vpColVector u2;
385 
386  tuBuildFromEu.extract(theta2, u2);
387  std::cout << "theta=" << vpMath::deg(theta2) << std::endl;
388  std::cout << "u=" << u2 << std::endl;
389 
390  assert(vpMath::abs(theta2 - theta) < std::numeric_limits<double>::epsilon() * 1e10);
391  assert(vpMath::abs(u[0] - u2[0]) < std::numeric_limits<double>::epsilon() * 1e10);
392  assert(vpMath::abs(u[1] - u2[1]) < std::numeric_limits<double>::epsilon() * 1e10);
393  assert(vpMath::abs(u[2] - u2[2]) < std::numeric_limits<double>::epsilon() * 1e10);
394  }
395  vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45));
396  std::cout << "Initialization vpRzyzVector " << std::endl;
397  std::cout << rzyz << std::endl;
398  std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl;
399  R.buildFrom(rzyz);
400  std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl;
401  vpRzyzVector rzyz_final;
402  rzyz_final.buildFrom(R);
403  std::cout << rzyz_final << std::endl;
404 
405  vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45));
406  std::cout << "Initialization vpRzyxVector " << std::endl;
407  std::cout << rzyx << std::endl;
408  std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl;
409  R.buildFrom(rzyx);
410  std::cout << R << std::endl;
411  std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl;
412  vpRzyxVector rzyx_final;
413  rzyx_final.buildFrom(R);
414  std::cout << rzyx_final << std::endl;
415  }
416  }
417  std::cout << "All tests succeed" << std::endl;
418  return EXIT_SUCCESS;
419  } catch (const vpException &e) {
420  std::cout << "Catch an exception: " << e << std::endl;
421  return EXIT_FAILURE;
422  }
423 }
Implementation of a generic rotation vector.
bool isARotationMatrix(double threshold=1e-6) const
void extract(double &theta, vpColVector &u) const
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
vpRzyzVector buildFrom(const vpRotationMatrix &R)
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static Type abs(const Type &x)
Definition: vpMath.h:160
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a rotation vector as quaternion angle minimal representation.
vpRzyxVector buildFrom(const vpRotationMatrix &R)
static double rad(double deg)
Definition: vpMath.h:110
static double deg(double rad)
Definition: vpMath.h:103
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
void set(double x, double y, double z, double w)
Implementation of a rotation vector as axis-angle minimal representation.