45#include <visp3/core/vpMath.h>
46#include <visp3/core/vpQuaternionVector.h>
47#include <visp3/core/vpRotationMatrix.h>
48#include <visp3/io/vpParseArgv.h>
55static unsigned int cpt = 0;
57bool test(
const std::string &s,
const vpArray2D<double> &v,
const std::vector<double> &bench)
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;
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;
77 std::cout <<
"** Test " << ++cpt << std::endl;
78 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
80 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
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;
93bool test(
const std::string &s,
const vpRotationVector &v,
const double &bench)
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;
114 if (test(
"r1", r1, bench1) ==
false)
118 bench1 = r1.toStdVector();
119 if (test(
"r1", r1, bench1) ==
false)
122 r1.buildFrom(bench3);
123 if (test(
"r1", r1, bench3) ==
false)
127 if (test(
"r2", r2, bench1) ==
false)
135 if (test(
"r3", r3, bench1) ==
false)
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;
148 if (test(
"r4", r4, bench2) ==
false)
152 if (test(
"r5", r5, bench1) ==
false)
159 if (test(
"r1", r1, bench1) ==
false)
163 bench1 = r1.toStdVector();
164 if (test(
"r1", r1, bench1) ==
false)
167 r1.buildFrom(bench3);
168 if (test(
"r1", r1, bench3) ==
false)
172 if (test(
"r2", r2, bench1) ==
false)
180 if (test(
"r3", r3, bench1) ==
false)
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;
193 if (test(
"r4", r4, bench2) ==
false)
197 if (test(
"r5", r5, bench1) ==
false)
204 if (test(
"r1", r1, bench1) ==
false)
208 bench1 = r1.toStdVector();
209 if (test(
"r1", r1, bench1) ==
false)
212 r1.buildFrom(bench3);
213 if (test(
"r1", r1, bench3) ==
false)
217 if (test(
"r2", r2, bench1) ==
false)
225 if (test(
"r3", r3, bench1) ==
false)
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;
238 if (test(
"r4", r4, bench2) ==
false)
242 if (test(
"r5", r5, bench1) ==
false)
249 if (test(
"r1", r1, bench1) ==
false)
253 bench1 = r1.toStdVector();
254 if (test(
"r1", r1, bench1) ==
false)
257 r1.buildFrom(bench3);
258 if (test(
"r1", r1, bench3) ==
false)
262 if (test(
"r2", r2, bench1) ==
false)
270 if (test(
"r3", r3, bench1) ==
false)
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;
283 if (test(
"r4", r4, bench2) ==
false)
287 if (test(
"r5", r5, bench1) ==
false)
294 if (test(
"r1", r1, bench1) ==
false)
298 bench1 = r1.toStdVector();
299 if (test(
"r1", r1, bench1) ==
false)
302 r1.buildFrom(bench3);
303 if (test(
"r1", r1, bench3) ==
false)
307 if (test(
"r2", r2, bench1) ==
false)
315 if (test(
"r3", r3, bench1) ==
false)
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;
328 if (test(
"r4", r4, bench2) ==
false)
332 if (test(
"r5", r5, bench1) ==
false)
337 for (
int i = -10; i < 10; i++) {
338 for (
int j = -10; j < 10; j++) {
342 std::cout <<
"Initialization " << std::endl;
348 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl;
349 std::cout <<
"u=" << u << std::endl;
351 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
354 std::cout <<
"Matrix R";
356 std::cout <<
" is a rotation matrix " << std::endl;
358 std::cout <<
" is not a rotation matrix " << std::endl;
360 std::cout << R << std::endl;
362 std::cout <<
"From vpRotationMatrix to vpQuaternionVector " << std::endl;
364 std::cout << q << std::endl;
367 std::cout <<
"From vpQuaternionVector to vpRotationMatrix " << std::endl;
369 std::cout <<
"From vpRotationMatrix to vpRxyzVector " << std::endl;
371 std::cout << RxyzBuildFromR << std::endl;
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;
380 std::cout << std::endl;
381 std::cout <<
"result : should equivalent to the first one " << std::endl;
386 tuBuildFromEu.
extract(theta2, u2);
387 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl;
388 std::cout <<
"u=" << u2 << std::endl;
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);
396 std::cout <<
"Initialization vpRzyzVector " << std::endl;
397 std::cout << rzyz << std::endl;
398 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl;
400 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl;
403 std::cout << rzyz_final << std::endl;
406 std::cout <<
"Initialization vpRzyxVector " << std::endl;
407 std::cout << rzyx << std::endl;
408 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl;
410 std::cout << R << std::endl;
411 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl;
414 std::cout << rzyx_final << std::endl;
417 std::cout <<
"All tests succeed" << std::endl;
420 std::cout <<
"Catch an exception: " << e << std::endl;
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 emited by ViSP classes.
static double rad(double deg)
static Type abs(const Type &x)
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)