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