Visual Servoing Platform  version 3.4.1 under development (2021-10-17)
testMatrixInitialization.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 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 #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  {
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  {
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  {
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  {
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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Implementation of a generic rotation vector.
vpRowVector t() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:115
vpRowVector t() const
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:293
error that can be emited by ViSP classes.
Definition: vpException.h:71
unsigned int getRows() const
Definition: vpArray2D.h:289
vpRowVector t() const
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:185
Implementation of a rotation matrix and operations on such kind of matrices.
unsigned int getCols() const
Definition: vpArray2D.h:279
Implementation of a rotation vector as quaternion angle minimal representation.
const char * what() const
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyzVector.h:182
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:271
void reshape(unsigned int nrows, unsigned int ncols)
Definition: vpArray2D.h:379
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.