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