41#include <visp3/core/vpMath.h>
42#include <visp3/core/vpQuaternionVector.h>
43#include <visp3/core/vpRotationMatrix.h>
44#include <visp3/io/vpParseArgv.h>
51static unsigned int cpt = 0;
53bool test(
const std::string &s,
const vpArray2D<double> &v,
const std::vector<double> &bench)
55 std::cout <<
"** Test " << ++cpt << std::endl;
56 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
57 if (bench.size() != v.
size()) {
58 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
61 for (
unsigned int i = 0; i < v.
size(); i++) {
62 if (std::fabs(v.
data[i] - bench[i]) > std::fabs(v.
data[i]) * std::numeric_limits<double>::epsilon()) {
63 std::cout <<
"Test fails: bad content" << std::endl;
73 std::cout <<
"** Test " << ++cpt << std::endl;
74 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
76 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
79 for (
unsigned int i = 0; i < v.
size(); i++) {
80 if (std::fabs(v.
data[i] - bench[i]) > std::fabs(v.
data[i]) * std::numeric_limits<double>::epsilon()) {
81 std::cout <<
"Test fails: bad content" << std::endl;
89bool test(
const std::string &s,
const vpRotationVector &v,
const double &bench)
91 std::cout <<
"** Test " << ++cpt << std::endl;
92 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
93 for (
unsigned int i = 0; i < v.
size(); i++) {
94 if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
95 std::cout <<
"Test fails: bad content" << std::endl;
105 for (
unsigned int i = 0; i < 4; i++) {
106 for (
unsigned int j = 0; j < 4; j++) {
122 if (test(
"r1", r1, bench1) ==
false)
126 bench1 = r1.toStdVector();
127 if (test(
"r1", r1, bench1) ==
false)
130 r1.buildFrom(bench3);
131 if (test(
"r1", r1, bench3) ==
false)
135 if (test(
"r2", r2, bench1) ==
false)
143 if (test(
"r3", r3, bench1) ==
false)
146 std::cout <<
"** Test " << ++cpt << std::endl;
147 for (
unsigned int i = 0; i < r3.
size(); i++) {
148 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
149 std::cout <<
"Test fails: bad content" << std::endl;
156 if (test(
"r4", r4, bench2) ==
false)
160 if (test(
"r5", r5, bench1) ==
false)
167 if (test(
"r1", r1, bench1) ==
false)
171 bench1 = r1.toStdVector();
172 if (test(
"r1", r1, bench1) ==
false)
175 r1.buildFrom(bench3);
176 if (test(
"r1", r1, bench3) ==
false)
180 if (test(
"r2", r2, bench1) ==
false)
188 if (test(
"r3", r3, bench1) ==
false)
191 std::cout <<
"** Test " << ++cpt << std::endl;
192 for (
unsigned int i = 0; i < r3.
size(); i++) {
193 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
194 std::cout <<
"Test fails: bad content" << std::endl;
201 if (test(
"r4", r4, bench2) ==
false)
205 if (test(
"r5", r5, bench1) ==
false)
212 if (test(
"r1", r1, bench1) ==
false)
216 bench1 = r1.toStdVector();
217 if (test(
"r1", r1, bench1) ==
false)
220 r1.buildFrom(bench3);
221 if (test(
"r1", r1, bench3) ==
false)
225 if (test(
"r2", r2, bench1) ==
false)
233 if (test(
"r3", r3, bench1) ==
false)
236 std::cout <<
"** Test " << ++cpt << std::endl;
237 for (
unsigned int i = 0; i < r3.
size(); i++) {
238 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
239 std::cout <<
"Test fails: bad content" << std::endl;
246 if (test(
"r4", r4, bench2) ==
false)
250 if (test(
"r5", r5, bench1) ==
false)
257 if (test(
"r1", r1, bench1) ==
false)
261 bench1 = r1.toStdVector();
262 if (test(
"r1", r1, bench1) ==
false)
265 r1.buildFrom(bench3);
266 if (test(
"r1", r1, bench3) ==
false)
270 if (test(
"r2", r2, bench1) ==
false)
278 if (test(
"r3", r3, bench1) ==
false)
281 std::cout <<
"** Test " << ++cpt << std::endl;
282 for (
unsigned int i = 0; i < r3.
size(); i++) {
283 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
284 std::cout <<
"Test fails: bad content" << std::endl;
291 if (test(
"r4", r4, bench2) ==
false)
295 if (test(
"r5", r5, bench1) ==
false)
302 if (test(
"r1", r1, bench1) ==
false)
306 bench1 = r1.toStdVector();
307 if (test(
"r1", r1, bench1) ==
false)
310 r1.buildFrom(bench3);
311 if (test(
"r1", r1, bench3) ==
false)
315 if (test(
"r2", r2, bench1) ==
false)
323 if (test(
"r3", r3, bench1) ==
false)
326 std::cout <<
"** Test " << ++cpt << std::endl;
327 for (
unsigned int i = 0; i < r3.
size(); i++) {
328 if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
329 std::cout <<
"Test fails: bad content" << std::endl;
336 if (test(
"r4", r4, bench2) ==
false)
340 if (test(
"r5", r5, bench1) ==
false)
345 for (
int i = -10; i < 10; i++) {
346 for (
int j = -10; j < 10; j++) {
350 std::cout <<
"Initialization " << std::endl;
356 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl;
357 std::cout <<
"u=" << u << std::endl;
359 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
362 std::cout <<
"Matrix R";
364 std::cout <<
" is a rotation matrix " << std::endl;
366 std::cout <<
" is not a rotation matrix " << std::endl;
368 std::cout << R << std::endl;
370 std::cout <<
"From vpRotationMatrix to vpQuaternionVector " << std::endl;
372 std::cout << q << std::endl;
375 std::cout <<
"From vpQuaternionVector to vpRotationMatrix " << std::endl;
377 std::cout <<
"From vpRotationMatrix to vpRxyzVector " << std::endl;
379 std::cout << RxyzBuildFromR << std::endl;
381 std::cout <<
"From vpRxyzVector to vpThetaUVector " << std::endl;
382 std::cout <<
" use From vpRxyzVector to vpRotationMatrix " << std::endl;
383 std::cout <<
" use From vpRotationMatrix to vpThetaUVector " << std::endl;
388 std::cout << std::endl;
389 std::cout <<
"result : should equivalent to the first one " << std::endl;
394 tuBuildFromEu.
extract(theta2, u2);
395 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl;
396 std::cout <<
"u=" << u2 << std::endl;
398 assert(
vpMath::abs(theta2 - theta) < std::numeric_limits<double>::epsilon() * 1e10);
399 assert(
vpMath::abs(u[0] - u2[0]) < std::numeric_limits<double>::epsilon() * 1e10);
400 assert(
vpMath::abs(u[1] - u2[1]) < std::numeric_limits<double>::epsilon() * 1e10);
401 assert(
vpMath::abs(u[2] - u2[2]) < std::numeric_limits<double>::epsilon() * 1e10);
404 std::cout <<
"Initialization vpRzyzVector " << std::endl;
405 std::cout << rzyz << std::endl;
406 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl;
408 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl;
411 std::cout << rzyz_final << std::endl;
414 std::cout <<
"Initialization vpRzyxVector " << std::endl;
415 std::cout << rzyx << std::endl;
416 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl;
418 std::cout << R << std::endl;
419 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl;
422 std::cout << rzyx_final << std::endl;
428 _1_M_2_truth[0][0] = 0.9835;
429 _1_M_2_truth[0][1] = -0.0581;
430 _1_M_2_truth[0][2] = 0.1716;
431 _1_M_2_truth[0][3] = 0;
432 _1_M_2_truth[1][0] = -0.0489;
433 _1_M_2_truth[1][1] = -0.9972;
434 _1_M_2_truth[1][2] = -0.0571;
435 _1_M_2_truth[1][3] = 0;
436 _1_M_2_truth[2][0] = 0.1744;
437 _1_M_2_truth[2][1] = 0.0478;
438 _1_M_2_truth[2][2] = -0.9835;
439 _1_M_2_truth[2][3] = 0;
441 _2_M_3_[0][0] = 0.9835;
442 _2_M_3_[0][1] = -0.0581;
443 _2_M_3_[0][2] = 0.1716;
444 _2_M_3_[0][3] = 0.0072;
445 _2_M_3_[1][0] = -0.0489;
446 _2_M_3_[1][1] = -0.9972;
447 _2_M_3_[1][2] = -0.0571;
448 _2_M_3_[1][3] = 0.0352;
449 _2_M_3_[2][0] = 0.1744;
450 _2_M_3_[2][1] = 0.0478;
451 _2_M_3_[2][2] = -0.9835;
452 _2_M_3_[2][3] = 0.9470;
457 bool success = test_matrix_equal(_1_M_3_, _1_M_3_truth);
458 std::cout <<
"Test vpHomogeneousMatrix vpRotationMatrix::operator*(vpHomogeneousMatrix) " << (success ?
"succeed" :
"failed") << std::endl;
463 std::cout <<
"All tests succeed" << std::endl;
467 std::cout <<
"Catch an exception: " << e << std::endl;
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
static double rad(double deg)
static Type abs(const Type &x)
static bool equal(double x, double y, double threshold=0.001)
static double deg(double rad)
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.
Implementation of a rotation vector as Euler angle minimal representation.
vpRzyxVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as Euler angle minimal representation.
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)