Visual Servoing Platform  version 3.6.1 under development (2024-10-15)
testMatrixPseudoInverse.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Test various svd decompositions.
32  */
33 
39 #include <algorithm>
40 #include <stdio.h>
41 #include <stdlib.h>
42 #include <vector>
43 
44 #include <visp3/core/vpColVector.h>
45 #include <visp3/core/vpMatrix.h>
46 #include <visp3/core/vpTime.h>
47 #include <visp3/io/vpParseArgv.h>
48 
49 // List of allowed command line options
50 #define GETOPTARGS "cdn:i:pf:R:C:vh"
51 
52 #ifdef ENABLE_VISP_NAMESPACE
53 using namespace VISP_NAMESPACE_NAME;
54 #endif
55 
64 void usage(const char *name, const char *badparam)
65 {
66  fprintf(stdout, "\n\
67 Test matrix pseudo-inverse.\n\
68 Outputs a comparison of the results obtained by supported 3rd parties.\n\
69 \n\
70 SYNOPSIS\n\
71  %s [-n <number of matrices>] [-f <plot filename>]\n\
72  [-R <number of rows>] [-C <number of columns>]\n\
73  [-i <number of iterations>] [-p] [-h]\n",
74  name);
75 
76  fprintf(stdout, "\n\
77 OPTIONS: Default\n\
78  -n <number of matrices> \n\
79  Number of matrices inverted during each test loop.\n\
80 \n\
81  -i <number of iterations> \n\
82  Number of iterations of the test.\n\
83 \n\
84  -f <plot filename> \n\
85  Set output path for plot output.\n\
86  The plot logs the times of \n\
87  the different inversion methods: \n\
88  QR,LU,Cholesky and Pseudo-inverse.\n\
89 \n\
90  -R <number of rows>\n\
91  Number of rows of the automatically generated matrices \n\
92  we test on.\n\
93 \n\
94  -C <number of columns>\n\
95  Number of colums of the automatically generated matrices \n\
96  we test on.\n\
97 \n\
98  -p \n\
99  Plot into filename in the gnuplot format. \n\
100  If this option is used, tests results will be logged \n\
101  into a filename specified with -f.\n\
102 \n\
103  -h\n\
104  Print the help.\n\n");
105 
106  if (badparam) {
107  fprintf(stderr, "ERROR: \n");
108  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
109  }
110 }
111 
119 bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
120  bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
121 {
122  const char *optarg_;
123  int c;
124  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
125 
126  switch (c) {
127  case 'h':
128  usage(argv[0], nullptr);
129  return false;
130  break;
131  case 'n':
132  nb_matrices = (unsigned int)atoi(optarg_);
133  break;
134  case 'i':
135  nb_iterations = (unsigned int)atoi(optarg_);
136  break;
137  case 'f':
138  plotfile = optarg_;
139  use_plot_file = true;
140  break;
141  case 'p':
142  use_plot_file = true;
143  break;
144  case 'R':
145  nbrows = (unsigned int)atoi(optarg_);
146  break;
147  case 'C':
148  nbcols = (unsigned int)atoi(optarg_);
149  break;
150  case 'v':
151  verbose = true;
152  break;
153  // add default options -c -d
154  case 'c':
155  break;
156  case 'd':
157  break;
158  default:
159  usage(argv[0], optarg_);
160  return false;
161  break;
162  }
163  }
164 
165  if ((c == 1) || (c == -1)) {
166  // standalone param or error
167  usage(argv[0], nullptr);
168  std::cerr << "ERROR: " << std::endl;
169  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
170  return false;
171  }
172 
173  return true;
174 }
175 
176 vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
177 {
178  vpMatrix A;
179  A.resize(nbrows, nbcols);
180 
181  for (unsigned int i = 0; i < A.getRows(); i++) {
182  for (unsigned int j = 0; j < A.getCols(); j++) {
183  A[i][j] = (double)rand() / (double)RAND_MAX;
184  }
185  }
186 
187  return A;
188 }
189 
190 void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
191  std::vector<vpMatrix> &bench)
192 {
193  if (verbose)
194  std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl;
195  bench.clear();
196  for (unsigned int i = 0; i < nb_matrices; i++) {
197  vpMatrix M = make_random_matrix(nb_rows, nb_cols);
198  bench.push_back(M);
199  }
200 }
201 
202 int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api)
203 {
204  double allowed_error = 1e-3;
205  double error = 0;
206  vpMatrix A_Api, Api_A;
207 
208  for (unsigned int i = 0; i < A.size(); i++) {
209  error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm();
210  if (error > allowed_error) {
211  std::cout << "Bad pseudo-inverse [" << i << "] test A A^+ A = A: euclidean norm: " << error << std::endl;
212  return EXIT_FAILURE;
213  }
214  error = (Api[i] * A[i] * Api[i] - Api[i]).frobeniusNorm();
215  if (error > allowed_error) {
216  std::cout << "Bad pseudo-inverse [" << i << "] test A^+ A A^+ = A^+: euclidean norm: " << error << std::endl;
217  return EXIT_FAILURE;
218  }
219  A_Api = A[i] * Api[i];
220  error = (A_Api.transpose() - A_Api).frobeniusNorm();
221  if (error > allowed_error) {
222  std::cout << "Bad pseudo-inverse [" << i << "] test (A A^+)^T = A A^+: euclidean norm: " << error << std::endl;
223  return EXIT_FAILURE;
224  }
225  Api_A = Api[i] * A[i];
226  error = (Api_A.transpose() - Api_A).frobeniusNorm();
227  if (error > allowed_error) {
228  std::cout << "Bad pseudo-inverse [" << i << "] test (A^+ A )^T = A^+ A: euclidean norm: " << error << std::endl;
229  return EXIT_FAILURE;
230  }
231  }
232 
233  return EXIT_SUCCESS;
234 }
235 
236 int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api,
237  const std::vector<vpColVector> &sv, const std::vector<vpMatrix> &imA,
238  const std::vector<vpMatrix> &imAt, const std::vector<vpMatrix> &kerAt)
239 {
240  double allowed_error = 1e-3;
241  // test Api
242  if (test_pseudo_inverse(A, Api) == EXIT_FAILURE) {
243  return EXIT_FAILURE;
244  }
245 
246  // test kerA
247  for (unsigned int i = 0; i < kerAt.size(); i++) {
248  if (kerAt[i].size()) {
249  vpMatrix nullspace = A[i] * kerAt[i].t();
250  double error = nullspace.frobeniusNorm();
251  if (error > allowed_error) {
252  std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl;
253  return EXIT_FAILURE;
254  }
255  }
256  }
257 
258  // test sv, imA, imAt, kerA
259  for (unsigned int i = 0; i < kerAt.size(); i++) {
260  unsigned int rank = imA[i].getCols();
261  vpMatrix U, S(rank, A[i].getCols()), Vt(A[i].getCols(), A[i].getCols());
262  U = imA[i];
263 
264  for (unsigned int j = 0; j < rank; j++)
265  S[j][j] = sv[i][j];
266 
267  Vt.insert(imAt[i].t(), 0, 0);
268  Vt.insert(kerAt[i], imAt[i].getCols(), 0);
269 
270  double error = (U * S * Vt - A[i]).frobeniusNorm();
271 
272  if (error > allowed_error) {
273  std::cout << "Bad imA, imAt, sv, kerAt [" << i << "]: euclidean norm: " << error << std::endl;
274  return EXIT_FAILURE;
275  }
276  }
277 
278  return EXIT_SUCCESS;
279 }
280 
281 int test_pseudo_inverse_default(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
282 {
283  if (verbose)
284  std::cout << "Test pseudo-inverse using default 3rd party" << std::endl;
285  if (verbose)
286  std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
287 
288  size_t size = bench.size();
289  std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
290  std::vector<vpColVector> sv(size);
291  int ret = EXIT_SUCCESS;
292  time.clear();
293 
294  // test 1
295  double t = vpTime::measureTimeMs();
296  for (unsigned int i = 0; i < bench.size(); i++) {
297  PI[i] = bench[i].pseudoInverse();
298  }
299  time.push_back(vpTime::measureTimeMs() - t);
300  for (unsigned int i = 0; i < time.size(); i++) {
301  ret += test_pseudo_inverse(bench, PI);
302  }
303 
304  // test 2
305  t = vpTime::measureTimeMs();
306  for (unsigned int i = 0; i < bench.size(); i++) {
307  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
308  unsigned int rank = bench[i].pseudoInverse(PI[i]);
309  if (rank != rank_bench) {
310  if (verbose) {
311  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
312  }
313  ret += EXIT_FAILURE;
314  }
315  }
316  time.push_back(vpTime::measureTimeMs() - t);
317  for (unsigned int i = 0; i < time.size(); i++) {
318  ret += test_pseudo_inverse(bench, PI);
319  }
320 
321  // test 3
322  t = vpTime::measureTimeMs();
323  for (unsigned int i = 0; i < bench.size(); i++) {
324  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
325  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i]);
326  if (rank != rank_bench) {
327  if (verbose) {
328  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
329  }
330  ret += EXIT_FAILURE;
331  }
332  }
333  time.push_back(vpTime::measureTimeMs() - t);
334  for (unsigned int i = 0; i < time.size(); i++) {
335  ret += test_pseudo_inverse(bench, PI);
336  }
337 
338  // test 4
339  t = vpTime::measureTimeMs();
340  for (unsigned int i = 0; i < bench.size(); i++) {
341  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
342  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i]);
343  if (rank != rank_bench) {
344  if (verbose) {
345  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
346  }
347  ret += EXIT_FAILURE;
348  }
349  }
350  time.push_back(vpTime::measureTimeMs() - t);
351  for (unsigned int i = 0; i < time.size(); i++) {
352  ret += test_pseudo_inverse(bench, PI);
353  }
354 
355  // test 5
356  t = vpTime::measureTimeMs();
357  for (unsigned int i = 0; i < bench.size(); i++) {
358  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
359  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
360  if (rank != rank_bench) {
361  if (verbose) {
362  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
363  }
364  ret += EXIT_FAILURE;
365  }
366  }
367  time.push_back(vpTime::measureTimeMs() - t);
368  for (unsigned int i = 0; i < time.size(); i++) {
369  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
370  }
371 
372  //-------------------
373 
374  // test 6
375  t = vpTime::measureTimeMs();
376  for (unsigned int i = 0; i < bench.size(); i++) {
377  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
378  PI[i] = bench[i].pseudoInverse(static_cast<int>(rank_bench));
379  }
380  time.push_back(vpTime::measureTimeMs() - t);
381  for (unsigned int i = 0; i < time.size(); i++) {
382  ret += test_pseudo_inverse(bench, PI);
383  }
384 
385  // test 7
386  t = vpTime::measureTimeMs();
387  for (unsigned int i = 0; i < bench.size(); i++) {
388  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
389  unsigned int rank = bench[i].pseudoInverse(PI[i], static_cast<int>(rank_bench));
390  if (rank != rank_bench) {
391  if (verbose) {
392  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
393  }
394  ret += EXIT_FAILURE;
395  }
396  }
397  time.push_back(vpTime::measureTimeMs() - t);
398  for (unsigned int i = 0; i < time.size(); i++) {
399  ret += test_pseudo_inverse(bench, PI);
400  }
401 
402  // test 8
403  t = vpTime::measureTimeMs();
404  for (unsigned int i = 0; i < bench.size(); i++) {
405  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
406  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast<int>(rank_bench));
407  if (rank != rank_bench) {
408  if (verbose) {
409  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
410  }
411  ret += EXIT_FAILURE;
412  }
413  }
414  time.push_back(vpTime::measureTimeMs() - t);
415  for (unsigned int i = 0; i < time.size(); i++) {
416  ret += test_pseudo_inverse(bench, PI);
417  }
418 
419  // test 9
420  t = vpTime::measureTimeMs();
421  for (unsigned int i = 0; i < bench.size(); i++) {
422  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
423  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast<int>(rank_bench), imA[i], imAt[i]);
424  if (rank != rank_bench) {
425  if (verbose) {
426  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
427  }
428  ret += EXIT_FAILURE;
429  }
430  }
431  time.push_back(vpTime::measureTimeMs() - t);
432  for (unsigned int i = 0; i < time.size(); i++) {
433  ret += test_pseudo_inverse(bench, PI);
434  }
435 
436  // test 10
437  t = vpTime::measureTimeMs();
438  for (unsigned int i = 0; i < bench.size(); i++) {
439  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
440  unsigned int rank = bench[i].pseudoInverse(PI[i], sv[i], static_cast<int>(rank_bench), imA[i], imAt[i], kerAt[i]);
441  if (rank != rank_bench) {
442  if (verbose) {
443  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
444  }
445  ret += EXIT_FAILURE;
446  }
447  }
448  time.push_back(vpTime::measureTimeMs() - t);
449  for (unsigned int i = 0; i < time.size(); i++) {
450  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
451  }
452 
453  return ret;
454 }
455 
456 #if defined(VISP_HAVE_EIGEN3)
457 int test_pseudo_inverse_eigen3(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
458 {
459  if (verbose)
460  std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl;
461  if (verbose)
462  std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
463 
464  size_t size = bench.size();
465  std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
466  std::vector<vpColVector> sv(size);
467  int ret = EXIT_SUCCESS;
468  time.clear();
469 
470  // test 1
471  double t = vpTime::measureTimeMs();
472  for (unsigned int i = 0; i < bench.size(); i++) {
473  PI[i] = bench[i].pseudoInverseEigen3();
474  }
475  time.push_back(vpTime::measureTimeMs() - t);
476  for (unsigned int i = 0; i < time.size(); i++) {
477  ret += test_pseudo_inverse(bench, PI);
478  }
479 
480  // test 2
481  t = vpTime::measureTimeMs();
482  for (unsigned int i = 0; i < bench.size(); i++) {
483  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
484  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i]);
485  if (rank != rank_bench) {
486  if (verbose) {
487  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
488  }
489  ret += EXIT_FAILURE;
490  }
491  }
492  time.push_back(vpTime::measureTimeMs() - t);
493  for (unsigned int i = 0; i < time.size(); i++) {
494  ret += test_pseudo_inverse(bench, PI);
495  }
496 
497  // test 3
498  t = vpTime::measureTimeMs();
499  for (unsigned int i = 0; i < bench.size(); i++) {
500  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
501  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i]);
502  if (rank != rank_bench) {
503  if (verbose) {
504  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
505  }
506  ret += EXIT_FAILURE;
507  }
508  }
509  time.push_back(vpTime::measureTimeMs() - t);
510  for (unsigned int i = 0; i < time.size(); i++) {
511  ret += test_pseudo_inverse(bench, PI);
512  }
513 
514  // test 4
515  t = vpTime::measureTimeMs();
516  for (unsigned int i = 0; i < bench.size(); i++) {
517  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
518  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
519  if (rank != rank_bench) {
520  if (verbose) {
521  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
522  }
523  ret += EXIT_FAILURE;
524  }
525  }
526  time.push_back(vpTime::measureTimeMs() - t);
527  for (unsigned int i = 0; i < time.size(); i++) {
528  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
529  }
530 
531  //-------------------
532 
533  // test 5
534  t = vpTime::measureTimeMs();
535  for (unsigned int i = 0; i < bench.size(); i++) {
536  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
537  PI[i] = bench[i].pseudoInverseEigen3(static_cast<int>(rank_bench));
538  }
539  time.push_back(vpTime::measureTimeMs() - t);
540  for (unsigned int i = 0; i < time.size(); i++) {
541  ret += test_pseudo_inverse(bench, PI);
542  }
543 
544  // test 6
545  t = vpTime::measureTimeMs();
546  for (unsigned int i = 0; i < bench.size(); i++) {
547  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
548  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], static_cast<int>(rank_bench));
549  if (rank != rank_bench) {
550  if (verbose) {
551  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
552  }
553  ret += EXIT_FAILURE;
554  }
555  }
556  time.push_back(vpTime::measureTimeMs() - t);
557  for (unsigned int i = 0; i < time.size(); i++) {
558  ret += test_pseudo_inverse(bench, PI);
559  }
560 
561  // test 7
562  t = vpTime::measureTimeMs();
563  for (unsigned int i = 0; i < bench.size(); i++) {
564  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
565  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], static_cast<int>(rank_bench));
566  if (rank != rank_bench) {
567  if (verbose) {
568  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
569  }
570  ret += EXIT_FAILURE;
571  }
572  }
573  time.push_back(vpTime::measureTimeMs() - t);
574  for (unsigned int i = 0; i < time.size(); i++) {
575  ret += test_pseudo_inverse(bench, PI);
576  }
577 
578  // test 8
579  t = vpTime::measureTimeMs();
580  for (unsigned int i = 0; i < bench.size(); i++) {
581  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
582  unsigned int rank = bench[i].pseudoInverseEigen3(PI[i], sv[i], static_cast<int>(rank_bench), imA[i], imAt[i], kerAt[i]);
583  if (rank != rank_bench) {
584  if (verbose) {
585  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
586  }
587  ret += EXIT_FAILURE;
588  }
589  }
590  time.push_back(vpTime::measureTimeMs() - t);
591 
592  for (unsigned int i = 0; i < time.size(); i++) {
593  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
594  }
595 
596  return ret;
597 }
598 #endif
599 
600 #if defined(VISP_HAVE_LAPACK)
601 int test_pseudo_inverse_lapack(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
602 {
603  if (verbose)
604  std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl;
605  if (verbose)
606  std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
607 
608  size_t size = bench.size();
609  std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
610  std::vector<vpColVector> sv(size);
611  int ret = EXIT_SUCCESS;
612  time.clear();
613 
614  // test 1
615  double t = vpTime::measureTimeMs();
616  for (unsigned int i = 0; i < bench.size(); i++) {
617  PI[i] = bench[i].pseudoInverseLapack();
618  }
619  time.push_back(vpTime::measureTimeMs() - t);
620  for (unsigned int i = 0; i < time.size(); i++) {
621  ret += test_pseudo_inverse(bench, PI);
622  }
623 
624  // test 2
625  t = vpTime::measureTimeMs();
626  for (unsigned int i = 0; i < bench.size(); i++) {
627  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
628  unsigned int rank = bench[i].pseudoInverseLapack(PI[i]);
629  if (rank != rank_bench) {
630  if (verbose) {
631  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
632  }
633  ret += EXIT_FAILURE;
634  }
635  }
636  time.push_back(vpTime::measureTimeMs() - t);
637  for (unsigned int i = 0; i < time.size(); i++) {
638  ret += test_pseudo_inverse(bench, PI);
639  }
640 
641  // test 3
642  t = vpTime::measureTimeMs();
643  for (unsigned int i = 0; i < bench.size(); i++) {
644  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
645  unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i]);
646  if (rank != rank_bench) {
647  if (verbose) {
648  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
649  }
650  ret += EXIT_FAILURE;
651  }
652  }
653  time.push_back(vpTime::measureTimeMs() - t);
654  for (unsigned int i = 0; i < time.size(); i++) {
655  ret += test_pseudo_inverse(bench, PI);
656  }
657 
658  // test 4
659  t = vpTime::measureTimeMs();
660  for (unsigned int i = 0; i < bench.size(); i++) {
661  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
662  unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
663  if (rank != rank_bench) {
664  if (verbose) {
665  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
666  }
667  ret += EXIT_FAILURE;
668  }
669  }
670  time.push_back(vpTime::measureTimeMs() - t);
671  for (unsigned int i = 0; i < time.size(); i++) {
672  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
673  }
674 
675  //-------------------
676 
677  // test 5
678  t = vpTime::measureTimeMs();
679  for (unsigned int i = 0; i < bench.size(); i++) {
680  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
681  PI[i] = bench[i].pseudoInverseLapack(static_cast<int>(rank_bench));
682  }
683  time.push_back(vpTime::measureTimeMs() - t);
684  for (unsigned int i = 0; i < time.size(); i++) {
685  ret += test_pseudo_inverse(bench, PI);
686  }
687 
688  // test 6
689  t = vpTime::measureTimeMs();
690  for (unsigned int i = 0; i < bench.size(); i++) {
691  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
692  unsigned int rank = bench[i].pseudoInverseLapack(PI[i], static_cast<int>(rank_bench));
693  if (rank != rank_bench) {
694  if (verbose) {
695  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
696  }
697  ret += EXIT_FAILURE;
698  }
699  }
700  time.push_back(vpTime::measureTimeMs() - t);
701  for (unsigned int i = 0; i < time.size(); i++) {
702  ret += test_pseudo_inverse(bench, PI);
703  }
704 
705  // test 7
706  t = vpTime::measureTimeMs();
707  for (unsigned int i = 0; i < bench.size(); i++) {
708  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
709  unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], static_cast<int>(rank_bench));
710  if (rank != rank_bench) {
711  if (verbose) {
712  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
713  }
714  ret += EXIT_FAILURE;
715  }
716  }
717  time.push_back(vpTime::measureTimeMs() - t);
718  for (unsigned int i = 0; i < time.size(); i++) {
719  ret += test_pseudo_inverse(bench, PI);
720  }
721 
722  // test 8
723  t = vpTime::measureTimeMs();
724  for (unsigned int i = 0; i < bench.size(); i++) {
725  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
726  unsigned int rank = bench[i].pseudoInverseLapack(PI[i], sv[i], static_cast<int>(rank_bench), imA[i], imAt[i], kerAt[i]);
727  if (rank != rank_bench) {
728  if (verbose) {
729  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
730  }
731  ret += EXIT_FAILURE;
732  }
733  }
734  time.push_back(vpTime::measureTimeMs() - t);
735 
736  for (unsigned int i = 0; i < time.size(); i++) {
737  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
738  }
739 
740  return ret;
741 }
742 #endif
743 
744 #if defined(VISP_HAVE_OPENCV)
745 int test_pseudo_inverse_opencv(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
746 {
747  if (verbose)
748  std::cout << "Test pseudo-inverse using OpenCV 3rd party" << std::endl;
749  if (verbose)
750  std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
751 
752  size_t size = bench.size();
753  std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
754  std::vector<vpColVector> sv(size);
755  int ret = EXIT_SUCCESS;
756  time.clear();
757 
758  // test 1
759  double t = vpTime::measureTimeMs();
760  for (unsigned int i = 0; i < bench.size(); i++) {
761  PI[i] = bench[i].pseudoInverseOpenCV();
762  }
763  time.push_back(vpTime::measureTimeMs() - t);
764  for (unsigned int i = 0; i < time.size(); i++) {
765  ret += test_pseudo_inverse(bench, PI);
766  }
767 
768  // test 2
769  t = vpTime::measureTimeMs();
770  for (unsigned int i = 0; i < bench.size(); i++) {
771  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
772  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i]);
773  if (rank != rank_bench) {
774  if (verbose) {
775  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
776  }
777  ret += EXIT_FAILURE;
778  }
779  }
780  time.push_back(vpTime::measureTimeMs() - t);
781  for (unsigned int i = 0; i < time.size(); i++) {
782  ret += test_pseudo_inverse(bench, PI);
783  }
784 
785  // test 3
786  t = vpTime::measureTimeMs();
787  for (unsigned int i = 0; i < bench.size(); i++) {
788  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
789  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i]);
790  if (rank != rank_bench) {
791  if (verbose) {
792  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
793  }
794  ret += EXIT_FAILURE;
795  }
796  }
797  time.push_back(vpTime::measureTimeMs() - t);
798  for (unsigned int i = 0; i < time.size(); i++) {
799  ret += test_pseudo_inverse(bench, PI);
800  }
801 
802  // test 4
803  t = vpTime::measureTimeMs();
804  for (unsigned int i = 0; i < bench.size(); i++) {
805  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
806  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
807  if (rank != rank_bench) {
808  if (verbose) {
809  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
810  }
811  ret += EXIT_FAILURE;
812  }
813  }
814  time.push_back(vpTime::measureTimeMs() - t);
815  for (unsigned int i = 0; i < time.size(); i++) {
816  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
817  }
818  //-------------------
819 
820  // test 5
821  t = vpTime::measureTimeMs();
822  for (unsigned int i = 0; i < bench.size(); i++) {
823  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
824  PI[i] = bench[i].pseudoInverseOpenCV(static_cast<int>(rank_bench));
825  }
826  time.push_back(vpTime::measureTimeMs() - t);
827  for (unsigned int i = 0; i < time.size(); i++) {
828  ret += test_pseudo_inverse(bench, PI);
829  }
830 
831  // test 6
832  t = vpTime::measureTimeMs();
833  for (unsigned int i = 0; i < bench.size(); i++) {
834  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
835  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], static_cast<int>(rank_bench));
836  if (rank != rank_bench) {
837  if (verbose) {
838  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
839  }
840  ret += EXIT_FAILURE;
841  }
842  }
843  time.push_back(vpTime::measureTimeMs() - t);
844  for (unsigned int i = 0; i < time.size(); i++) {
845  ret += test_pseudo_inverse(bench, PI);
846  }
847 
848  // test 7
849  t = vpTime::measureTimeMs();
850  for (unsigned int i = 0; i < bench.size(); i++) {
851  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
852  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], static_cast<int>(rank_bench));
853  if (rank != rank_bench) {
854  if (verbose) {
855  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
856  }
857  ret += EXIT_FAILURE;
858  }
859  }
860  time.push_back(vpTime::measureTimeMs() - t);
861  for (unsigned int i = 0; i < time.size(); i++) {
862  ret += test_pseudo_inverse(bench, PI);
863  }
864 
865  // test 8
866  t = vpTime::measureTimeMs();
867  for (unsigned int i = 0; i < bench.size(); i++) {
868  unsigned int rank_bench = std::min(bench[i].getRows(), bench[i].getCols());
869  unsigned int rank = bench[i].pseudoInverseOpenCV(PI[i], sv[i], static_cast<int>(rank_bench), imA[i], imAt[i], kerAt[i]);
870  if (rank != rank_bench) {
871  if (verbose) {
872  std::cout << " Error in the rank (" << rank << ")" << " while expected rank is " << rank_bench << std::endl;
873  }
874  ret += EXIT_FAILURE;
875  }
876  }
877  time.push_back(vpTime::measureTimeMs() - t);
878 
879  for (unsigned int i = 0; i < time.size(); i++) {
880  ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
881  }
882 
883  return ret;
884 }
885 #endif
886 
887 void save_time(const std::string &method, unsigned int nrows, unsigned int ncols, bool verbose, bool use_plot_file,
888  std::ofstream &of, const std::vector<double> &time)
889 {
890  for (size_t i = 0; i < time.size(); i++) {
891  if (use_plot_file)
892  of << time[i] << "\t";
893  if (verbose) {
894  std::cout << " " << method << " pseudo inverse (" << nrows << "x" << ncols << ")"
895  << " time test " << i << ": " << time[i] << std::endl;
896  }
897  }
898 }
899 
900 int main(int argc, const char *argv[])
901 {
902  try {
903 #if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV)
904  unsigned int nb_matrices = 10;
905  unsigned int nb_iterations = 10;
906  unsigned int nb_rows = 12;
907  unsigned int nb_cols = 6;
908  bool verbose = false;
909  std::string plotfile("plot-pseudo-inv.csv");
910  bool use_plot_file = false;
911  std::ofstream of;
912 
913  unsigned int nb_svd_functions = 4; // 4 tests for each existing vpMatrix::pseudoInverse(...) functions
914  unsigned int nb_test_matrix_size = 3; // 3 tests: m > n, m = n, m < n
915  std::vector<double> time(nb_svd_functions);
916  std::vector<unsigned int> nrows(nb_test_matrix_size), ncols(nb_test_matrix_size);
917 
918  // Read the command line options
919  if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) ==
920  false) {
921  return EXIT_FAILURE;
922  }
923 
924  for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
925  // consider m > n, m = n, m < n
926  if (s == 0) {
927  nrows[s] = nb_rows;
928  ncols[s] = nb_cols;
929  }
930  else if (s == 1) {
931  nrows[s] = nb_cols;
932  ncols[s] = nb_cols;
933  }
934  else {
935  nrows[s] = nb_cols;
936  ncols[s] = nb_rows;
937  }
938  }
939 
940  if (use_plot_file) {
941  of.open(plotfile.c_str());
942  of << "iter"
943  << "\t";
944 
945  for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
946  for (unsigned int i = 0; i < nb_svd_functions; i++)
947  of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
948  << "\t";
949 
950 #if defined(VISP_HAVE_LAPACK)
951  for (unsigned int i = 0; i < nb_svd_functions; i++)
952  of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
953  << "\t";
954 #endif
955 #if defined(VISP_HAVE_EIGEN3)
956  for (unsigned int i = 0; i < nb_svd_functions; i++)
957  of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
958  << "\t";
959 #endif
960 #if defined(VISP_HAVE_OPENCV)
961  for (unsigned int i = 0; i < nb_svd_functions; i++)
962  of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
963  << "\t";
964 #endif
965  }
966  of << std::endl;
967  }
968 
969  int ret_default = EXIT_SUCCESS;
970  int ret_lapack = EXIT_SUCCESS;
971  int ret_eigen3 = EXIT_SUCCESS;
972  int ret_opencv = EXIT_SUCCESS;
973 
974  for (unsigned int iter = 0; iter < nb_iterations; iter++) {
975 
976  if (use_plot_file)
977  of << iter << "\t";
978 
979  for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
980  std::vector<vpMatrix> bench_random_matrices;
981  create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, bench_random_matrices);
982 
983  ret_default += test_pseudo_inverse_default(verbose, bench_random_matrices, time);
984  save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
985 
986 #if defined(VISP_HAVE_LAPACK)
987  ret_lapack += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time);
988  save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
989 #endif
990 
991 #if defined(VISP_HAVE_EIGEN3)
992  ret_eigen3 += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time);
993  save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
994 #endif
995 
996 #if defined(VISP_HAVE_OPENCV)
997  ret_opencv += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time);
998  save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
999 #endif
1000  }
1001  if (use_plot_file)
1002  of << std::endl;
1003  }
1004  if (use_plot_file) {
1005  of.close();
1006  std::cout << "Result saved in " << plotfile << std::endl;
1007  }
1008 
1009  std::cout << "Resume testing:" << std::endl;
1010  std::cout << " Pseudo-inverse (default): " << (ret_default ? "failed" : "success") << std::endl;
1011 #if defined(VISP_HAVE_LAPACK)
1012  std::cout << " Pseudo-inverse (lapack) : " << (ret_lapack ? "failed" : "success") << std::endl;
1013 #endif
1014 #if defined(VISP_HAVE_EIGEN3)
1015  std::cout << " Pseudo-inverse (eigen3) : " << (ret_eigen3 ? "failed" : "success") << std::endl;
1016 #endif
1017 #if defined(VISP_HAVE_OPENCV)
1018  std::cout << " Pseudo-inverse (opencv) : " << (ret_opencv ? "failed" : "success") << std::endl;
1019 #endif
1020 
1021  int ret = ret_default + ret_lapack + ret_eigen3 + ret_opencv;
1022 
1023  std::cout << " Global test : " << (ret ? "failed" : "success") << std::endl;
1024 
1025  return ret;
1026 #else
1027  (void)argc;
1028  (void)argv;
1029  std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl;
1030  return EXIT_SUCCESS;
1031 #endif
1032  }
1033  catch (const vpException &e) {
1034  std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
1035  return EXIT_FAILURE;
1036  }
1037 }
unsigned int getCols() const
Definition: vpArray2D.h:337
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
error that can be emitted by ViSP classes.
Definition: vpException.h:60
const std::string & getStringMessage() const
Definition: vpException.cpp:67
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
double frobeniusNorm() const
Definition: vpMatrix.cpp:1894
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:1133
vpMatrix transpose() const
vpMatrix t() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:70
VISP_EXPORT double measureTimeMs()