Visual Servoing Platform version 3.5.0
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
49namespace
50{
51template<typename Type>
52std::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
57template<typename Type>
58std::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
67TEST_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
99TEST_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
127TEST_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
155TEST_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
183TEST_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
211TEST_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
224TEST_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
238TEST_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
251TEST_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
264TEST_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
278TEST_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
292TEST_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
309TEST_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
323int 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
338int main()
339{
340 return 0;
341}
342#endif
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
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:154
Implementation of a rotation vector as quaternion angle minimal representation.
vpRowVector t() const
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:116
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)