Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testEigenConversion.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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> std::ostream &operator<<(std::ostream &os, const Eigen::Quaternion<Type> &q)
52 {
53  return os << "qw: " << q.w() << " ; qx: " << q.x() << " ; qy: " << q.y() << " ; qz: " << q.z();
54 }
55 
56 template <typename Type> std::ostream &operator<<(std::ostream &os, const Eigen::AngleAxis<Type> &aa)
57 {
58  return os << "angle: " << aa.angle() << " ; axis: " << aa.axis()(0) << " ; " << aa.axis()(1) << " ; " << aa.axis()(2)
59  << " ; thetau: " << aa.angle() * aa.axis()(0) << " ; " << aa.angle() * aa.axis()(1) << " ; "
60  << aa.angle() * aa.axis()(2);
61 }
62 } // namespace
63 
64 TEST_CASE("vpMatrix <--> Eigen::MatrixXd/Matrix3Xd conversion", "[eigen_conversion]")
65 {
66  vpMatrix visp_m(3, 4);
67  for (unsigned int i = 0; i < visp_m.size(); i++) {
68  visp_m.data[i] = i;
69  }
70 
71  {
72  Eigen::MatrixXd eigen_m;
73  vp::visp2eigen(visp_m, eigen_m);
74  std::cout << "Eigen MatrixXd:\n" << eigen_m << std::endl;
75 
76  vpMatrix visp_m2;
77  vp::eigen2visp(eigen_m, visp_m2);
78  std::cout << "ViSP vpMatrix:\n" << visp_m2 << std::endl;
79 
80  REQUIRE(visp_m == visp_m2);
81  std::cout << std::endl;
82  }
83  {
84  Eigen::Matrix3Xd eigen_m;
85  vp::visp2eigen(visp_m, eigen_m);
86  std::cout << "Eigen Matrix3Xd:\n" << eigen_m << std::endl;
87 
88  vpMatrix visp_m2;
89  vp::eigen2visp(eigen_m, visp_m2);
90  std::cout << "ViSP vpMatrix:\n" << visp_m2 << std::endl;
91 
92  REQUIRE(visp_m == visp_m2);
93  std::cout << std::endl;
94  }
95 }
96 
97 TEST_CASE("Eigen::MatrixXd <--> vpMatrix conversion", "[eigen_conversion]")
98 {
99  Eigen::MatrixXd eigen_m(3, 5);
100 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
101  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
102  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
103 #else
104  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
105  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
106 #endif
107  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
108  }
109  }
110  std::cout << "Eigen Matrix (row major: " << eigen_m.IsRowMajor << "):\n" << eigen_m << std::endl;
111 
112  vpMatrix visp_m;
113  vp::eigen2visp(eigen_m, visp_m);
114  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
115 
116  Eigen::MatrixXd eigen_m2;
117  vp::visp2eigen(visp_m, eigen_m2);
118  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
119 
120  vpMatrix visp_m2;
121  vp::eigen2visp(eigen_m2, visp_m2);
122  REQUIRE(visp_m == visp_m2);
123  std::cout << std::endl;
124 }
125 
126 TEST_CASE("Eigen::MatrixX4d <--> vpMatrix conversion", "[eigen_conversion]")
127 {
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 {
157  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> eigen_m(3, 5);
158 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
159  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
160  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
161 #else
162  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
163  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
164 #endif
165  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
166  }
167  }
168  std::cout << "Eigen Matrix (RowMajor):\n" << eigen_m << std::endl;
169 
170  vpMatrix visp_m;
171  vp::eigen2visp(eigen_m, visp_m);
172  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
173 
174  Eigen::MatrixXd eigen_m2;
175  vp::visp2eigen(visp_m, eigen_m2);
176  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
177 
178  vpMatrix visp_m2;
179  vp::eigen2visp(eigen_m2, visp_m2);
180  REQUIRE(visp_m == visp_m2);
181  std::cout << std::endl;
182 }
183 
184 TEST_CASE("Eigen::Matrix<double, Dynamic, Dynamic, ColMajor> <--> vpMatrix conversion", "[eigen_conversion]")
185 {
186  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> eigen_m(3, 5);
187 #if (VP_VERSION_INT(EIGEN_WORLD_VERSION, EIGEN_MAJOR_VERSION, EIGEN_MINOR_VERSION) < 0x030300)
188  for (Eigen::DenseIndex i = 0; i < eigen_m.rows(); i++) {
189  for (Eigen::DenseIndex j = 0; j < eigen_m.cols(); j++) {
190 #else
191  for (Eigen::Index i = 0; i < eigen_m.rows(); i++) {
192  for (Eigen::Index j = 0; j < eigen_m.cols(); j++) {
193 #endif
194  eigen_m(i, j) = static_cast<double>(i * eigen_m.cols() + j);
195  }
196  }
197  std::cout << "Eigen Matrix (ColMajor):\n" << eigen_m << std::endl;
198 
199  vpMatrix visp_m;
200  vp::eigen2visp(eigen_m, visp_m);
201  std::cout << "ViSP vpMatrix:\n" << visp_m << std::endl;
202 
203  Eigen::MatrixXd eigen_m2;
204  vp::visp2eigen(visp_m, eigen_m2);
205  std::cout << "Eigen MatrixXd (row major: " << eigen_m2.IsRowMajor << "):\n" << eigen_m2 << std::endl;
206 
207  vpMatrix visp_m2;
208  vp::eigen2visp(eigen_m2, visp_m2);
209  REQUIRE(visp_m == visp_m2);
210  std::cout << std::endl;
211 }
212 
213 TEST_CASE("vpHomogeneousMatrix <--> Eigen::Matrix4d conversion", "[eigen_conversion]")
214 {
215  vpHomogeneousMatrix visp_cMo(0.1, 0.2, 0.3, 0.1, 0.2, 0.3);
216  Eigen::Matrix4d eigen_cMo;
217  vp::visp2eigen(visp_cMo, eigen_cMo);
218  std::cout << "Eigen Matrix4d cMo:\n" << eigen_cMo << std::endl;
219 
220  vpHomogeneousMatrix visp_cMo2;
221  vp::eigen2visp(eigen_cMo, visp_cMo2);
222  std::cout << "ViSP vpHomogeneousMatrix cMo:\n" << visp_cMo2 << std::endl;
223  REQUIRE(visp_cMo == visp_cMo2);
224  std::cout << std::endl;
225 }
226 
227 TEST_CASE("vpHomogeneousMatrix <--> Eigen::Matrix4f + double casting conversion", "[eigen_conversion]")
228 {
229  vpHomogeneousMatrix visp_cMo; // identity for float to double casting
230  Eigen::Matrix4d eigen_cMo_tmp;
231  vp::visp2eigen(visp_cMo, eigen_cMo_tmp);
232  Eigen::Matrix4f eigen_cMo = eigen_cMo_tmp.cast<float>();
233  std::cout << "Eigen Matrix4f cMo:\n" << eigen_cMo << std::endl;
234 
235  vpHomogeneousMatrix visp_cMo2;
236  vp::eigen2visp(eigen_cMo.cast<double>(), visp_cMo2);
237  std::cout << "ViSP vpHomogeneousMatrix cMo:\n" << visp_cMo2 << std::endl;
238  REQUIRE(visp_cMo == visp_cMo2);
239  std::cout << std::endl;
240 }
241 
242 TEST_CASE("vpQuaternionVector <--> Eigen::Quaternionf conversion", "[eigen_conversion]")
243 {
244  vpQuaternionVector visp_quaternion(0, 1, 2, 3);
245  Eigen::Quaternionf eigen_quaternion;
246  vp::visp2eigen(visp_quaternion, eigen_quaternion);
247  std::cout << "Eigen quaternion: " << eigen_quaternion << std::endl;
248 
249  vpQuaternionVector visp_quaternion2;
250  vp::eigen2visp(eigen_quaternion, visp_quaternion2);
251  std::cout << "ViSP quaternion: " << visp_quaternion2.t() << std::endl;
252  REQUIRE(visp_quaternion == visp_quaternion2);
253  std::cout << std::endl;
254 }
255 
256 TEST_CASE("vpThetaUVector <--> Eigen::AngleAxisf conversion", "[eigen_conversion]")
257 {
258  vpThetaUVector visp_thetau(0, 1, 2);
259  Eigen::AngleAxisf eigen_angle_axis;
260  vp::visp2eigen(visp_thetau, eigen_angle_axis);
261  std::cout << "Eigen AngleAxis: " << eigen_angle_axis << std::endl;
262 
263  vpThetaUVector visp_thetau2;
264  vp::eigen2visp(eigen_angle_axis, visp_thetau2);
265  std::cout << "ViSP AngleAxis: " << visp_thetau2.t() << std::endl;
266  REQUIRE(visp_thetau == visp_thetau2);
267  std::cout << std::endl;
268 }
269 
270 TEST_CASE("vpColVector <--> Eigen::VectorXd conversion", "[eigen_conversion]")
271 {
272  vpColVector visp_col(4, 4);
273  visp_col = 10;
274  Eigen::VectorXd eigen_col;
275  vp::visp2eigen(visp_col, eigen_col);
276  std::cout << "Eigen VectorXd: " << eigen_col.transpose() << std::endl;
277 
278  vpColVector visp_col2;
279  vp::eigen2visp(eigen_col, visp_col2);
280  std::cout << "ViSP vpColVector: " << visp_col2.t() << std::endl;
281  REQUIRE(visp_col == visp_col2);
282  std::cout << std::endl;
283 }
284 
285 TEST_CASE("vpRowVector <--> Eigen::RowVectorXd conversion", "[eigen_conversion]")
286 {
287  vpRowVector visp_row(4, 10);
288  visp_row = 10;
289  Eigen::RowVectorXd eigen_row;
290  vp::visp2eigen(visp_row, eigen_row);
291  std::cout << "Eigen RowVectorXd: " << eigen_row << std::endl;
292 
293  vpRowVector visp_row2;
294  vp::eigen2visp(eigen_row, visp_row2);
295  std::cout << "ViSP vpRowVector: " << visp_row2 << std::endl;
296  REQUIRE(visp_row == visp_row2);
297  std::cout << std::endl;
298 }
299 
300 TEST_CASE("Eigen::RowVector4d <--> vpRowVector conversion", "[eigen_conversion]")
301 {
302  Eigen::RowVector4d eigen_row;
303  eigen_row << 9, 8, 7, 6;
304  vpRowVector visp_row;
305  vp::eigen2visp(eigen_row, visp_row);
306  std::cout << "ViSP vpRowVector: " << visp_row << std::endl;
307 
308  Eigen::RowVector4d eigen_row2;
309  vp::visp2eigen(visp_row, eigen_row2);
310  std::cout << "Eigen RowVector4d: " << eigen_row2 << std::endl;
311 
312  vpRowVector visp_row2;
313  vp::eigen2visp(eigen_row2, visp_row2);
314  REQUIRE(visp_row == visp_row2);
315  std::cout << std::endl;
316 }
317 
318 TEST_CASE("vpRowVector <--> Eigen::RowVector4d conversion", "[eigen_conversion]")
319 {
320  vpRowVector visp_row(4, 10);
321  visp_row = 10;
322  Eigen::RowVector4d eigen_row;
323  vp::visp2eigen(visp_row, eigen_row);
324  std::cout << "Eigen RowVector4d: " << eigen_row << std::endl;
325 
326  vpRowVector visp_row2;
327  vp::eigen2visp(eigen_row, visp_row2);
328  std::cout << "ViSP vpRowVector: " << visp_row2 << std::endl;
329  REQUIRE(visp_row == visp_row2);
330  std::cout << std::endl;
331 }
332 
333 int main(int argc, char *argv[])
334 {
335  Catch::Session session; // There must be exactly one instance
336 
337  // Let Catch (using Clara) parse the command line
338  session.applyCommandLine(argc, argv);
339 
340  int numFailed = session.run();
341 
342  // numFailed is clamped to 255 as some unices only use the lower 8 bits.
343  // This clamping has already been applied, so just return it here
344  // You can also do any post run clean-up here
345  return numFailed;
346 }
347 #else
348 int main() { return EXIT_SUCCESS; }
349 #endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:523
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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:146
Implementation of a rotation vector as quaternion angle minimal representation.
vpRowVector t() const
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
Implementation of a rotation vector as axis-angle minimal representation.
void visp2eigen(const vpMatrix &src, Eigen::MatrixBase< Derived > &dst)
VISP_EXPORT void eigen2visp(const Eigen::MatrixXd &src, vpMatrix &dst)