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