Visual Servoing Platform version 3.5.0
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
55static unsigned int cpt = 0;
56
57bool 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
75bool 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
93bool 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
107int 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
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;
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}
unsigned int getCols() const
Definition: vpArray2D.h:279
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
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
error that can be emited by ViSP classes.
Definition: vpException.h:72
static double rad(double deg)
Definition: vpMath.h:110
static Type abs(const Type &x)
Definition: vpMath.h:160
static double deg(double rad)
Definition: vpMath.h:103
Implementation of a rotation vector as quaternion angle minimal representation.
void set(double x, double y, double z, double w)
Implementation of a rotation matrix and operations on such kind of matrices.
bool isARotationMatrix(double threshold=1e-6) const
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:184
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:186
vpRzyxVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:183
vpRzyzVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)