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