test_operator.cpp
Go to the documentation of this file.
1 #include <iostream>
4 
5 #include "../../src/matrix/operator.cpp"
6 
7 namespace dg = ::dynamicgraph;
8 using namespace dynamicgraph::sot;
9 
10 #define BOOST_TEST_MODULE test - operator
11 
12 #include <boost/test/output_test_stream.hpp>
13 #include <boost/test/unit_test.hpp>
14 
15 using boost::test_tools::output_test_stream;
16 
17 BOOST_AUTO_TEST_CASE(test_vector_selecter) {
18  // Test VectorSelecter registered as aSelec_of_vector
19  VectorSelecter aSelec_of_vector;
20 
21  output_test_stream output;
22 
23  output << aSelec_of_vector.nameTypeIn();
24  BOOST_CHECK(output.is_equal("Vector"));
25 
26  output << aSelec_of_vector.nameTypeOut();
27  BOOST_CHECK(output.is_equal("Vector"));
28 
29  output << aSelec_of_vector.getDocString();
30  BOOST_CHECK(
31  output.is_equal("Undocumented unary operator\n"
32  " - input Vector\n"
33  " - output Vector\n"));
34  dg::Vector vIn(10), vOut(10);
35  for (unsigned int i = 0; i < 10; i++) vIn(i) = i;
36 
37  aSelec_of_vector.setBounds(3, 5);
38  aSelec_of_vector.addBounds(7, 10);
39  aSelec_of_vector(vIn, vOut);
40  output << vOut;
41 
42  BOOST_CHECK(output.is_equal("3\n4\n7\n8\n9"));
43 
44  output << dg::sot::UnaryOp<VectorSelecter>::CLASS_NAME;
45  BOOST_CHECK(output.is_equal("Selec_of_vector"));
46 
47  dg::Entity *anEntity = regFunction_Selec_of_vector("test_Selec_of_vector");
48  dg::sot::UnaryOp<VectorSelecter> *aVectorSelecter =
49  dynamic_cast<dg::sot::UnaryOp<VectorSelecter> *>(anEntity);
50  output << aVectorSelecter->getTypeInName();
51  BOOST_CHECK(output.is_equal("Vector"));
52 
53  output << aVectorSelecter->getTypeOutName();
54  BOOST_CHECK(output.is_equal("Vector"));
55 
56  output << aVectorSelecter->getClassName();
57  BOOST_CHECK(output.is_equal("Selec_of_vector"));
58 
59  output << aVectorSelecter->getDocString();
60  BOOST_CHECK(
61  output.is_equal("Undocumented unary operator\n"
62  " - input Vector\n"
63  " - output Vector\n"));
64 }
65 
66 BOOST_AUTO_TEST_CASE(test_vector_component) {
67  // Test Vector Component
68  VectorComponent aComponent_of_vector;
69 
70  output_test_stream output;
71 
72  aComponent_of_vector.setIndex(1);
73  dg::Vector vIn(3);
74  for (unsigned int i = 0; i < 3; i++) vIn(i) = i;
75 
76  double res;
77  aComponent_of_vector(vIn, res);
78  BOOST_CHECK(res == 1.0);
79 
80  output << aComponent_of_vector.getDocString();
81  BOOST_CHECK(
82  output.is_equal("Select a component of a vector\n"
83  " - input vector\n"
84  " - output double"));
85 
86  output << aComponent_of_vector.nameTypeIn();
87  BOOST_CHECK(output.is_equal("Vector"));
88  output << aComponent_of_vector.nameTypeOut();
89  BOOST_CHECK(output.is_equal("double"));
90 
91  dg::Entity *anEntity =
92  regFunction_Component_of_vector("test_Component_of_vector");
93  dg::sot::UnaryOp<VectorComponent> *aVectorSelecter =
94  dynamic_cast<dg::sot::UnaryOp<VectorComponent> *>(anEntity);
95  output << aVectorSelecter->getTypeInName();
96  BOOST_CHECK(output.is_equal("Vector"));
97 
98  output << aVectorSelecter->getTypeOutName();
99  BOOST_CHECK(output.is_equal("double"));
100 
101  output << aVectorSelecter->getClassName();
102  BOOST_CHECK(output.is_equal("Component_of_vector"));
103 
104  output << aVectorSelecter->getDocString();
105  BOOST_CHECK(
106  output.is_equal("Select a component of a vector\n"
107  " - input vector\n"
108  " - output double"));
109 }
110 
111 BOOST_AUTO_TEST_CASE(test_matrix_selector) {
112  MatrixSelector aSelec_of_matrix;
113  output_test_stream output;
114 
115  aSelec_of_matrix.setBoundsRow(2, 4);
116  aSelec_of_matrix.setBoundsCol(2, 4);
117 
118  dg::Matrix aMatrix(5, 5);
119  for (unsigned int i = 0; i < 5; i++)
120  for (unsigned int j = 0; j < 5; j++) aMatrix(i, j) = i * 5 + j;
121 
122  dg::Matrix resMatrix(2, 2);
123  aSelec_of_matrix(aMatrix, resMatrix);
124  BOOST_CHECK(resMatrix(0, 0) == 12.0);
125  BOOST_CHECK(resMatrix(0, 1) == 13.0);
126  BOOST_CHECK(resMatrix(1, 0) == 17.0);
127  BOOST_CHECK(resMatrix(1, 1) == 18.0);
128 
129  output << aSelec_of_matrix.nameTypeIn();
130  BOOST_CHECK(output.is_equal("Matrix"));
131  output << aSelec_of_matrix.nameTypeOut();
132  BOOST_CHECK(output.is_equal("Matrix"));
133 
134  dg::Entity *anEntity = regFunction_Selec_of_matrix("test_Selec_of_matrix");
135  dg::sot::UnaryOp<MatrixSelector> *aMatrixSelector =
136  dynamic_cast<dg::sot::UnaryOp<MatrixSelector> *>(anEntity);
137  output << aMatrixSelector->getTypeInName();
138  BOOST_CHECK(output.is_equal("Matrix"));
139 
140  output << aMatrixSelector->getTypeOutName();
141  BOOST_CHECK(output.is_equal("Matrix"));
142 
143  output << aMatrixSelector->getClassName();
144  BOOST_CHECK(output.is_equal("Selec_of_matrix"));
145 }
146 
147 BOOST_AUTO_TEST_SUITE(test_rotation_conversions)
148 
149 template <typename type>
150 type random();
151 template <>
153  return VectorRollPitchYaw::Random();
154 }
155 template <>
157  return VectorQuaternion(Eigen::Vector4d::Random().normalized());
158 }
159 template <>
162 }
163 template <>
165  MatrixHomogeneous matrix_homo;
166  matrix_homo.translation() = Eigen::Vector3d::Random();
167  matrix_homo.linear() = random<MatrixRotation>();
168  return matrix_homo;
169 }
170 
171 template <typename type>
172 bool compare(const type &a, const type &b) {
173  return a.isApprox(b);
174 }
175 template <>
177  const VectorQuaternion &b) {
178  return a.isApprox(b) || a.coeffs().isApprox(-b.coeffs());
179 }
180 
181 template <typename AtoB, typename BtoA>
182 void test_impl() {
183  typedef typename AtoB::Tin A;
184  typedef typename AtoB::Tout B;
185 
186  AtoB a2b;
187  BtoA b2a;
188 
189  for (std::size_t i = 1; i < 10; ++i) {
190  A ain = random<A>(), aout;
191  B b;
192 
193  a2b(ain, b);
194  b2a(b, aout);
195 
196  BOOST_CHECK(compare(ain, aout));
197  }
198 }
199 
200 BOOST_AUTO_TEST_CASE(matrix_rpy) { test_impl<MatrixToRPY, RPYToMatrix>(); }
201 BOOST_AUTO_TEST_CASE(quaternion_rpy) {
202  test_impl<QuaternionToRPY, RPYToQuaternion>();
203 }
204 BOOST_AUTO_TEST_CASE(matrix_quaternion) {
205  test_impl<MatrixToQuaternion, QuaternionToMatrix>();
206 }
207 BOOST_AUTO_TEST_CASE(matrixHomo_poseQuaternion) {
208  test_impl<MatrixHomoToPoseQuaternion, PoseQuaternionToMatrixHomo>();
209 }
210 BOOST_AUTO_TEST_CASE(matrixHomo_se3Vector) {
211  test_impl<MatrixHomoToSE3Vector, SE3VectorToMatrixHomo>();
212 }
213 
bool compare< VectorQuaternion >(const VectorQuaternion &a, const VectorQuaternion &b)
Eigen::VectorXd Vector
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
int i
Vec3f b
void setBoundsCol(const int &m, const int &M)
Definition: operator.hh:158
Eigen::Transform< double, 3, Eigen::Affine > MatrixHomogeneous
Eigen::Matrix< double, 3, 3 > MatrixRotation
static std::string nameTypeIn(void)
Definition: operator.hh:42
VectorRollPitchYaw random< VectorRollPitchYaw >()
void addBounds(const int &m, const int &M)
Definition: operator.hh:87
static std::string nameTypeOut(void)
Definition: operator.hh:45
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
std::tuple_element< i, std::tuple< Args... > >::type type
void test_impl()
MatrixHomogeneous random< MatrixHomogeneous >()
VectorQuaternion random< VectorQuaternion >()
Vec3f a
std::string getDocString() const
Definition: operator.hh:49
MatrixRotation random< MatrixRotation >()
void setBounds(const int &m, const int &M)
Definition: operator.hh:83
B
std::string getDocString() const
Definition: operator.hh:131
res
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Eigen::MatrixXd Matrix
BOOST_AUTO_TEST_SUITE_END() MatrixHomogeneous randomM()
type random()
A
bool compare(const type &a, const type &b)
void setBoundsRow(const int &m, const int &M)
Definition: operator.hh:154
BOOST_AUTO_TEST_CASE(test_vector_selecter)


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