Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
testSvd.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 inversions\n\
68 using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
69 Outputs a comparison of these methods.\n\
70 \n\
71 SYNOPSIS\n\
72  %s [-n <number of matrices>] [-f <plot filename>]\n\
73  [-R <number of rows>] [-C <number of columns>]\n\
74  [-i <number of iterations>] [-p] [-h]\n",
75  name);
76 
77  fprintf(stdout, "\n\
78 OPTIONS: Default\n\
79  -n <number of matrices> \n\
80  Number of matrices inverted during each test loop.\n\
81 \n\
82  -i <number of iterations> \n\
83  Number of iterations of the test.\n\
84 \n\
85  -f <plot filename> \n\
86  Set output path for plot output.\n\
87  The plot logs the times of \n\
88  the different inversion methods: \n\
89  QR,LU,Cholesky and Pseudo-inverse.\n\
90 \n\
91  -R <number of rows>\n\
92  Number of rows of the automatically generated matrices \n\
93  we test on.\n\
94 \n\
95  -C <number of columns>\n\
96  Number of colums of the automatically generated matrices \n\
97  we test on.\n\
98 \n\
99  -p \n\
100  Plot into filename in the gnuplot format. \n\
101  If this option is used, tests results will be logged \n\
102  into a filename specified with -f.\n\
103 \n\
104  -h\n\
105  Print the help.\n\n");
106 
107  if (badparam) {
108  fprintf(stderr, "ERROR: \n");
109  fprintf(stderr, "\nBad parameter [%s]\n", badparam);
110  }
111 }
112 
120 bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
121  bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
122 {
123  const char *optarg_;
124  int c;
125  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
126 
127  switch (c) {
128  case 'h':
129  usage(argv[0], nullptr);
130  return false;
131  break;
132  case 'n':
133  nb_matrices = (unsigned int)atoi(optarg_);
134  break;
135  case 'i':
136  nb_iterations = (unsigned int)atoi(optarg_);
137  break;
138  case 'f':
139  plotfile = optarg_;
140  use_plot_file = true;
141  break;
142  case 'p':
143  use_plot_file = true;
144  break;
145  case 'R':
146  nbrows = (unsigned int)atoi(optarg_);
147  break;
148  case 'C':
149  nbcols = (unsigned int)atoi(optarg_);
150  break;
151  case 'v':
152  verbose = true;
153  break;
154  // add default options -c -d
155  case 'c':
156  break;
157  case 'd':
158  break;
159  default:
160  usage(argv[0], optarg_);
161  return false;
162  break;
163  }
164  }
165 
166  if ((c == 1) || (c == -1)) {
167  // standalone param or error
168  usage(argv[0], nullptr);
169  std::cerr << "ERROR: " << std::endl;
170  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
171  return false;
172  }
173 
174  return true;
175 }
176 
177 vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
178 {
179  vpMatrix A;
180  A.resize(nbrows, nbcols);
181 
182  for (unsigned int i = 0; i < A.getRows(); i++) {
183  for (unsigned int j = 0; j < A.getCols(); j++) {
184  A[i][j] = static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
185  }
186  }
187 
188  return A;
189 }
190 
191 vpMatrix make_random_symmetric_matrix(unsigned int nbrows)
192 {
193  vpMatrix A;
194  A.resize(nbrows, nbrows);
195 
196  for (unsigned int i = 0; i < A.getRows(); i++) {
197  for (unsigned int j = i; j < A.getCols(); j++) {
198  A[i][j] = static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
199  if (i != j) {
200  A[j][i] = A[i][j];
201  }
202  }
203  }
204 
205  return A;
206 }
207 
208 void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
209  std::vector<vpMatrix> &bench)
210 {
211  if (verbose)
212  std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl;
213  bench.clear();
214  for (unsigned int i = 0; i < nb_matrices; i++) {
215  vpMatrix M;
216  //#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) ||
217  //(VISP_HAVE_OPENCV_VERSION >= 0x020101)
218  // double det = 0.;
219  // // don't put singular matrices in the benchmark
220  // for(M = make_random_matrix(nb_rows, nb_cols);
221  // std::fabs(det=M.AtA().det())<.01; M = make_random_matrix(nb_rows,
222  // nb_cols)) {
223  // if(verbose) {
224  // std::cout << " Generated random matrix AtA=" << std::endl <<
225  // M.AtA() << std::endl; std::cout << " Generated random matrix
226  // not invertible: det=" << det << ". Retrying..." << std::endl;
227  // }
228  // }
229  //#else
230  M = make_random_matrix(nb_rows, nb_cols);
231  //#endif
232  bench.push_back(M);
233  }
234 }
235 
236 void create_bench_random_symmetric_matrix(unsigned int nb_matrices, unsigned int nb_rows, bool verbose,
237  std::vector<vpMatrix> &bench)
238 {
239  if (verbose)
240  std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_rows << " symmetric matrices" << std::endl;
241  bench.clear();
242  for (unsigned int i = 0; i < nb_matrices; i++) {
243  vpMatrix M;
244  //#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) ||
245  //(VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL)
246  // double det = 0.;
247  // // don't put singular matrices in the benchmark
248  // for(M = make_random_matrix(nb_rows, nb_cols);
249  // std::fabs(det=M.AtA().det())<.01; M = make_random_matrix(nb_rows,
250  // nb_cols)) {
251  // if(verbose) {
252  // std::cout << " Generated random matrix AtA=" << std::endl <<
253  // M.AtA() << std::endl; std::cout << " Generated random matrix
254  // not invertible: det=" << det << ". Retrying..." << std::endl;
255  // }
256  // }
257  //#else
258  M = make_random_symmetric_matrix(nb_rows);
259  //#endif
260  bench.push_back(M);
261  }
262 }
263 
264 int test_svd(std::vector<vpMatrix> M, std::vector<vpMatrix> U, std::vector<vpColVector> s, std::vector<vpMatrix> V, double &error)
265 {
266  for (unsigned int i = 0; i < M.size(); i++) {
267  vpMatrix S;
268  S.diag(s[i]);
269  vpMatrix U_S_Vt = U[i] * S * V[i].t();
270  vpMatrix D = M[i] - U_S_Vt;
271  error = D.frobeniusNorm();
272  if (error > 1e-6) {
273  std::cout << "SVD decomposition failed. Error: " << error << std::endl;
274  return EXIT_FAILURE;
275  }
276  }
277  return EXIT_SUCCESS;
278 }
279 
280 int test_eigen_values(std::vector<vpMatrix> M, std::vector<vpColVector> e, std::vector<vpMatrix> V,
281  std::vector<vpColVector> e2)
282 {
283  for (unsigned int i = 0; i < M.size(); i++) {
284  vpColVector error_e = e[i] - e2[i];
285  if (error_e.frobeniusNorm() > 1e-6) {
286  std::cout << "Eigen values differ" << std::endl;
287  return EXIT_FAILURE;
288  }
289  vpMatrix D;
290  D.diag(e[i]);
291  vpMatrix MV_VD = M[i] * V[i] - V[i] * D;
292  if (MV_VD.frobeniusNorm() > 1e-6) {
293  std::cout << "Eigen values/vector decomposition failed" << std::endl;
294  return EXIT_FAILURE;
295  }
296  }
297  return EXIT_SUCCESS;
298 }
299 
300 #if defined(VISP_HAVE_EIGEN3)
301 int test_svd_eigen3(bool verbose, const std::vector<vpMatrix> &bench, double &time, double &error)
302 {
303  if (verbose)
304  std::cout << "Test SVD using Eigen3 3rd party" << std::endl;
305  // Compute inverse
306  if (verbose)
307  std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
308 
309  std::vector<vpMatrix> U = bench;
310  std::vector<vpMatrix> V(bench.size());
311  std::vector<vpColVector> s(bench.size());
312 
313  double t = vpTime::measureTimeMs();
314  for (unsigned int i = 0; i < bench.size(); i++) {
315  U[i].svdEigen3(s[i], V[i]);
316  }
317 
318  time = vpTime::measureTimeMs() - t;
319 
320  return test_svd(bench, U, s, V, error);
321 }
322 #endif
323 
324 #if defined(VISP_HAVE_LAPACK)
325 int test_svd_lapack(bool verbose, const std::vector<vpMatrix> &bench, double &time, double &error)
326 {
327  if (verbose)
328  std::cout << "Test SVD using Lapack 3rd party" << std::endl;
329  // Compute inverse
330  if (verbose)
331  std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
332 
333  std::vector<vpMatrix> U = bench;
334  std::vector<vpMatrix> V(bench.size());
335  std::vector<vpColVector> s(bench.size());
336 
337  double t = vpTime::measureTimeMs();
338  for (unsigned int i = 0; i < bench.size(); i++) {
339  U[i].svdLapack(s[i], V[i]);
340  }
341  time = vpTime::measureTimeMs() - t;
342 
343  return test_svd(bench, U, s, V, error);
344 }
345 
346 int test_eigen_values_lapack(bool verbose, const std::vector<vpMatrix> &bench, double &time)
347 {
348  if (verbose)
349  std::cout << "Test eigenValues() using Lapack 3rd party" << std::endl;
350 
351  std::vector<vpColVector> e(bench.size());
352  std::vector<vpColVector> e2(bench.size());
353  std::vector<vpMatrix> V(bench.size());
354 
355  for (unsigned int i = 0; i < bench.size(); i++) {
356  e2[i] = bench[i].eigenValues();
357  }
358 
359  // Compute the eigenvalues and eigenvectors
360  double t = vpTime::measureTimeMs();
361  for (unsigned int i = 0; i < bench.size(); i++) {
362  bench[i].eigenValues(e[i], V[i]);
363  }
364  time = vpTime::measureTimeMs() - t;
365 
366  return test_eigen_values(bench, e, V, e2);
367 }
368 #endif
369 
370 #if defined(VISP_HAVE_OPENCV)
371 int test_svd_opencv(bool verbose, const std::vector<vpMatrix> &bench, double &time, double &error)
372 {
373  if (verbose)
374  std::cout << "Test SVD using OpenCV 3rd party" << std::endl;
375  // Compute inverse
376  if (verbose)
377  std::cout << " SVD on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
378 
379  std::vector<vpMatrix> U = bench;
380  std::vector<vpMatrix> V(bench.size());
381  std::vector<vpColVector> s(bench.size());
382 
383  double t = vpTime::measureTimeMs();
384  for (unsigned int i = 0; i < bench.size(); i++) {
385  U[i].svdOpenCV(s[i], V[i]);
386  }
387  time = vpTime::measureTimeMs() - t;
388 
389  return test_svd(bench, U, s, V, error);
390 }
391 #endif
392 
393 void save_time(const std::string &method, bool verbose, bool use_plot_file, std::ofstream &of, double time, double error)
394 {
395  if (use_plot_file)
396  of << time << "\t";
397  if (verbose || !use_plot_file) {
398  std::cout << method << "took " << time << "s, error = " << error << std::endl;
399  }
400 }
401 
402 bool testAllSvds(const std::string &test_name, unsigned nb_matrices, unsigned nb_iterations,
403  unsigned nb_rows, unsigned nb_cols,
404  bool doEigenValues, bool verbose, bool use_plot_file, std::ofstream &of)
405 {
406  int ret = EXIT_SUCCESS;
407  int ret_test = 0;
408  for (unsigned int iter = 0; iter < nb_iterations; iter++) {
409  std::cout << "\n-> Iteration: " << iter << std::endl;
410  std::vector<vpMatrix> bench_random_matrices;
411  create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices);
412  std::vector<vpMatrix> bench_random_symmetric_matrices;
413  create_bench_random_symmetric_matrix(nb_matrices, nb_rows, verbose, bench_random_symmetric_matrices);
414 
415  if (use_plot_file)
416  of << test_name << iter << "\t";
417  double time;
418  double error;
419 
420 #if defined(VISP_HAVE_LAPACK)
421  std::cout << "\n-- Test SVD using lapack" << std::endl;
422  ret_test = test_svd_lapack(verbose, bench_random_matrices, time, error);
423  ret += ret_test;
424  std::cout << test_name << ": SVD (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl;
425  save_time("SVD (Lapack): ", verbose, use_plot_file, of, time, error);
426 #endif
427 
428 #if defined(VISP_HAVE_EIGEN3)
429  std::cout << "\n-- Test SVD using eigen" << std::endl;
430  ret_test = test_svd_eigen3(verbose, bench_random_matrices, time, error);
431  ret += ret_test;
432  std::cout << test_name << ": SVD (Eigen) " << (ret_test ? "failed" : "succeed") << std::endl;
433  save_time("SVD (Eigen3): ", verbose, use_plot_file, of, time, error);
434 #endif
435 
436 #if defined(VISP_HAVE_OPENCV)
437  std::cout << "\n-- Test SVD using OpenCV" << std::endl;
438  ret_test = test_svd_opencv(verbose, bench_random_matrices, time, error);
439  ret += ret_test;
440  std::cout << test_name << ": SVD (OpenCV) " << (ret_test ? "failed" : "succeed") << std::endl;
441  save_time("SVD (OpenCV): ", verbose, use_plot_file, of, time, error);
442 #endif
443 
444 #if defined(VISP_HAVE_LAPACK)
445  if (doEigenValues) {
446  std::cout << "\n-- Test Eigen Values using lapack" << std::endl;
447  ret_test = test_eigen_values_lapack(verbose, bench_random_symmetric_matrices, time);
448  ret += ret_test;
449  std::cout << "Eigen values (Lapack) " << (ret_test ? "failed" : "succeed") << std::endl;
450  error = 0.0;
451  save_time("Eigen values (Lapack): ", verbose, use_plot_file, of, time, error);
452  }
453 #endif
454  std::cout << "Result after iteration " << iter << ": " << (ret ? "failed" : "succeed") << std::endl;
455  if (use_plot_file)
456  of << std::endl;
457  }
458  return (ret == EXIT_SUCCESS);
459 }
460 
461 int main(int argc, const char *argv[])
462 {
463  try {
464 #if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV)
465  unsigned int nb_matrices = 100;
466  unsigned int nb_iterations = 10;
467  unsigned int nb_rows = 6;
468  unsigned int nb_cols = 6;
469  bool verbose = false;
470  std::string plotfile("plot-svd.csv");
471  bool use_plot_file = false;
472  std::ofstream of;
473 
474  // Read the command line options
475  if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) ==
476  false) {
477  return EXIT_FAILURE;
478  }
479 
480  if (use_plot_file) {
481  of.open(plotfile.c_str());
482  of << "iter"
483  << "\t";
484 
485 #if defined(VISP_HAVE_LAPACK)
486  of << "\"SVD Lapack\""
487  << "\t";
488 #endif
489 #if defined(VISP_HAVE_EIGEN3)
490  of << "\"SVD Eigen3\""
491  << "\t";
492 #endif
493 #if defined(VISP_HAVE_OPENCV)
494  of << "\"SVD OpenCV\""
495  << "\t";
496 #endif
497  of << std::endl;
498  }
499  bool success = true;
500  std::string test_case;
501  test_case = "Test case: Square matrices";
502  std::cout << "\n== " << test_case << ": " << nb_rows << " x " << nb_cols << " ==" << std::endl;
503  bool defaultSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_rows, nb_cols,
504  true, verbose, use_plot_file, of);
505  std::cout << "=> " << test_case << ": " << (defaultSuccess ? "succeed" : "failed") << std::endl;
506 
507  test_case = "Test case: More rows than columns";
508  std::cout << "\n== " << test_case << ": " << nb_cols * 2 << " x " << nb_cols << " ==" << std::endl;
509  bool rowsSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_cols * 2, nb_cols,
510  false, verbose, use_plot_file, of);
511  std::cout << "=> " << test_case << ": " << (rowsSuccess ? "succeed" : "failed") << std::endl;
512 
513  test_case = "Test case: More columns than rows";
514  std::cout << "\n== " << test_case << ": " << nb_rows << " x " << nb_rows * 2 << " ==" << std::endl;
515  bool colsSuccess = testAllSvds(test_case, nb_matrices, nb_iterations, nb_rows, nb_rows * 2,
516  false, verbose, use_plot_file, of);
517  std::cout << "=> " << test_case << ": " << (colsSuccess ? "succeed" : "failed") << std::endl;
518 
519  std::cout << "\nResume:" << std::endl;
520  std::cout << "- Square matrices (" << nb_rows << "x" << nb_cols << "): " << (defaultSuccess ? "succeed" : "failed") << std::endl;
521 
522  std::cout << "- More rows case (" << nb_cols * 2 << "x" << nb_cols << "): " << (rowsSuccess ? "succeed" : "failed") << std::endl;
523 
524  std::cout << "- More columns case (" << nb_rows << "x" << nb_rows * 2 << "): " << (colsSuccess ? "succeed" : "failed") << std::endl;
525 
526  success = defaultSuccess && rowsSuccess && colsSuccess;
527 
528  if (use_plot_file) {
529  of.close();
530  std::cout << "Result saved in " << plotfile << std::endl;
531  }
532 
533  if (success) {
534  std::cout << "Test succeed" << std::endl;
535  }
536  else {
537  std::cout << "Test failed" << std::endl;
538  }
539 
540  return success ? EXIT_SUCCESS : EXIT_FAILURE;
541 #else
542  (void)argc;
543  (void)argv;
544  std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl;
545  return EXIT_SUCCESS;
546 #endif
547  }
548  catch (const vpException &e) {
549  std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
550  return EXIT_FAILURE;
551  }
552 }
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
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
double frobeniusNorm() const
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
void diag(const double &val=1.0)
double frobeniusNorm() const
Definition: vpMatrix.cpp:1894
vpMatrix t() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:70
VISP_EXPORT double measureTimeMs()