Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testMatrix.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 some vpMatrix functionalities.
33  *
34 *****************************************************************************/
35 
42 #include <visp3/core/vpConfig.h>
43 #include <visp3/core/vpDebug.h>
44 #include <visp3/core/vpGEMM.h>
45 #include <visp3/core/vpHomogeneousMatrix.h>
46 #include <visp3/core/vpMath.h>
47 #include <visp3/core/vpVelocityTwistMatrix.h>
48 
49 #include <stdio.h>
50 #include <stdlib.h>
51 
52 #include <iterator> // for std::back_inserter
53 
54 namespace
55 {
56 bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
57 {
58  static unsigned int cpt = 0;
59  std::cout << "** Test " << ++cpt << std::endl;
60  std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
61  if (bench.size() != M.size()) {
62  std::cout << "Test fails: bad size wrt bench" << std::endl;
63  return false;
64  }
65  for (unsigned int i = 0; i < M.size(); i++) {
66  if (std::fabs(M.data[i] - bench[i]) > std::fabs(M.data[i]) * std::numeric_limits<double>::epsilon()) {
67  std::cout << "Test fails: bad content" << std::endl;
68  return false;
69  }
70  }
71 
72  return true;
73 }
74 
75 double getRandomValues(double min, double max) { return (max - min) * ((double)rand() / (double)RAND_MAX) + min; }
76 
77 bool equalMatrix(const vpMatrix &A, const vpMatrix &B, double tol = std::numeric_limits<double>::epsilon())
78 {
79  if (A.getRows() != B.getRows() || A.getCols() != B.getCols()) {
80  return false;
81  }
82 
83  for (unsigned int i = 0; i < A.getRows(); i++) {
84  for (unsigned int j = 0; j < A.getCols(); j++) {
85  if (!vpMath::equal(A[i][j], B[i][j], tol)) {
86  return false;
87  }
88  }
89  }
90 
91  return true;
92 }
93 
94 vpMatrix generateRandomMatrix(unsigned int rows, unsigned int cols, double min, double max)
95 {
96  vpMatrix M(rows, cols);
97 
98  for (unsigned int i = 0; i < M.getRows(); i++) {
99  for (unsigned int j = 0; j < M.getCols(); j++) {
100  M[i][j] = getRandomValues(min, max);
101  }
102  }
103 
104  return M;
105 }
106 
107 std::vector<double> computeHadamard(const std::vector<double> &v1, const std::vector<double> &v2)
108 {
109  std::vector<double> result;
110  std::transform(v1.begin(), v1.end(), v2.begin(), std::back_inserter(result), std::multiplies<double>());
111  return result;
112 }
113 } // namespace
114 
115 int main(int argc, char *argv[])
116 {
117  try {
118  bool ctest = true;
119  for (int i = 1; i < argc; i++) {
120  if (std::string(argv[i]) == "--benchmark") {
121  ctest = false;
122  }
123  }
124 
125  {
126  const double val = 10.0;
127  vpMatrix M, M2(5, 5, val);
128  M.resize(5, 5, false, false);
129  M = val;
130  for (unsigned int i = 0; i < M.getRows(); i++) {
131  for (unsigned int j = 0; j < M.getCols(); j++) {
132  if (!vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
133  std::cerr << "Issue with matrix assignment with value." << std::endl;
134  return EXIT_FAILURE;
135  }
136 
137  if (!vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
138  std::cerr << "Issue with matrix constructor initialized with value." << std::endl;
139  return EXIT_FAILURE;
140  }
141  }
142  }
143  }
144  {
145  // Test vpRotationMatrix construction
146  std::vector<double> bench(9, 0);
147  bench[2] = bench[4] = bench[6] = 1.;
148  vpMatrix M(3, 3);
149  M[2][0] = M[1][1] = M[0][2] = 1.;
150  vpRotationMatrix R1(M);
151  if (test("R1", R1, bench) == false)
152  return EXIT_FAILURE;
153  vpRotationMatrix R2;
154  R2 = M;
155  if (test("R2", R2, bench) == false)
156  return EXIT_FAILURE;
157  }
158  {
159  vpColVector c(6, 1);
160  vpRowVector r(6, 1);
161  std::vector<double> bench(6, 1);
162  vpMatrix M1(c);
163  if (test("M1", M1, bench) == false)
164  return EXIT_FAILURE;
165  vpMatrix M2(r);
166  if (test("M2", M2, bench) == false)
167  return EXIT_FAILURE;
168  }
169  {
170  vpMatrix M(4, 5);
171  int val = 0;
172  for (unsigned int i = 0; i < M.getRows(); i++) {
173  for (unsigned int j = 0; j < M.getCols(); j++) {
174  M[i][j] = val++;
175  }
176  }
177  std::cout << "M ";
178  M.print(std::cout, 4);
179 
180  vpMatrix N;
181  N.init(M, 0, 1, 2, 3);
182  std::cout << "N ";
183  N.print(std::cout, 4);
184  std::string header("My 4-by-5 matrix\nwith a second line");
185 
186  // Save matrix in text format
187  if (vpMatrix::saveMatrix("matrix.mat", M, false, header.c_str()))
188  std::cout << "Matrix saved in matrix.mat file" << std::endl;
189  else
190  return EXIT_FAILURE;
191 
192  // Load matrix in text format
193  vpMatrix M1;
194  char header_[100];
195  if (vpMatrix::loadMatrix("matrix.mat", M1, false, header_))
196  std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
197  else
198  return EXIT_FAILURE;
199  if (header != std::string(header_)) {
200  std::cout << "Bad header in matrix.mat" << std::endl;
201  return EXIT_FAILURE;
202  }
203 
204  // Save matrix in binary format
205  if (vpMatrix::saveMatrix("matrix.bin", M, true, header.c_str()))
206  std::cout << "Matrix saved in matrix.bin file" << std::endl;
207  else
208  return EXIT_FAILURE;
209 
210  // Load matrix in binary format
211  if (vpMatrix::loadMatrix("matrix.bin", M1, true, header_))
212  std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
213  else
214  return EXIT_FAILURE;
215  if (header != std::string(header_)) {
216  std::cout << "Bad header in matrix.bin" << std::endl;
217  return EXIT_FAILURE;
218  }
219 
220  // Save matrix in YAML format
221  if (vpMatrix::saveMatrixYAML("matrix.yml", M, header.c_str()))
222  std::cout << "Matrix saved in matrix.yml file" << std::endl;
223  else
224  return EXIT_FAILURE;
225 
226  // Read matrix in YAML format
227  vpMatrix M2;
228  if (vpMatrix::loadMatrixYAML("matrix.yml", M2, header_))
229  std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
230  else
231  return EXIT_FAILURE;
232  if (header != std::string(header_)) {
233  std::cout << "Bad header in matrix.mat" << std::endl;
234  return EXIT_FAILURE;
235  }
236  }
237 
238  {
240  std::cout << "R: \n" << R << std::endl;
241  vpMatrix M1(R);
242  std::cout << "M1: \n" << M1 << std::endl;
243  vpMatrix M2(M1);
244  std::cout << "M2: \n" << M2 << std::endl;
245  vpMatrix M3 = R;
246  std::cout << "M3: \n" << M3 << std::endl;
247  vpMatrix M4 = M1;
248  std::cout << "M4: \n" << M4 << std::endl;
249  }
250  {
251 
252  std::cout << "------------------------" << std::endl;
253  std::cout << "--- TEST PRETTY PRINT---" << std::endl;
254  std::cout << "------------------------" << std::endl;
255  vpMatrix M;
256  M.eye(4);
257 
258  std::cout << "call std::cout << M;" << std::endl;
259  std::cout << M << std::endl;
260 
261  std::cout << "call M.print (std::cout, 4);" << std::endl;
262  M.print(std::cout, 4);
263 
264  std::cout << "------------------------" << std::endl;
265  M.resize(3, 3);
266  M.eye(3);
267  M[1][0] = 1.235;
268  M[1][1] = 12.345;
269  M[1][2] = .12345;
270  std::cout << "call std::cout << M;" << std::endl;
271  std::cout << M << std::endl;
272  std::cout << "call M.print (std::cout, 6);" << std::endl;
273  M.print(std::cout, 6);
274  std::cout << std::endl;
275 
276  std::cout << "------------------------" << std::endl;
277  M[0][0] = -1.235;
278  M[1][0] = -12.235;
279 
280  std::cout << "call std::cout << M;" << std::endl;
281  std::cout << M << std::endl;
282 
283  std::cout << "call M.print (std::cout, 10);" << std::endl;
284  M.print(std::cout, 10);
285  std::cout << std::endl;
286 
287  std::cout << "call M.print (std::cout, 2);" << std::endl;
288  M.print(std::cout, 2);
289  std::cout << std::endl;
290 
291  std::cout << "------------------------" << std::endl;
292  M.resize(3, 3);
293  M.eye(3);
294  M[0][2] = -0.0000876;
295  std::cout << "call std::cout << M;" << std::endl;
296  std::cout << M << std::endl;
297 
298  std::cout << "call M.print (std::cout, 4);" << std::endl;
299  M.print(std::cout, 4);
300  std::cout << std::endl;
301  std::cout << "call M.print (std::cout, 6, \"M\");" << std::endl;
302  M.print(std::cout, 6, "M");
303  std::cout << std::endl;
304  std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
305  M.print(std::cout, 10, "M");
306  std::cout << std::endl;
307 
308  M.resize(2, 3);
309  M[0][0] = -1;
310  M[0][1] = -2;
311  M[0][2] = -3;
312  M[1][0] = 4;
313  M[1][1] = 5.5;
314  M[1][2] = 6.0f;
315  std::cout << "call std::cout << M;" << std::endl;
316  std::cout << M << std::endl;
317  std::cout << "call M.print (std::cout, 5, \"M\");" << std::endl;
318  M.print(std::cout, 5, "M");
319  std::cout << std::endl;
320 
321  M.resize(2, 3);
322  M[0][0] = -1;
323  M[0][1] = -2;
324  M[0][2] = -3;
325  M[1][0] = 4;
326  M[1][1] = 5.;
327  M[1][2] = 6;
328  std::cout << "call std::cout << M;" << std::endl;
329  std::cout << M << std::endl;
330  std::cout << "call M.print (std::cout, 5, \"M\");" << std::endl;
331  M.print(std::cout, 5, "M");
332  std::cout << std::endl;
333 
334  std::cout << "------------------------" << std::endl;
335  std::cout << "--- TEST RESIZE --------" << std::endl;
336  std::cout << "------------------------" << std::endl;
337  std::cout << "5x5" << std::endl;
338  M.resize(5, 5, false);
339  std::cout << M << std::endl;
340  std::cout << "3x2" << std::endl;
341  M.resize(3, 2, false);
342  std::cout << M << std::endl;
343  std::cout << "2x2" << std::endl;
344  M.resize(2, 2, false);
345  std::cout << M << std::endl;
346  std::cout << "------------------------" << std::endl;
347 
349  vpMatrix A(1, 6), B;
350 
351  A = 1.0;
352  // vMe=1.0;
353  B = A * vMe;
354 
355  std::cout << "------------------------" << std::endl;
356  std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
357  std::cout << "------------------------" << std::endl;
358  vpRowVector r(3);
359  r[0] = 2;
360  r[1] = 3;
361  r[2] = 4;
362 
363  vpColVector c(3);
364  c[0] = 1;
365  c[1] = 2;
366  c[2] = -1;
367 
368  double rc = r * c;
369 
370  r.print(std::cout, 2, "r");
371  c.print(std::cout, 2, "c");
372  std::cout << "r * c = " << rc << std::endl;
373 
374  std::cout << "------------------------" << std::endl;
375  std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
376  std::cout << "------------------------" << std::endl;
377  M.resize(3, 3);
378  M.eye(3);
379 
380  M[1][0] = 1.5;
381  M[2][0] = 2.3;
382 
383  vpRowVector rM = r * M;
384 
385  r.print(std::cout, 2, "r");
386  M.print(std::cout, 10, "M");
387  std::cout << "r * M = " << rM << std::endl;
388 
389  std::cout << "------------------------" << std::endl;
390  std::cout << "--- TEST vpGEMM " << std::endl;
391  std::cout << "------------------------" << std::endl;
392  M.resize(3, 3);
393  M.eye(3);
394  vpMatrix N(3, 3);
395  N[0][0] = 2;
396  N[1][0] = 1.2;
397  N[1][2] = 0.6;
398  N[2][2] = 0.25;
399 
400  vpMatrix C(3, 3);
401  C.eye(3);
402 
403  vpMatrix D;
404 
405  // realise the operation D = 2 * M^T * N + 3 C
406  vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
407  std::cout << D << std::endl;
408  }
409 
410  {
411  std::cout << "------------------------" << std::endl;
412  std::cout << "--- TEST vpMatrix insert() with same colNum " << std::endl;
413  std::cout << "------------------------" << std::endl;
414  unsigned int nb = ctest ? 10 : 100; // 10000;
415  const unsigned int size = ctest ? 10 : 100;
416 
417  vpMatrix m_big(nb *size, 6);
418  std::vector<vpMatrix> submatrices(nb);
419  for (size_t cpt = 0; cpt < submatrices.size(); cpt++) {
420  vpMatrix m(size, 6);
421 
422  for (unsigned int i = 0; i < m.getRows(); i++) {
423  for (unsigned int j = 0; j < m.getCols(); j++) {
424  m[i][j] = getRandomValues(-100.0, 100.0);
425  }
426  }
427 
428  submatrices[cpt] = m;
429  }
430 
431  double t = vpTime::measureTimeMs();
432  for (unsigned int i = 0; i < nb; i++) {
433  m_big.insert(submatrices[(size_t)i], i * size, 0);
434  }
435  t = vpTime::measureTimeMs() - t;
436  std::cout << "Matrix insert(): " << t << " ms" << std::endl;
437 
438  for (unsigned int cpt = 0; cpt < nb; cpt++) {
439  for (unsigned int i = 0; i < size; i++) {
440  for (unsigned int j = 0; j < 6; j++) {
441  if (!vpMath::equal(m_big[cpt * size + i][j], submatrices[(size_t)cpt][i][j],
442  std::numeric_limits<double>::epsilon())) {
443  std::cerr << "Problem with vpMatrix insert()!" << std::endl;
444  return EXIT_FAILURE;
445  }
446  }
447  }
448  }
449 
450  // Try to insert empty matrices
451  vpMatrix m1(2, 3), m2, m3;
452  m1.insert(m2, 0, 0);
453  m3.insert(m2, 0, 0);
454 
455  std::cout << "Insert empty matrices:" << std::endl;
456  std::cout << "m1:\n" << m1 << std::endl;
457  std::cout << "m2:\n" << m2 << std::endl;
458  std::cout << "m3:\n" << m3 << std::endl;
459 
460  std::cout << "\n------------------------" << std::endl;
461  std::cout << "--- TEST vpMatrix stack()" << std::endl;
462  std::cout << "------------------------" << std::endl;
463 
464  {
465  vpMatrix L, L2(2, 6);
466  L2 = 2;
467  L.stack(L2);
468  std::cout << "L:\n" << L << std::endl;
469  L2.resize(3, 6);
470  L2 = 3;
471  L.stack(L2);
472  std::cout << "L:\n" << L << std::endl;
473  }
474 
475  {
476  vpMatrix m_big_stack;
477  t = vpTime::measureTimeMs();
478  for (unsigned int i = 0; i < nb; i++) {
479  m_big_stack.stack(submatrices[(size_t)i]);
480  }
481  t = vpTime::measureTimeMs() - t;
482  std::cout << "Matrix stack(): " << t << " ms" << std::endl;
483 
484  if (!equalMatrix(m_big, m_big_stack)) {
485  std::cerr << "Problem with vpMatrix stack()!" << std::endl;
486  return EXIT_FAILURE;
487  }
488  }
489 
490  std::cout << "\n------------------------" << std::endl;
491  std::cout << "--- TEST vpMatrix stack(vpRowVector)" << std::endl;
492  std::cout << "------------------------" << std::endl;
493 
494  vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
495  std::cout << "m_big_stack: " << m_big_stack.getRows() << "x" << m_big_stack.getCols() << std::endl;
496 
497  vpMatrix m_big_stack_row;
498  t = vpTime::measureTimeMs();
499  for (unsigned int i = 0; i < m_big_stack.getRows(); i++) {
500  m_big_stack_row.stack(m_big_stack.getRow(i));
501  }
502  t = vpTime::measureTimeMs() - t;
503  std::cout << "Matrix stack(vpRowVector): " << t << " ms" << std::endl;
504 
505  if (!equalMatrix(m_big_stack, m_big_stack_row)) {
506  std::cerr << "Problem with vpMatrix stack(vpRowVector)!" << std::endl;
507  return EXIT_FAILURE;
508  }
509 
510  std::cout << "\n------------------------" << std::endl;
511  std::cout << "--- TEST vpMatrix stack(vpColVector)" << std::endl;
512  std::cout << "------------------------" << std::endl;
513 
514  vpMatrix m_big_stack_col;
515  t = vpTime::measureTimeMs();
516  for (unsigned int j = 0; j < m_big_stack.getCols(); j++) {
517  m_big_stack_col.stack(m_big_stack.getCol(j));
518  }
519  t = vpTime::measureTimeMs() - t;
520  std::cout << "Matrix stack(vpColVector): " << t << " ms" << std::endl;
521 
522  if (!equalMatrix(m_big_stack, m_big_stack_col)) {
523  std::cerr << "Problem with vpMatrix stack(vpColVector)!" << std::endl;
524  return EXIT_FAILURE;
525  }
526 
527  std::cout << "\n------------------------" << std::endl;
528  std::cout << "--- TEST vpMatrix::stack()" << std::endl;
529  std::cout << "------------------------" << std::endl;
530 
531  {
532  vpMatrix L, L2(2, 6), L_tmp;
533  L2 = 2;
534  vpMatrix::stack(L_tmp, L2, L);
535  std::cout << "L:\n" << L << std::endl;
536  L2.resize(3, 6);
537  L2 = 3;
538  L_tmp = L;
539  vpMatrix::stack(L_tmp, L2, L);
540  std::cout << "L:\n" << L << std::endl;
541  }
542 
543  {
544  vpMatrix m_big_stack_static, m_big_stack_static_tmp;
545  t = vpTime::measureTimeMs();
546  for (unsigned int i = 0; i < nb; i++) {
547  vpMatrix::stack(m_big_stack_static_tmp, submatrices[(size_t)i], m_big_stack_static);
548  m_big_stack_static_tmp = m_big_stack_static;
549  }
550  t = vpTime::measureTimeMs() - t;
551  std::cout << "Matrix::stack(): " << t << " ms" << std::endl;
552 
553  if (!equalMatrix(m_big, m_big_stack_static)) {
554  std::cerr << "Problem with vpMatrix::stack()!" << std::endl;
555  return EXIT_FAILURE;
556  }
557  }
558 
559  std::cout << "\n------------------------" << std::endl;
560  std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
561  std::cout << "------------------------" << std::endl;
562 
563  vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
564  std::cout << "m_big_stack_static: " << m_big_stack_static.getRows() << "x" << m_big_stack_static.getCols()
565  << std::endl;
566 
567  vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
568  t = vpTime::measureTimeMs();
569  for (unsigned int i = 0; i < m_big_stack_static.getRows(); i++) {
570  vpMatrix::stack(m_big_stack_static_row_tmp, m_big_stack_static.getRow(i), m_big_stack_static_row);
571  m_big_stack_static_row_tmp = m_big_stack_static_row;
572  }
573  t = vpTime::measureTimeMs() - t;
574  std::cout << "Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t << " ms" << std::endl;
575 
576  if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
577  std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
578  "vpMatrix)!"
579  << std::endl;
580  return EXIT_FAILURE;
581  }
582 
583  std::cout << "\n------------------------" << std::endl;
584  std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
585  std::cout << "------------------------" << std::endl;
586 
587  vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
588  t = vpTime::measureTimeMs();
589  for (unsigned int j = 0; j < m_big_stack_static.getCols(); j++) {
590  vpMatrix::stack(m_big_stack_static_col_tmp, m_big_stack_static.getCol(j), m_big_stack_static_col);
591  m_big_stack_static_col_tmp = m_big_stack_static_col;
592  }
593  t = vpTime::measureTimeMs() - t;
594  std::cout << "Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t << " ms" << std::endl;
595 
596  if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
597  std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpColVector, "
598  "vpMatrix)!"
599  << std::endl;
600  return EXIT_FAILURE;
601  }
602  }
603 
604  {
605  vpMatrix m1(11, 9), m2(3, 4);
606  for (unsigned int i = 0; i < m2.getRows(); i++) {
607  for (unsigned int j = 0; j < m2.getCols(); j++) {
608  m2[i][j] = getRandomValues(-100.0, 100.0);
609  }
610  }
611 
612  unsigned int offset_i = 4, offset_j = 3;
613  m1.insert(m2, offset_i, offset_j);
614 
615  for (unsigned int i = 0; i < m2.getRows(); i++) {
616  for (unsigned int j = 0; j < m2.getCols(); j++) {
617  if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
618  std::cerr << "Problem with vpMatrix insert()!" << std::endl;
619  return EXIT_FAILURE;
620  }
621  }
622  }
623 
624  offset_i = 4;
625  offset_j = 5;
626  m1.insert(m2, offset_i, offset_j);
627 
628  for (unsigned int i = 0; i < m2.getRows(); i++) {
629  for (unsigned int j = 0; j < m2.getCols(); j++) {
630  if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
631  std::cerr << "Problem with vpMatrix insert()!" << std::endl;
632  return EXIT_FAILURE;
633  }
634  }
635  }
636 
637  offset_i = 8;
638  offset_j = 5;
639  m1.insert(m2, offset_i, offset_j);
640 
641  for (unsigned int i = 0; i < m2.getRows(); i++) {
642  for (unsigned int j = 0; j < m2.getCols(); j++) {
643  if (!vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
644  std::cerr << "Problem with vpMatrix insert()!" << std::endl;
645  return EXIT_FAILURE;
646  }
647  }
648  }
649  }
650 
651  {
652  std::cout << "\n------------------------" << std::endl;
653  std::cout << "--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
654  std::cout << "------------------------" << std::endl;
655 
656  vpMatrix A(5, 6), B(5, 4);
657  for (unsigned int i = 0; i < A.getRows(); i++) {
658  for (unsigned int j = 0; j < A.getCols(); j++) {
659  A[i][j] = i * A.getCols() + j;
660 
661  if (j < B.getCols()) {
662  B[i][j] = (i * B.getCols() + j) * 10;
663  }
664  }
665  }
666 
667  vpMatrix juxtaposeM;
668  vpMatrix::juxtaposeMatrices(A, B, juxtaposeM);
669  std::cout << "juxtaposeM:\n" << juxtaposeM << std::endl;
670  }
671 
672 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
673  {
674  std::vector<vpMatrix> vec_mat;
675  vec_mat.emplace_back(5, 5);
676 
677  vpMatrix A(4, 4), B(4, 4);
678  A = 1;
679  B = 2;
680  vpMatrix res = A + B;
681  std::cout << "\n1) A+B:\n" << res << std::endl;
682 
683  vpMatrix res2;
684  res2 = A + B;
685  std::cout << "\n2) A+B:\n" << res2 << std::endl;
686  }
687 #endif
688 
689  {
690  std::cout << "\n------------------------" << std::endl;
691  std::cout << "--- TEST vpMatrix::hadamard()" << std::endl;
692  std::cout << "------------------------" << std::endl;
693 
694  vpMatrix M1(3, 5), M2(3, 5);
695  for (unsigned int i = 0; i < M1.size(); i++) {
696  M1.data[i] = i;
697  M2.data[i] = i + 2;
698  }
699 
700  // Reference
701  std::vector<double> references = computeHadamard(std::vector<double>(M1.data, M1.data + M1.size()),
702  std::vector<double>(M2.data, M2.data + M2.size()));
703 
704  std::cout << "M1:\n" << M1 << std::endl;
705  std::cout << "\nM2:\n" << M2 << std::endl;
706  M2 = M1.hadamard(M2);
707  std::cout << "\nRes:\n" << M2 << std::endl;
708 
709  if (!test("M2", M2, references)) {
710  std::cerr << "Error with Hadamard product" << std::endl;
711  return EXIT_FAILURE;
712  }
713  }
714 
715  {
716  std::cout << "\n------------------------" << std::endl;
717  std::cout << "--- TEST vpMatrix::stackColums()" << std::endl;
718  std::cout << "------------------------" << std::endl;
719  vpMatrix M(3, 5);
720  for (unsigned int j = 0; j < M.getCols(); j++) {
721  for (unsigned int i = 0; i < M.getRows(); i++) {
722  M[i][j] = i + j * M.getRows();
723  }
724  }
725  std::cout << "M:\n" << M << std::endl;
726  vpColVector v = M.stackColumns();
727  std::cout << "Column stack: " << v.t() << std::endl;
728  if (M.size() != v.size()) {
729  std::cerr << "Problem in vpMatrix::stackColumns(): size differ" << std::endl;
730  return EXIT_FAILURE;
731  }
732  for (unsigned int i = 0; i < v.size(); i++) {
733  if (std::fabs(v[i] - static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
734  std::cerr << "Problem in vpMatrix::stackColumns(): content differ" << std::endl;
735  return EXIT_FAILURE;
736  }
737  }
738  }
739 
740  {
741  std::cout << "\n------------------------" << std::endl;
742  std::cout << "--- TEST vpMatrix::stackRows()" << std::endl;
743  std::cout << "------------------------" << std::endl;
744  vpMatrix M(3, 5);
745  for (unsigned int i = 0; i < M.getRows(); i++) {
746  for (unsigned int j = 0; j < M.getCols(); j++) {
747  M[i][j] = i * M.getCols() + j;
748  }
749  }
750  std::cout << "M:\n" << M << std::endl;
751  vpRowVector v = M.stackRows();
752  std::cout << "Rows stack: " << v << std::endl;
753  if (M.size() != v.size()) {
754  std::cerr << "Problem in vpMatrix::stackRows(): size differ" << std::endl;
755  return EXIT_FAILURE;
756  }
757  for (unsigned int i = 0; i < v.size(); i++) {
758  if (std::fabs(v[i] - static_cast<double>(i)) > std::numeric_limits<double>::epsilon()) {
759  std::cerr << "Problem in vpMatrix::stackRows(): content differ" << std::endl;
760  return EXIT_FAILURE;
761  }
762  }
763  }
764 
765  {
766  std::cout << "\n------------------------" << std::endl;
767  std::cout << "--- TEST vpMatrix::getCol()" << std::endl;
768  std::cout << "------------------------" << std::endl;
769  vpMatrix A(4, 4);
770  for (unsigned int i = 0; i < A.getRows(); i++)
771  for (unsigned int j = 0; j < A.getCols(); j++)
772  A[i][j] = i * A.getCols() + j;
773 
774  {
775  vpColVector cv = A.getCol(1, 1, 3);
776  vpColVector ref;
777  ref << 5, 9, 13;
778  if (cv != ref) {
779  std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
780  return EXIT_FAILURE;
781  }
782  }
783  {
784  vpColVector cv = A.getCol(1);
785  vpColVector ref;
786  ref << 1, 5, 9, 13;
787  if (cv != ref) {
788  std::cerr << "Problem in vpMatrix::getCol(): values are different" << std::endl;
789  return EXIT_FAILURE;
790  }
791  }
792  }
793 
794  {
795  std::cout << "\n------------------------" << std::endl;
796  std::cout << "--- TEST vpMatrix::getRow()" << std::endl;
797  std::cout << "------------------------" << std::endl;
798  vpMatrix A(4, 4);
799  for (unsigned int i = 0; i < A.getRows(); i++)
800  for (unsigned int j = 0; j < A.getCols(); j++)
801  A[i][j] = i * A.getCols() + j;
802 
803  {
804  vpRowVector rv = A.getRow(1, 1, 3);
805  vpRowVector ref;
806  ref << 5, 6, 7;
807  if (rv != ref) {
808  std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
809  return EXIT_FAILURE;
810  }
811  }
812  {
813  vpRowVector rv = A.getRow(1);
814  vpRowVector ref;
815  ref << 4, 5, 6, 7;
816  if (rv != ref) {
817  std::cerr << "Problem in vpMatrix::getRow(): values are different" << std::endl;
818  return EXIT_FAILURE;
819  }
820  }
821  }
822 
823  {
824  std::cout << "\n------------------------" << std::endl;
825  std::cout << "--- TEST vpMatrix::getDiag()" << std::endl;
826  std::cout << "------------------------" << std::endl;
827  vpMatrix A(3, 4);
828  for (unsigned int i = 0; i < A.getRows(); i++)
829  for (unsigned int j = 0; j < A.getCols(); j++)
830  A[i][j] = i * A.getCols() + j;
831 
832  vpColVector diag = A.getDiag();
833  vpColVector ref;
834  ref << 0.0, 5.0, 10.0;
835  if (diag != ref) {
836  std::cerr << "Problem in vpMatrix::getDiag(): values are different" << std::endl;
837  return EXIT_FAILURE;
838  }
839  }
840 
841  std::cout << "\nAll tests succeeded" << std::endl;
842  return EXIT_SUCCESS;
843  }
844  catch (const vpException &e) {
845  std::cout << "Catch an exception: " << e << std::endl;
846  return EXIT_FAILURE;
847  }
848 }
unsigned int getCols() const
Definition: vpArray2D.h:274
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
void vpGEMM(const vpArray2D< double > &A, const vpArray2D< double > &B, const double &alpha, const vpArray2D< double > &C, const double &beta, vpArray2D< double > &D, const unsigned int &ops=0)
Definition: vpGEMM.h:388
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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpRowVector t() const
error that can be emitted by ViSP classes.
Definition: vpException.h:59
static double rad(double deg)
Definition: vpMath.h:127
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
vpMatrix hadamard(const vpMatrix &m) const
Definition: vpMatrix.cpp:1806
int print(std::ostream &s, unsigned int length, const std::string &intro="") const
Definition: vpMatrix.cpp:5588
void init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
Definition: vpMatrix.cpp:338
void eye()
Definition: vpMatrix.cpp:442
static bool loadMatrix(const std::string &filename, vpArray2D< double > &M, bool binary=false, char *header=nullptr)
Definition: vpMatrix.h:748
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5865
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5521
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5219
vpColVector getDiag() const
Definition: vpMatrix.cpp:5308
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5181
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5977
static bool saveMatrixYAML(const std::string &filename, const vpArray2D< double > &M, const char *header="")
Definition: vpMatrix.h:972
static bool loadMatrixYAML(const std::string &filename, vpArray2D< double > &M, char *header=nullptr)
Definition: vpMatrix.h:823
static bool saveMatrix(const std::string &filename, const vpArray2D< double > &M, bool binary=false, const char *header="")
Definition: vpMatrix.h:896
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of row vector and the associated operations.
Definition: vpRowVector.h:107
void resize(unsigned int i, bool flagNullify=true)
Definition: vpRowVector.h:259
VISP_EXPORT double measureTimeMs()