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