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