matrix-homogeneous.cpp
Go to the documentation of this file.
1 // Copyright 2011 Florent Lamiraux
2 
3 #include <sstream>
4 
5 #define BOOST_TEST_MODULE matrix_homogeneous
6 
7 #include <boost/math/constants/constants.hpp>
8 #include <boost/test/floating_point_comparison.hpp>
9 #include <boost/test/output_test_stream.hpp>
10 #include <boost/test/unit_test.hpp>
12 
13 using boost::math::constants::pi;
14 using boost::test_tools::output_test_stream;
15 
16 namespace dg = dynamicgraph;
17 
18 #define MATRIX_BOOST_REQUIRE_CLOSE(N, M, LEFT, RIGHT, TOLERANCE) \
19  for (unsigned i = 0; i < N; ++i) \
20  for (unsigned j = 0; j < M; ++j) \
21  BOOST_REQUIRE_CLOSE(LEFT(i, j), RIGHT(i, j), TOLERANCE)
22 
23 #define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE) \
24  MATRIX_BOOST_REQUIRE_CLOSE(4, 4, LEFT, RIGHT, TOLERANCE)
25 
26 #define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE) \
27  for (unsigned i = 0; i < 4; ++i) \
28  for (unsigned j = 0; j < 4; ++j) \
29  if (i == j) \
30  BOOST_REQUIRE_CLOSE(M(i, j), 1., TOLERANCE); \
31  else \
32  BOOST_CHECK_SMALL(M(i, j), .01 * TOLERANCE)
33 
34 #define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw) \
35  M(0, 0) = cos(pitch) * cos(yaw); \
36  M(0, 1) = sin(roll) * sin(pitch) * cos(yaw) - cos(roll) * sin(yaw); \
37  M(0, 2) = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * sin(yaw); \
38  M(0, 3) = tx; \
39  M(1, 0) = cos(pitch) * sin(yaw); \
40  M(1, 1) = sin(roll) * sin(pitch) * sin(yaw) + cos(roll) * cos(yaw); \
41  M(1, 2) = cos(roll) * sin(pitch) * sin(yaw) - sin(roll) * cos(yaw); \
42  M(1, 3) = ty; \
43  M(2, 0) = -sin(pitch); \
44  M(2, 1) = sin(roll) * cos(pitch); \
45  M(2, 2) = cos(roll) * cos(pitch); \
46  M(2, 3) = tz; \
47  M(3, 0) = 0.; \
48  M(3, 1) = 0.; \
49  M(3, 2) = 0.; \
50  M(3, 3) = 1.
51 
53  for (unsigned int i = 0; i < 1000; i++) {
54  double tx, ty, tz;
55  double roll, pitch, yaw;
57  tx = (10. * rand()) / RAND_MAX - 5.;
58  ty = (10. * rand()) / RAND_MAX - 5.;
59  tz = (10. * rand()) / RAND_MAX - 5.;
60  roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
61  pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
62  yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
63  MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
64  dg::Matrix M1(H1.matrix());
66  tx = (10. * rand()) / RAND_MAX;
67  ty = (10. * rand()) / RAND_MAX - 5.;
68  tz = (10. * rand()) / RAND_MAX - 5.;
69  roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
70  pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
71  yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
72  MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
73  dg::Matrix M2(H2.matrix());
75  dg::Matrix M3;
76  M3 = M1 * M2;
77 
78  MATRIX_4x4_BOOST_REQUIRE_CLOSE(M3, H3.matrix(), 0.0001);
79  }
80 }
81 
83  for (unsigned int i = 0; i < 1000; i++) {
84  double tx, ty, tz;
85  double roll, pitch, yaw;
87  tx = (10. * rand()) / RAND_MAX - 5.;
88  ty = (10. * rand()) / RAND_MAX - 5.;
89  tz = (10. * rand()) / RAND_MAX - 5.;
90  roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
91  pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
92  yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
93  MATRIX_HOMO_INIT(H1, tx, ty, tz, roll, pitch, yaw);
95  tx = (10. * rand()) / RAND_MAX;
96  ty = (10. * rand()) / RAND_MAX - 5.;
97  tz = (10. * rand()) / RAND_MAX - 5.;
98  roll = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
99  pitch = (pi<double>() * rand()) / RAND_MAX - .5 * pi<double>();
100  yaw = (2 * pi<double>() * rand()) / RAND_MAX - pi<double>();
101  MATRIX_HOMO_INIT(H2, tx, ty, tz, roll, pitch, yaw);
103  dynamicgraph::sot::MatrixHomogeneous invH1, invH2, invH3;
104  invH1 = H1.inverse();
105  invH2 = H2.inverse();
106  invH3 = H3.inverse();
107 
112  dynamicgraph::sot::MatrixHomogeneous P4 = invH2 * invH1;
113 
117  MATRIX_4x4_BOOST_REQUIRE_CLOSE(P4, invH3, 0.0001);
118  }
119 }
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
tuple P1
#define MATRIX_IDENTITY_4x4_REQUIRE_CLOSE(M, TOLERANCE)
int i
BOOST_AUTO_TEST_CASE(product)
tuple P3
def rand(n)
Eigen::MatrixXd Matrix
tuple P2
#define MATRIX_4x4_BOOST_REQUIRE_CLOSE(LEFT, RIGHT, TOLERANCE)
#define MATRIX_HOMO_INIT(M, tx, ty, tz, roll, pitch, yaw)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26