40 #include <visp3/core/vpConfig.h>
42 #ifdef VISP_HAVE_CATCH2
44 #include <visp3/core/vpThetaUVector.h>
45 #include <visp3/core/vpUniRand.h>
47 #define CATCH_CONFIG_RUNNER
62 const double ctheta = std::cos(angle);
63 const double stheta = std::sin(angle);
64 const double ax = rng.
uniform(-1.0, 1.0);
65 const double ay = rng.
uniform(-1.0, 1.0);
66 const double az = rng.
uniform(-1.0, 1.0);
72 bool test(
const std::string &s,
const vpArray2D<double> &v,
const std::vector<double> &bench)
74 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
75 if (bench.size() != v.
size()) {
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;
91 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
93 std::cout <<
"Test fails: bad size wrt bench" << std::endl;
96 for (
unsigned int i = 0; i < v.
size(); i++) {
97 if (std::fabs(v.
data[i] - bench[i]) > std::fabs(v.
data[i]) * std::numeric_limits<double>::epsilon()) {
98 std::cout <<
"Test fails: bad content" << std::endl;
106 bool test(
const std::string &s,
const vpRotationVector &v,
const double &bench)
108 std::cout << s <<
"(" << v.
getRows() <<
"," << v.
getCols() <<
") = [" << v <<
"]" << std::endl;
109 for (
unsigned int i = 0; i < v.
size(); i++) {
110 if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
111 std::cout <<
"Test fails: bad content" << std::endl;
121 for (
unsigned int i = 0; i < 4; i++) {
122 for (
unsigned int j = 0; j < 4; j++) {
131 TEST_CASE(
"Common rotation operations",
"[rotation]")
133 SECTION(
"Theta u initialization")
138 CHECK(test(
"r1", r1, bench1));
141 bench1 = r1.toStdVector();
142 CHECK(test(
"r1", r1, bench1));
144 r1.buildFrom(bench3);
145 CHECK(test(
"r1", r1, bench3));
148 CHECK(test(
"r2", r2, bench1));
149 CHECK(r2.
data != r1.data);
155 CHECK(test(
"r3", r3, bench1));
157 for (
unsigned int i = 0; i < r3.
size(); i++) {
158 CHECK(std::fabs(r3[i] - bench1[i]) < std::fabs(r3[i]) * std::numeric_limits<double>::epsilon());
163 CHECK(test(
"r4", r4, bench2));
166 CHECK(test(
"r5", r5, bench1));
168 SECTION(
"Rxyz initialization")
173 CHECK(test(
"r1", r1, bench1));
176 bench1 = r1.toStdVector();
177 CHECK(test(
"r1", r1, bench1));
179 r1.buildFrom(bench3);
180 CHECK(test(
"r1", r1, bench3));
183 CHECK(test(
"r2", r2, bench1));
189 CHECK(test(
"r3", r3, bench1));
191 for (
unsigned int i = 0; i < r3.
size(); i++) {
192 CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits<double>::epsilon());
197 CHECK(test(
"r4", r4, bench2));
200 CHECK(test(
"r5", r5, bench1));
202 SECTION(
"rzyx initialization")
207 CHECK(test(
"r1", r1, bench1));
210 bench1 = r1.toStdVector();
211 CHECK(test(
"r1", r1, bench1));
213 r1.buildFrom(bench3);
214 CHECK(test(
"r1", r1, bench3));
217 CHECK(test(
"r2", r2, bench1));
223 CHECK(test(
"r3", r3, bench1));
225 for (
unsigned int i = 0; i < r3.
size(); i++) {
226 CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits<double>::epsilon());
231 CHECK(test(
"r4", r4, bench2));
234 CHECK(test(
"r5", r5, bench1));
236 SECTION(
"rzyz initialiation")
241 CHECK(test(
"r1", r1, bench1));
244 bench1 = r1.toStdVector();
245 CHECK(test(
"r1", r1, bench1));
247 r1.buildFrom(bench3);
248 CHECK(test(
"r1", r1, bench3));
251 CHECK(test(
"r2", r2, bench1));
257 CHECK(test(
"r3", r3, bench1));
259 for (
unsigned int i = 0; i < r3.
size(); i++) {
260 CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits<double>::epsilon());
265 CHECK(test(
"r4", r4, bench2));
268 CHECK(test(
"r5", r5, bench1));
270 SECTION(
"Test quaternion initialization",
"[quaternion]")
275 CHECK(test(
"r1", r1, bench1));
278 bench1 = r1.toStdVector();
279 CHECK(test(
"r1", r1, bench1));
281 r1.buildFrom(bench3);
282 CHECK(test(
"r1", r1, bench3));
285 CHECK(test(
"r2", r2, bench1));
291 CHECK(test(
"r3", r3, bench1));
293 for (
unsigned int i = 0; i < r3.
size(); i++) {
294 CHECK(std::fabs(r3[i] - bench1[i]) <= std::fabs(r3[i]) * std::numeric_limits<double>::epsilon());
299 CHECK(test(
"r4", r4, bench2));
302 CHECK(test(
"r5", r5, bench1));
304 SECTION(
"Conversions")
307 for (
int i = -10; i < 10; i++) {
308 for (
int j = -10; j < 10; j++) {
312 std::cout <<
"Initialization " << std::endl;
318 std::cout <<
"theta=" <<
vpMath::deg(theta) << std::endl;
319 std::cout <<
"u=" << u << std::endl;
321 std::cout <<
"From vpThetaUVector to vpRotationMatrix " << std::endl;
324 std::cout <<
"Matrix R";
327 std::cout << R << std::endl;
329 std::cout <<
"From vpRotationMatrix to vpQuaternionVector " << std::endl;
331 CHECK(q.magnitude() == Approx(1.0).margin(1e-4));
332 std::cout << q << std::endl;
336 std::cout <<
"From vpQuaternionVector to vpRotationMatrix " << std::endl;
338 std::cout <<
"From vpRotationMatrix to vpRxyzVector " << std::endl;
340 std::cout << RxyzBuildFromR << std::endl;
342 std::cout <<
"From vpRxyzVector to vpThetaUVector " << std::endl;
343 std::cout <<
" use From vpRxyzVector to vpRotationMatrix " << std::endl;
344 std::cout <<
" use From vpRotationMatrix to vpThetaUVector " << std::endl;
349 std::cout << std::endl;
350 std::cout <<
"result : should equivalent to the first one " << std::endl;
355 tuBuildFromEu.
extract(theta2, u2);
356 std::cout <<
"theta=" <<
vpMath::deg(theta2) << std::endl;
357 std::cout <<
"u=" << u2 << std::endl;
359 CHECK(
vpMath::abs(theta2 - theta) < std::numeric_limits<double>::epsilon() * 1e10);
360 CHECK(
vpMath::abs(u[0] - u2[0]) < std::numeric_limits<double>::epsilon() * 1e10);
361 CHECK(
vpMath::abs(u[1] - u2[1]) < std::numeric_limits<double>::epsilon() * 1e10);
362 CHECK(
vpMath::abs(u[2] - u2[2]) < std::numeric_limits<double>::epsilon() * 1e10);
365 SECTION(
"Conversion from and to rzyz vector")
368 std::cout <<
"Initialization vpRzyzVector " << std::endl;
369 std::cout << rzyz << std::endl;
370 std::cout <<
"From vpRzyzVector to vpRotationMatrix " << std::endl;
373 std::cout <<
"From vpRotationMatrix to vpRzyzVector " << std::endl;
376 CHECK(test(
"rzyz", rzyz_final, rzyz));
377 std::cout << rzyz_final << std::endl;
379 SECTION(
"Conversion from and to rzyx vector")
382 std::cout <<
"Initialization vpRzyxVector " << std::endl;
383 std::cout << rzyx << std::endl;
384 std::cout <<
"From vpRzyxVector to vpRotationMatrix " << std::endl;
387 std::cout << R << std::endl;
388 std::cout <<
"From vpRotationMatrix to vpRzyxVector " << std::endl;
391 bool ret = test(
"rzyx", rzyx_final, rzyx);
394 std::cout <<
"Rzyx vector differ. Test rotation matrix..." << std::endl;
397 std::cout <<
"Rzyx vector differ but rotation matrix is valid" << std::endl;
402 std::cout << rzyx_final << std::endl;
405 SECTION(
"Rotation matrix extraction from homogeneous matrix and multiplication")
409 _1_M_2_truth[0][0] = 0.9835;
410 _1_M_2_truth[0][1] = -0.0581;
411 _1_M_2_truth[0][2] = 0.1716;
412 _1_M_2_truth[0][3] = 0;
413 _1_M_2_truth[1][0] = -0.0489;
414 _1_M_2_truth[1][1] = -0.9972;
415 _1_M_2_truth[1][2] = -0.0571;
416 _1_M_2_truth[1][3] = 0;
417 _1_M_2_truth[2][0] = 0.1744;
418 _1_M_2_truth[2][1] = 0.0478;
419 _1_M_2_truth[2][2] = -0.9835;
420 _1_M_2_truth[2][3] = 0;
422 _2_M_3_[0][0] = 0.9835;
423 _2_M_3_[0][1] = -0.0581;
424 _2_M_3_[0][2] = 0.1716;
425 _2_M_3_[0][3] = 0.0072;
426 _2_M_3_[1][0] = -0.0489;
427 _2_M_3_[1][1] = -0.9972;
428 _2_M_3_[1][2] = -0.0571;
429 _2_M_3_[1][3] = 0.0352;
430 _2_M_3_[2][0] = 0.1744;
431 _2_M_3_[2][1] = 0.0478;
432 _2_M_3_[2][2] = -0.9835;
433 _2_M_3_[2][3] = 0.9470;
438 CHECK(test_matrix_equal(_1_M_3_, _1_M_3_truth));
442 TEST_CASE(
"Theta u multiplication",
"[theta.u]")
444 const int nTrials = 100;
445 const uint64_t seed = 0x123456789;
447 for (
int iter = 0; iter < nTrials; iter++) {
459 const double tolerance = 1e-9;
460 for (
unsigned int i = 0; i < 3; i++) {
461 for (
unsigned int j = 0; j < 3; j++) {
462 CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance));
468 TEST_CASE(
"Quaternion multiplication",
"[quaternion]")
470 const int nTrials = 100;
471 const uint64_t seed = 0x123456789;
473 for (
int iter = 0; iter < nTrials; iter++) {
486 const double tolerance = 1e-9;
487 for (
unsigned int i = 0; i < 3; i++) {
488 for (
unsigned int j = 0; j < 3; j++) {
489 CHECK(c1Rc3_ref[i][j] == Approx(c1Rc3[i][j]).epsilon(0).margin(tolerance));
495 int main(
int argc,
char *argv[])
497 Catch::Session session;
500 session.applyCommandLine(argc, argv);
502 int numFailed = session.run();
512 int main() {
return EXIT_SUCCESS; }
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
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)
Class for generating random numbers with uniform probability density.
int uniform(int a, int b)