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 Matrix initialization.
33 *
34 *****************************************************************************/
35
36 /*!
37 \example testMatrixInitialization.cpp
38
39 Test Matrix initialization
40 */
41
42 #include <visp3/core/vpMatrix.h>
43
equal(const vpArray2D<double> & a1,const vpArray2D<double> & a2,double epsilon)44 bool equal(const vpArray2D<double> &a1, const vpArray2D<double> &a2, double epsilon)
45 {
46 if (a1.size() != a2.size()) {
47 std::cout << "Rotation vector size differ" << std::endl;
48 return false;
49 }
50 for (unsigned int i=0; i < a1.getRows(); i++) {
51 for (unsigned int j=0; j < a1.getCols(); j++) {
52 if (! vpMath::equal(a1[i][j], a2[i][j], epsilon)) {
53 std::cout << "Array content differ" << std::endl;
54 return false;
55 }
56 }
57 }
58 return true;
59 }
60
equal(const vpRotationVector & a1,const vpRotationVector & a2,double epsilon)61 bool equal(const vpRotationVector &a1, const vpRotationVector &a2, double epsilon)
62 {
63 if (a1.size() != a2.size()) {
64 std::cout << "Rotation vector size differ" << std::endl;
65 return false;
66 }
67 for (unsigned int i=0; i < a1.size(); i++) {
68 if (! vpMath::equal(a1[i], a2[i], epsilon)) {
69 std::cout << "Rotation vector content differ" << std::endl;
70 return false;
71 }
72 }
73 return true;
74 }
75
equal(const vpColVector & a1,const vpColVector & a2,double epsilon)76 bool equal(const vpColVector &a1, const vpColVector &a2, double epsilon)
77 {
78 if (a1.size() != a2.size()) {
79 std::cout << "Column vector size differ" << std::endl;
80 return false;
81 }
82 for (unsigned int i=0; i < a1.size(); i++) {
83 if (! vpMath::equal(a1[i], a2[i], epsilon)) {
84 std::cout << "Column vector content differ" << std::endl;
85 return false;
86 }
87 }
88 return true;
89 }
90
equal(const vpRowVector & a1,const vpRowVector & a2,double epsilon)91 bool equal(const vpRowVector &a1, const vpRowVector &a2, double epsilon)
92 {
93 if (a1.size() != a2.size()) {
94 std::cout << "Row vector size differ" << std::endl;
95 return false;
96 }
97 for (unsigned int i=0; i < a1.size(); i++) {
98 if (! vpMath::equal(a1[i], a2[i], epsilon)) {
99 std::cout << "Row vector content differ" << std::endl;
100 return false;
101 }
102 }
103 return true;
104 }
105
main()106 int main()
107 {
108 double epsilon = 1e-10;
109 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
110 {
111 vpArray2D<float> a{ 1.f, 2.f, 3.f };
112 std::cout << "a:\n" << a << std::endl;
113 a = { -1, -2, -3, 4, 5.5, 6.0f };
114 std::cout << "a:\n" << a << std::endl;
115 a.reshape(2, 3);
116 std::cout << "a.reshape(2,3):\n" << a << std::endl;
117 a.reshape(3, 2);
118 std::cout << "a.reshape(3,2):\n" << a << std::endl;
119
120 vpArray2D<float> a2;
121 a2.resize(2,2);
122 a2 = {1, 2, 3, 4};
123 std::cout << "a2:\n" << a2 << std::endl;
124
125 vpArray2D<double> a3(2, 3, { 1, 2, 3, 4, 5, 6 });
126 std::cout << "a3:\n" << a3 << std::endl;
127
128 vpArray2D<int> a4{ {1,2,3},{4,5,6},{7,8,9} };
129 std::cout << "a4:\n" << a4 << std::endl;
130
131 vpArray2D<int> a5;
132 a5 = { {1,2,3},{4,5,6},{7,8,9} };
133 std::cout << "a5:\n" << a5 << std::endl;
134
135 vpArray2D<int> a6{a5};
136 std::cout << "a6:\n" << a6 << std::endl;
137
138 vpMatrix m{1, 2, 3};
139 std::cout << "m:\n" << m << std::endl;
140 m = { -1, -2, -3, -4 };
141 std::cout << "m:\n" << m << std::endl;
142 m.reshape(2, 2);
143 std::cout << "m:\n" << m << std::endl;
144
145 vpMatrix m2(3, 2, { 1, 2, 3, 4, 5, 6 });
146 std::cout << "m2:\n" << m2 << std::endl;
147
148 vpMatrix m3{ {1,2,3},{4,5,6},{7,8,9} };
149 std::cout << "m3:\n" << m3 << std::endl;
150
151 vpMatrix m4;
152 m4 = { {1,2,3},{4,5,6},{7,8,9} };
153 std::cout << "m4:\n" << m4 << std::endl;
154
155 vpMatrix m5{m4};
156 std::cout << "m5:\n" << m5 << std::endl;
157
158 // vpMatrix m6;
159 // m6 = {m2}; // Fails on travis
160 // std::cout << "m6:\n" << m6 << std::endl;
161 }
162 #endif
163
164 {
165 vpMatrix m1;
166 m1 << 1, 2, 3;
167 std::cout << "m1:\n" << m1 << std::endl;
168
169 m1 << -1, -2, -3, -4;
170 m1.reshape(1, 4);
171 std::cout << "m1:\n" << m1 << std::endl;
172
173 vpMatrix m2(2,2);
174 m2 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
175 std::cout << "m2:\n" << m2 << std::endl;
176
177 m2.resize(3, 3, false);
178 std::cout << "m2:\n" << m2 << std::endl;
179
180 m2 << 0.0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11;
181 m2.reshape(2, 6);
182 std::cout << "m2:\n" << m2 << std::endl;
183 }
184
185 {
186 std::cout << "** Test vpColVector" << std::endl;
187 vpColVector c_ref(6);
188 for (unsigned int i=0; i < 6; i ++) {
189 c_ref[i] = i;
190 }
191 std::cout << "c_ref: " << c_ref.t() << std::endl;
192 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
193 {
194 vpColVector c{ 0, 1, 2, 3, 4, 5 };
195 std::cout << "c: " << c.t() << std::endl;
196 if (! equal(c_ref, c, epsilon)) {
197 return EXIT_FAILURE;
198 }
199 c_ref.resize(3, false);
200 c_ref *= -1;
201 std::cout << "c_ref: " << c_ref.t() << std::endl;
202 c = { 0, -1, -2 };
203 std::cout << "c: " << c.t() << std::endl;
204 if (! equal(c_ref, c, epsilon)) {
205 return EXIT_FAILURE;
206 }
207
208 // Test move constructor
209 vpColVector c1(c_ref);
210 std::cout << "c1: " << c1.t() << std::endl;
211 if (! equal(c_ref, c1, epsilon)) {
212 return EXIT_FAILURE;
213 }
214 vpColVector c2 = std::move(c1); // Move c1 into c2; c1 is now "empty"
215 std::cout << "c1: " << c1.t() << std::endl;
216 if (c1.size()) {
217 return EXIT_FAILURE;
218 }
219 std::cout << "c2: " << c2.t() << std::endl;
220 if (! equal(c_ref, c2, epsilon)) {
221 return EXIT_FAILURE;
222 }
223 }
224 #endif
225 {
226 vpColVector c;
227 c << 1, 2, 3, 4;
228 std::cout << "c: " << c << std::endl;
229
230 try {
231 c.reshape(2, 2);
232 std::cout << "after c.reshape(2, 2): " << c.t() << std::endl;
233 c = c.reshape(2, 2);
234 std::cout << "c:" << c << std::endl;
235 } catch (const vpException &e) {
236 std::cerr << "Exception expected: c = c.reshape(2, 2);\n" << e.what() << std::endl;
237 }
238
239 std::cout << "c: " << c.t() << std::endl;
240 vpArray2D<double> * ptr_array = &c;
241 ptr_array->reshape(2,2);
242 std::cout << "ptr_array->reshape(2,2)" << std::endl;
243 std::cout << "c: (" << c.getRows() << ", " << c.getCols() << "):\n" << c << std::endl;
244 std::cout << "dynamic_cast<vpColVector *>(ptr_array):\n" << *dynamic_cast<vpColVector *>(ptr_array) << std::endl;
245 std::cout << "ptr_array:\n" << *ptr_array << std::endl;
246 }
247 }
248
249 {
250 std::cout << "** Test vpRowVector" << std::endl;
251 vpRowVector r_ref(6);
252 for (unsigned int i=0; i < 6; i ++) {
253 r_ref[i] = i;
254 }
255 std::cout << "r_ref: " << r_ref << std::endl;
256 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
257 {
258 vpRowVector r{ 0, 1, 2, 3, 4, 5 };
259 std::cout << "r: " << r << std::endl;
260 if (! equal(r_ref, r, epsilon)) {
261 return EXIT_FAILURE;
262 }
263 r_ref.resize(3, false);
264 r_ref *= -1;
265 std::cout << "r_ref: " << r_ref << std::endl;
266 r = { 0, -1, -2 };
267 std::cout << "r: " << r << std::endl;
268 if (! equal(r_ref, r, epsilon)) {
269 return EXIT_FAILURE;
270 }
271
272 // Test move constructor
273 vpRowVector r1(r_ref);
274 std::cout << "r1: " << r1 << std::endl;
275 if (! equal(r_ref, r1, epsilon)) {
276 return EXIT_FAILURE;
277 }
278 vpRowVector r2 = std::move(r1); // Move r1 into r2; r1 is now "empty"
279 std::cout << "r1: " << r1 << std::endl;
280 if (r1.size()) {
281 return EXIT_FAILURE;
282 }
283 std::cout << "r2: " << r2 << std::endl;
284 if (! equal(r_ref, r2, epsilon)) {
285 return EXIT_FAILURE;
286 }
287 }
288 #endif
289 {
290 vpRowVector r;
291 r << 1, 2, 3;
292 std::cout << "r: " << r << std::endl;
293
294 vpMatrix m = r.reshape(3, 1);
295 std::cout << "m:\n" << m << std::endl;
296
297 try {
298 r.reshape(3, 1);
299 std::cout << "after r.reshape(3, 1): " << r << std::endl;
300 } catch (const vpException &e) {
301 std::cerr << "Exception: r.reshape(3, 1);\n" << e.what() << std::endl;
302 }
303 }
304 }
305
306 {
307 std::cout << "** Test vpThetaUVector" << std::endl;
308 vpThetaUVector tu_ref(0, M_PI_2, M_PI);
309 std::cout << "tu_ref: " << tu_ref.t() << std::endl;
310 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
311 {
312 vpThetaUVector tu = {0, M_PI_2, M_PI};
313 std::cout << "tu: " << tu.t() << std::endl;
314 if (! equal(tu_ref, tu, epsilon)) {
315 return EXIT_FAILURE;
316 }
317 }
318 #endif
319 {
320 vpThetaUVector tu;
321 tu << 0, M_PI_2, M_PI;
322 std::cout << "tu: " << tu.t() << std::endl;
323 if (! equal(tu_ref, tu, epsilon)) {
324 return EXIT_FAILURE;
325 }
326 // Do it twice
327 tu << 0, M_PI_2, M_PI;
328 std::cout << "tu: " << tu.t() << std::endl;
329 if (! equal(tu_ref, tu, epsilon)) {
330 return EXIT_FAILURE;
331 }
332 }
333 }
334
335 {
336 std::cout << "** Test vpRxyzVector" << std::endl;
337 vpRxyzVector rxyz_ref(0, M_PI_2, M_PI);
338 std::cout << "rxyz_ref: " << rxyz_ref.t() << std::endl;
339 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
340 {
341 vpRxyzVector rxyz = {0, M_PI_2, M_PI};
342 std::cout << "rxyz: " << rxyz.t() << std::endl;
343 if (! equal(rxyz_ref, rxyz, epsilon)) {
344 return EXIT_FAILURE;
345 }
346 }
347 #endif
348 {
349 vpRxyzVector rxyz;
350 rxyz << 0, M_PI_2, M_PI;
351 std::cout << "rxyz: " << rxyz.t() << std::endl;
352 if (! equal(rxyz_ref, rxyz, epsilon)) {
353 return EXIT_FAILURE;
354 }
355 // Do it twice
356 rxyz << 0, M_PI_2, M_PI;
357 std::cout << "rxyz: " << rxyz.t() << std::endl;
358 if (! equal(rxyz_ref, rxyz, epsilon)) {
359 return EXIT_FAILURE;
360 }
361 }
362 }
363
364 {
365 std::cout << "** Test vpRzyxVector" << std::endl;
366 vpRzyxVector rzyx_ref(0, M_PI_2, M_PI);
367 std::cout << "rzyx_ref: " << rzyx_ref.t() << std::endl;
368 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
369 {
370 vpRzyxVector rzyx = {0, M_PI_2, M_PI};
371 std::cout << "rzyx: " << rzyx.t() << std::endl;
372 if (! equal(rzyx_ref, rzyx, epsilon)) {
373 return EXIT_FAILURE;
374 }
375 }
376 #endif
377 {
378 vpRzyxVector rzyx;
379 rzyx << 0, M_PI_2, M_PI;
380 std::cout << "rzyx: " << rzyx.t() << std::endl;
381 if (! equal(rzyx_ref, rzyx, epsilon)) {
382 return EXIT_FAILURE;
383 }
384 // Do it twice
385 rzyx << 0, M_PI_2, M_PI;
386 std::cout << "rzyx: " << rzyx.t() << std::endl;
387 if (! equal(rzyx_ref, rzyx, epsilon)) {
388 return EXIT_FAILURE;
389 }
390 }
391 }
392
393 {
394 std::cout << "** Test vpRzyzVector" << std::endl;
395 vpRzyzVector rzyz_ref(0, M_PI_2, M_PI);
396 std::cout << "rzyz_ref: " << rzyz_ref.t() << std::endl;
397 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
398 {
399 vpRzyzVector rzyz = {0, M_PI_2, M_PI};
400 std::cout << "rzyz: " << rzyz.t() << std::endl;
401 if (! equal(rzyz_ref, rzyz, epsilon)) {
402 return EXIT_FAILURE;
403 }
404 }
405 #endif
406 {
407 vpRzyzVector rzyz;
408 rzyz << 0, M_PI_2, M_PI;
409 std::cout << "rzyz: " << rzyz.t() << std::endl;
410 if (! equal(rzyz_ref, rzyz, epsilon)) {
411 return EXIT_FAILURE;
412 }
413 // Do it twice
414 rzyz << 0, M_PI_2, M_PI;
415 std::cout << "rzyz: " << rzyz.t() << std::endl;
416 if (! equal(rzyz_ref, rzyz, epsilon)) {
417 return EXIT_FAILURE;
418 }
419 }
420 }
421
422 {
423 std::cout << "** Test vpQuaternionVector" << std::endl;
424 vpThetaUVector tu_ref(0, M_PI_2, M_PI);
425 vpQuaternionVector q_ref(tu_ref);
426 std::cout << "q_ref: " << q_ref.t() << std::endl;
427 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
428 {
429 vpQuaternionVector q = {q_ref[0], q_ref[1], q_ref[2], q_ref[3]};
430 std::cout << "q: " << q.t() << std::endl;
431 if (! equal(q_ref, q, epsilon)) {
432 return EXIT_FAILURE;
433 }
434 }
435 #endif
436 {
437 vpQuaternionVector q;
438 q << q_ref[0], q_ref[1], q_ref[2], q_ref[3];
439 std::cout << "q: " << q.t() << std::endl;
440 if (! equal(q_ref, q, epsilon)) {
441 return EXIT_FAILURE;
442 }
443 // Do it twice
444 q << q_ref[0], q_ref[1], q_ref[2], q_ref[3];
445 std::cout << "q: " << q.t() << std::endl;
446 if (! equal(q_ref, q, epsilon)) {
447 return EXIT_FAILURE;
448 }
449 }
450 }
451
452 {
453 std::cout << "** Test vpTranslationVector" << std::endl;
454 vpTranslationVector t_ref(0, 0.1, 0.5);
455 std::cout << "t_ref: " << t_ref.t() << std::endl;
456 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
457 {
458 vpTranslationVector t = {t_ref[0], t_ref[1], t_ref[2]};
459 std::cout << "t: " << t.t() << std::endl;
460 if (! equal(t_ref, t, epsilon)) {
461 return EXIT_FAILURE;
462 }
463 }
464 #endif
465 {
466 vpTranslationVector t;
467 t << 0, 0.1, 0.5;
468 std::cout << "t: " << t.t() << std::endl;
469 if (! equal(t_ref, t, epsilon)) {
470 return EXIT_FAILURE;
471 }
472 // Do it twice
473 t << 0, 0.1, 0.5;
474 std::cout << "t: " << t.t() << std::endl;
475 if (! equal(t_ref, t, epsilon)) {
476 return EXIT_FAILURE;
477 }
478 }
479 }
480
481 {
482 std::cout << "** Test vpRotationMatrix" << std::endl;
483 vpRotationMatrix R_ref(vpRxyzVector(0, -M_PI_2, M_PI));
484 std::cout << "R_ref:\n" << R_ref << std::endl;
485 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
486 {
487 vpRotationMatrix R({0, 0, -1, 0, -1, 0, -1, 0, 0});
488 std::cout << "R:\n" << R << std::endl;
489 if (! equal(R_ref, R, epsilon)) {
490 return EXIT_FAILURE;
491 }
492 }
493 {
494 vpRotationMatrix R;
495 R = {0, 0, -1, 0, -1, 0, -1, 0, 0};
496 std::cout << "R:\n" << R << std::endl;
497 if (! equal(R_ref, R, epsilon)) {
498 return EXIT_FAILURE;
499 }
500 }
501 #endif
502 {
503 vpRotationMatrix R;
504 R << 0, 0, -1, 0, -1, 0, -1, 0, 0;
505 std::cout << "R:\n" << R << std::endl;
506 if (! equal(R_ref, R, epsilon)) {
507 return EXIT_FAILURE;
508 }
509 // Do it twice
510 R << 0, 0, -1, 0, -1, 0, -1, 0, 0;
511 std::cout << "R:\n" << R << std::endl;
512 if (! equal(R_ref, R, epsilon)) {
513 return EXIT_FAILURE;
514 }
515 }
516 }
517
518 std::cout << "Test succeed" << std::endl;
519 return EXIT_SUCCESS;
520 }
521