Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
testEigenConversion.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test conversion between ViSP and Eigen type.
33  *
34  *****************************************************************************/
35 
42 #include <visp3/core/vpConfig.h>
43 #include <visp3/core/vpEigenConversion.h>
44 
45 #if defined(VISP_HAVE_EIGEN3) && defined(VISP_HAVE_CATCH2)
46 #define CATCH_CONFIG_RUNNER
47 #include <catch.hpp>
48 
49 namespace
50 {
51 template<typename Type>
52 std::ostream &operator<<(std::ostream &os, const Eigen::Quaternion<Type> &q) {
53  return os << "qw: " << q.w() << " ; qx: " << q.x() << " ; qy: "
54  << q.y() << " ; qz: " << q.z();
55 }
56 
57 template<typename Type>
58 std::ostream &operator<<(std::ostream &os, const Eigen::AngleAxis<Type> &aa) {
59  return os << "angle: " << aa.angle() << " ; axis: " << aa.axis()(0)
60  << " ; " << aa.axis()(1) << " ; " << aa.axis()(2)
61  << " ; thetau: " << aa.angle()*aa.axis()(0)
62  << " ; " << aa.angle()*aa.axis()(1)
63  << " ; " << aa.angle()*aa.axis()(2);
64 }
65 }
66 
67 TEST_CASE("vpMatrix <--> Eigen::MatrixXd/Matrix3Xd conversion", "[eigen_conversion]") {
68  vpMatrix visp_m(3, 4);
69  for (unsigned int i = 0; i < visp_m.size(); i++) {
70  visp_m.data[i] = i;
71  }
72 
73  {
74  Eigen::MatrixXd eigen_m;
75  vp::visp2eigen(visp_m, eigen_m);
76  std::cout << "Eigen MatrixXd:\n" << eigen_m << std::endl;
77 
78  vpMatrix visp_m2;
79  vp::eigen2visp(eigen_m, visp_m2);
80  std::cout << "ViSP vpMatrix:\n" << visp_m2 << std::endl;
81 
82  REQUIRE(visp_m == visp_m2);
83  std::cout << std::endl;
84  }
85  {
86  Eigen::Matrix3Xd eigen_m;
87  vp::visp2eigen(visp_m, eigen_m);
88  std::cout << "Eigen Matrix3Xd:\n" << eigen_m << std::endl;
89 
90  vpMatrix visp_m2;
91  vp::eigen2visp(eigen_m, visp_m2);
92  std::cout << "ViSP vpMatrix:\n" << visp_m2 << std::endl;
93 
94  REQUIRE(visp_m == visp_m2);
95  std::cout << std::endl;
96  }
97 }
98 
99 TEST_CASE("Eigen::MatrixXd <--> vpMatrix conversion", "[eigen_conversion]") {
100  Eigen::MatrixXd eigen_m(3, 5);
101 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
102  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
103  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
104 #else
105  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
106  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
107 #endif
108  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
109  }
110  }
111  std::cout << "Eigen Matrix (row major: " << eigen_m.IsRowMajor << "):\n" << eigen_m << std::endl;
112 
113  vpMatrix visp_m;
114  vp::eigen2visp(eigen_m, visp_m);
115  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
116 
117  Eigen::MatrixXd eigen_m2;
118  vp::visp2eigen(visp_m, eigen_m2);
119  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
120 
121  vpMatrix visp_m2;
122  vp::eigen2visp(eigen_m2, visp_m2);
123  REQUIRE(visp_m == visp_m2);
124  std::cout << std::endl;
125 }
126 
127 TEST_CASE("Eigen::MatrixX4d <--> vpMatrix conversion", "[eigen_conversion]") {
128  Eigen::MatrixX4d eigen_m(2, 4);
129 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
130  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
131  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
132 #else
133  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
134  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
135 #endif
136  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
137  }
138  }
139  std::cout << "Eigen MatrixX4d (row major: " << eigen_m.IsRowMajor << "):\n" << eigen_m << std::endl;
140 
141  vpMatrix visp_m;
142  vp::eigen2visp(eigen_m, visp_m);
143  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
144 
145  Eigen::MatrixX4d eigen_m2;
146  vp::visp2eigen(visp_m, eigen_m2);
147  std::cout << "Eigen MatrixX4d (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
148 
149  vpMatrix visp_m2;
150  vp::eigen2visp(eigen_m2, visp_m2);
151  REQUIRE(visp_m == visp_m2);
152  std::cout << std::endl;
153 }
154 
155 TEST_CASE("Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> <--> vpMatrix conversion", "[eigen_conversion]") {
156  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> eigen_m(3, 5);
157 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
158  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
159  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
160 #else
161  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
162  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
163 #endif
164  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
165  }
166  }
167  std::cout << "Eigen Matrix (RowMajor):\n" << eigen_m << std::endl;
168 
169  vpMatrix visp_m;
170  vp::eigen2visp(eigen_m, visp_m);
171  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
172 
173  Eigen::MatrixXd eigen_m2;
174  vp::visp2eigen(visp_m, eigen_m2);
175  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
176 
177  vpMatrix visp_m2;
178  vp::eigen2visp(eigen_m2, visp_m2);
179  REQUIRE(visp_m == visp_m2);
180  std::cout << std::endl;
181 }
182 
183 TEST_CASE("Eigen::Matrix<double, Dynamic, Dynamic, ColMajor> <--> vpMatrix conversion", "[eigen_conversion]") {
184  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> eigen_m(3, 5);
185 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
186  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
187  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
188 #else
189  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
190  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
191 #endif
192  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
193  }
194  }
195  std::cout << "Eigen Matrix (ColMajor):\n" << eigen_m << std::endl;
196 
197  vpMatrix visp_m;
198  vp::eigen2visp(eigen_m, visp_m);
199  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
200 
201  Eigen::MatrixXd eigen_m2;
202  vp::visp2eigen(visp_m, eigen_m2);
203  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
204 
205  vpMatrix visp_m2;
206  vp::eigen2visp(eigen_m2, visp_m2);
207  REQUIRE(visp_m == visp_m2);
208  std::cout << std::endl;
209 }
210 
211 TEST_CASE("vpHomogeneousMatrix <--> Eigen::Matrix4d conversion", "[eigen_conversion]") {
212  vpHomogeneousMatrix visp_cMo(0.1, 0.2, 0.3, 0.1, 0.2, 0.3);
213  Eigen::Matrix4d eigen_cMo;
214  vp::visp2eigen(visp_cMo, eigen_cMo);
215  std::cout << "Eigen Matrix4d cMo:\n" << eigen_cMo << std::endl;
216 
217  vpHomogeneousMatrix visp_cMo2;
218  vp::eigen2visp(eigen_cMo, visp_cMo2);
219  std::cout << "ViSP vpHomogeneousMatrix cMo:\n" << visp_cMo2 << std::endl;
220  REQUIRE(visp_cMo == visp_cMo2);
221  std::cout << std::endl;
222 }
223 
224 TEST_CASE("vpHomogeneousMatrix <--> Eigen::Matrix4f + double casting conversion", "[eigen_conversion]") {
225  vpHomogeneousMatrix visp_cMo; //identity for float to double casting
226  Eigen::Matrix4d eigen_cMo_tmp;
227  vp::visp2eigen(visp_cMo, eigen_cMo_tmp);
228  Eigen::Matrix4f eigen_cMo = eigen_cMo_tmp.cast<float>();
229  std::cout << "Eigen Matrix4f cMo:\n" << eigen_cMo << std::endl;
230 
231  vpHomogeneousMatrix visp_cMo2;
232  vp::eigen2visp(eigen_cMo.cast<double>(), visp_cMo2);
233  std::cout << "ViSP vpHomogeneousMatrix cMo:\n" << visp_cMo2 << std::endl;
234  REQUIRE(visp_cMo == visp_cMo2);
235  std::cout << std::endl;
236 }
237 
238 TEST_CASE("vpQuaternionVector <--> Eigen::Quaternionf conversion", "[eigen_conversion]") {
239  vpQuaternionVector visp_quaternion(0, 1, 2, 3);
240  Eigen::Quaternionf eigen_quaternion;
241  vp::visp2eigen(visp_quaternion, eigen_quaternion);
242  std::cout << "Eigen quaternion: " << eigen_quaternion << std::endl;
243 
244  vpQuaternionVector visp_quaternion2;
245  vp::eigen2visp(eigen_quaternion, visp_quaternion2);
246  std::cout << "ViSP quaternion: " << visp_quaternion2.t() << std::endl;
247  REQUIRE(visp_quaternion == visp_quaternion2);
248  std::cout << std::endl;
249 }
250 
251 TEST_CASE("vpThetaUVector <--> Eigen::AngleAxisf conversion", "[eigen_conversion]") {
252  vpThetaUVector visp_thetau(0, 1, 2);
253  Eigen::AngleAxisf eigen_angle_axis;
254  vp::visp2eigen(visp_thetau, eigen_angle_axis);
255  std::cout << "Eigen AngleAxis: " << eigen_angle_axis << std::endl;
256 
257  vpThetaUVector visp_thetau2;
258  vp::eigen2visp(eigen_angle_axis, visp_thetau2);
259  std::cout << "ViSP AngleAxis: " << visp_thetau2.t() << std::endl;
260  REQUIRE(visp_thetau == visp_thetau2);
261  std::cout << std::endl;
262 }
263 
264 TEST_CASE("vpColVector <--> Eigen::VectorXd conversion", "[eigen_conversion]") {
265  vpColVector visp_col(4, 4);
266  visp_col = 10;
267  Eigen::VectorXd eigen_col;
268  vp::visp2eigen(visp_col, eigen_col);
269  std::cout << "Eigen VectorXd: " << eigen_col.transpose() << std::endl;
270 
271  vpColVector visp_col2;
272  vp::eigen2visp(eigen_col, visp_col2);
273  std::cout << "ViSP vpColVector: " << visp_col2.t() << std::endl;
274  REQUIRE(visp_col == visp_col2);
275  std::cout << std::endl;
276 }
277 
278 TEST_CASE("vpRowVector <--> Eigen::RowVectorXd conversion", "[eigen_conversion]") {
279  vpRowVector visp_row(4, 10);
280  visp_row = 10;
281  Eigen::RowVectorXd eigen_row;
282  vp::visp2eigen(visp_row, eigen_row);
283  std::cout << "Eigen RowVectorXd: " << eigen_row << std::endl;
284 
285  vpRowVector visp_row2;
286  vp::eigen2visp(eigen_row, visp_row2);
287  std::cout << "ViSP vpRowVector: " << visp_row2 << std::endl;
288  REQUIRE(visp_row == visp_row2);
289  std::cout << std::endl;
290 }
291 
292 TEST_CASE("Eigen::RowVector4d <--> vpRowVector conversion", "[eigen_conversion]") {
293  Eigen::RowVector4d eigen_row;
294  eigen_row << 9, 8, 7, 6;
295  vpRowVector visp_row;
296  vp::eigen2visp(eigen_row, visp_row);
297  std::cout << "ViSP vpRowVector: " << visp_row << std::endl;
298 
299  Eigen::RowVector4d eigen_row2;
300  vp::visp2eigen(visp_row, eigen_row2);
301  std::cout << "Eigen RowVector4d: " << eigen_row2 << std::endl;
302 
303  vpRowVector visp_row2;
304  vp::eigen2visp(eigen_row2, visp_row2);
305  REQUIRE(visp_row == visp_row2);
306  std::cout << std::endl;
307 }
308 
309 TEST_CASE("vpRowVector <--> Eigen::RowVector4d conversion", "[eigen_conversion]") {
310  vpRowVector visp_row(4, 10);
311  visp_row = 10;
312  Eigen::RowVector4d eigen_row;
313  vp::visp2eigen(visp_row, eigen_row);
314  std::cout << "Eigen RowVector4d: " << eigen_row << std::endl;
315 
316  vpRowVector visp_row2;
317  vp::eigen2visp(eigen_row, visp_row2);
318  std::cout << "ViSP vpRowVector: " << visp_row2 << std::endl;
319  REQUIRE(visp_row == visp_row2);
320  std::cout << std::endl;
321 }
322 
323 int main(int argc, char *argv[])
324 {
325  Catch::Session session; // There must be exactly one instance
326 
327  // Let Catch (using Clara) parse the command line
328  session.applyCommandLine(argc, argv);
329 
330  int numFailed = session.run();
331 
332  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
333  // This clamping has already been applied, so just return it here
334  // You can also do any post run clean-up here
335  return numFailed;
336 }
337 #else
338 int main()
339 {
340  return 0;
341 }
342 #endif
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpRowVector t() const
VISP_EXPORT void eigen2visp(const Eigen::MatrixXd &src, vpMatrix &dst)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
vpRowVector t() const
Implementation of a rotation vector as quaternion angle minimal representation.
void visp2eigen(const vpMatrix &src, Eigen::MatrixBase< Derived > &dst)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a rotation vector as axis-angle minimal representation.