Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
62void usage(const char *name, const char *badparam)
63{
64 fprintf(stdout, "\n\
65Test matrix inversions\n\
66using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
67Outputs a comparison of these methods.\n\
68\n\
69SYNOPSIS\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\
76OPTIONS: 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
118bool 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], NULL);
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], NULL);
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
175vpMatrix 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
189vpMatrix 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
206void 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
234void 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
263int 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
278int 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)
299int 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)
323int 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
344int 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)
369int 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
391void 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
400int 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:280
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
double frobeniusNorm() const
error that can be emitted by ViSP classes.
Definition vpException.h:59
const std::string & getStringMessage() const
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
void diag(const double &val=1.0)
Definition vpMatrix.cpp:881
vpMatrix t() const
Definition vpMatrix.cpp:461
double frobeniusNorm() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
VISP_EXPORT double measureTimeMs()