Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testMatrixPseudoInverse.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 pseudo-inverse.\n\
66Outputs a comparison of the results obtained by supported 3rd parties.\n\
67\n\
68SYNOPSIS\n\
69 %s [-n <number of matrices>] [-f <plot filename>]\n\
70 [-R <number of rows>] [-C <number of columns>]\n\
71 [-i <number of iterations>] [-p] [-h]\n",
72 name);
73
74 fprintf(stdout, "\n\
75OPTIONS: Default\n\
76 -n <number of matrices> \n\
77 Number of matrices inverted during each test loop.\n\
78\n\
79 -i <number of iterations> \n\
80 Number of iterations of the test.\n\
81\n\
82 -f <plot filename> \n\
83 Set output path for plot output.\n\
84 The plot logs the times of \n\
85 the different inversion methods: \n\
86 QR,LU,Cholesky and Pseudo-inverse.\n\
87\n\
88 -R <number of rows>\n\
89 Number of rows of the automatically generated matrices \n\
90 we test on.\n\
91\n\
92 -C <number of columns>\n\
93 Number of colums of the automatically generated matrices \n\
94 we test on.\n\
95\n\
96 -p \n\
97 Plot into filename in the gnuplot format. \n\
98 If this option is used, tests results will be logged \n\
99 into a filename specified with -f.\n\
100\n\
101 -h\n\
102 Print the help.\n\n");
103
104 if (badparam) {
105 fprintf(stderr, "ERROR: \n");
106 fprintf(stderr, "\nBad parameter [%s]\n", badparam);
107 }
108}
109
117bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
118 bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
119{
120 const char *optarg_;
121 int c;
122 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
123
124 switch (c) {
125 case 'h':
126 usage(argv[0], NULL);
127 return false;
128 break;
129 case 'n':
130 nb_matrices = (unsigned int)atoi(optarg_);
131 break;
132 case 'i':
133 nb_iterations = (unsigned int)atoi(optarg_);
134 break;
135 case 'f':
136 plotfile = optarg_;
137 use_plot_file = true;
138 break;
139 case 'p':
140 use_plot_file = true;
141 break;
142 case 'R':
143 nbrows = (unsigned int)atoi(optarg_);
144 break;
145 case 'C':
146 nbcols = (unsigned int)atoi(optarg_);
147 break;
148 case 'v':
149 verbose = true;
150 break;
151 // add default options -c -d
152 case 'c':
153 break;
154 case 'd':
155 break;
156 default:
157 usage(argv[0], optarg_);
158 return false;
159 break;
160 }
161 }
162
163 if ((c == 1) || (c == -1)) {
164 // standalone param or error
165 usage(argv[0], NULL);
166 std::cerr << "ERROR: " << std::endl;
167 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
168 return false;
169 }
170
171 return true;
172}
173
174vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
175{
176 vpMatrix A;
177 A.resize(nbrows, nbcols);
178
179 for (unsigned int i = 0; i < A.getRows(); i++) {
180 for (unsigned int j = 0; j < A.getCols(); j++) {
181 A[i][j] = (double)rand() / (double)RAND_MAX;
182 }
183 }
184
185 return A;
186}
187
188void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
189 std::vector<vpMatrix> &bench)
190{
191 if (verbose)
192 std::cout << "Create a bench of " << nb_matrices << " " << nb_rows << " by " << nb_cols << " matrices" << std::endl;
193 bench.clear();
194 for (unsigned int i = 0; i < nb_matrices; i++) {
195 vpMatrix M = make_random_matrix(nb_rows, nb_cols);
196 bench.push_back(M);
197 }
198}
199
200int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api)
201{
202 double allowed_error = 1e-3;
203
204 for (unsigned int i = 0; i < A.size(); i++) {
205 double error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm();
206 if (error > allowed_error) {
207 std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl;
208 return EXIT_FAILURE;
209 }
210 }
211 return EXIT_SUCCESS;
212}
213
214int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api,
215 const std::vector<vpColVector> &sv, const std::vector<vpMatrix> &imA,
216 const std::vector<vpMatrix> &imAt, const std::vector<vpMatrix> &kerAt)
217{
218 double allowed_error = 1e-3;
219 // test Api
220 for (unsigned int i = 0; i < A.size(); i++) {
221 double error = (A[i] * Api[i] * A[i] - A[i]).frobeniusNorm();
222 if (error > allowed_error) {
223 std::cout << "Bad pseudo-inverse [" << i << "]: euclidean norm: " << error << std::endl;
224 return EXIT_FAILURE;
225 }
226 }
227
228 // test kerA
229 for (unsigned int i = 0; i < kerAt.size(); i++) {
230 if (kerAt[i].size()) {
231 vpMatrix nullspace = A[i] * kerAt[i].t();
232 double error = nullspace.frobeniusNorm();
233
234 if (error > allowed_error) {
235 std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl;
236 return EXIT_FAILURE;
237 }
238 }
239 }
240
241 // test sv, imA, imAt, kerA
242 for (unsigned int i = 0; i < kerAt.size(); i++) {
243 unsigned int rank = imA[i].getCols();
244 vpMatrix U, S(rank, A[i].getCols()), Vt(A[i].getCols(), A[i].getCols());
245 U = imA[i];
246
247 for (unsigned int j = 0; j < rank; j++)
248 S[j][j] = sv[i][j];
249
250 Vt.insert(imAt[i].t(), 0, 0);
251 Vt.insert(kerAt[i], imAt[i].getCols(), 0);
252
253 double error = (U * S * Vt - A[i]).frobeniusNorm();
254
255 if (error > allowed_error) {
256 std::cout << "Bad imA, imAt, sv, kerAt [" << i << "]: euclidean norm: " << error << std::endl;
257 return EXIT_FAILURE;
258 }
259 }
260
261 return EXIT_SUCCESS;
262}
263
264int test_pseudo_inverse_default(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
265{
266 if (verbose)
267 std::cout << "Test pseudo-inverse using default 3rd party" << std::endl;
268 if (verbose)
269 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
270
271 size_t size = bench.size();
272 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
273 std::vector<vpColVector> sv(size);
274 int ret = EXIT_SUCCESS;
275
276 // test 0
277 unsigned int test = 0;
278 double t = vpTime::measureTimeMs();
279 for (unsigned int i = 0; i < bench.size(); i++) {
280 PI[i] = bench[i].pseudoInverse();
281 }
282 time[test] = vpTime::measureTimeMs() - t;
283 for (unsigned int i = 0; i < time.size(); i++) {
284 ret += test_pseudo_inverse(bench, PI);
285 }
286
287 // test 1
288 test++;
290 for (unsigned int i = 0; i < bench.size(); i++) {
291 bench[i].pseudoInverse(PI[i]);
292 }
293 time[test] = vpTime::measureTimeMs() - t;
294 for (unsigned int i = 0; i < time.size(); i++) {
295 ret += test_pseudo_inverse(bench, PI);
296 }
297
298 // test 2
299 test++;
301 for (unsigned int i = 0; i < bench.size(); i++) {
302 bench[i].pseudoInverse(PI[i], sv[i]);
303 }
304 time[test] = vpTime::measureTimeMs() - t;
305 for (unsigned int i = 0; i < time.size(); i++) {
306 ret += test_pseudo_inverse(bench, PI);
307 }
308
309 // test 3
310 test++;
312 for (unsigned int i = 0; i < bench.size(); i++) {
313 bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
314 }
315 time[test] = vpTime::measureTimeMs() - t;
316
317 for (unsigned int i = 0; i < time.size(); i++) {
318 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
319 }
320
321 return ret;
322}
323
324#if defined(VISP_HAVE_EIGEN3)
325int test_pseudo_inverse_eigen3(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
326{
327 if (verbose)
328 std::cout << "Test pseudo-inverse using Eigen3 3rd party" << std::endl;
329 if (verbose)
330 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
331
332 size_t size = bench.size();
333 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
334 std::vector<vpColVector> sv(size);
335 int ret = EXIT_SUCCESS;
336
337 // test 0
338 unsigned int test = 0;
339 double t = vpTime::measureTimeMs();
340 for (unsigned int i = 0; i < bench.size(); i++) {
341 PI[i] = bench[i].pseudoInverseEigen3();
342 }
343 time[test] = vpTime::measureTimeMs() - t;
344 for (unsigned int i = 0; i < time.size(); i++) {
345 ret += test_pseudo_inverse(bench, PI);
346 }
347
348 // test 1
349 test++;
351 for (unsigned int i = 0; i < bench.size(); i++) {
352 bench[i].pseudoInverseEigen3(PI[i]);
353 }
354 time[test] = vpTime::measureTimeMs() - t;
355 for (unsigned int i = 0; i < time.size(); i++) {
356 ret += test_pseudo_inverse(bench, PI);
357 }
358
359 // test 2
360 test++;
362 for (unsigned int i = 0; i < bench.size(); i++) {
363 bench[i].pseudoInverseEigen3(PI[i], sv[i]);
364 }
365 time[test] = vpTime::measureTimeMs() - t;
366 for (unsigned int i = 0; i < time.size(); i++) {
367 ret += test_pseudo_inverse(bench, PI);
368 }
369
370 // test 3
371 test++;
373 for (unsigned int i = 0; i < bench.size(); i++) {
374 bench[i].pseudoInverseEigen3(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
375 }
376 time[test] = vpTime::measureTimeMs() - t;
377
378 for (unsigned int i = 0; i < time.size(); i++) {
379 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
380 }
381
382 return ret;
383}
384#endif
385
386#if defined(VISP_HAVE_LAPACK)
387int test_pseudo_inverse_lapack(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
388{
389 if (verbose)
390 std::cout << "Test pseudo-inverse using Lapack 3rd party" << std::endl;
391 if (verbose)
392 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
393
394 size_t size = bench.size();
395 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
396 std::vector<vpColVector> sv(size);
397 int ret = EXIT_SUCCESS;
398
399 // test 0
400 unsigned int test = 0;
401 double t = vpTime::measureTimeMs();
402 for (unsigned int i = 0; i < bench.size(); i++) {
403 PI[i] = bench[i].pseudoInverseLapack();
404 }
405 time[test] = vpTime::measureTimeMs() - t;
406 for (unsigned int i = 0; i < time.size(); i++) {
407 ret += test_pseudo_inverse(bench, PI);
408 }
409
410 // test 1
411 test++;
413 for (unsigned int i = 0; i < bench.size(); i++) {
414 bench[i].pseudoInverseLapack(PI[i]);
415 }
416 time[test] = vpTime::measureTimeMs() - t;
417 for (unsigned int i = 0; i < time.size(); i++) {
418 ret += test_pseudo_inverse(bench, PI);
419 }
420
421 // test 2
422 test++;
424 for (unsigned int i = 0; i < bench.size(); i++) {
425 bench[i].pseudoInverseLapack(PI[i], sv[i]);
426 }
427 time[test] = vpTime::measureTimeMs() - t;
428 for (unsigned int i = 0; i < time.size(); i++) {
429 ret += test_pseudo_inverse(bench, PI);
430 }
431
432 // test 3
433 test++;
435 for (unsigned int i = 0; i < bench.size(); i++) {
436 bench[i].pseudoInverseLapack(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
437 }
438 time[test] = vpTime::measureTimeMs() - t;
439
440 for (unsigned int i = 0; i < time.size(); i++) {
441 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
442 }
443
444 return ret;
445}
446#endif
447
448#if defined(VISP_HAVE_OPENCV)
449int test_pseudo_inverse_opencv(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &time)
450{
451 if (verbose)
452 std::cout << "Test pseudo-inverse using OpenCV 3rd party" << std::endl;
453 if (verbose)
454 std::cout << " Pseudo-inverse on a " << bench[0].getRows() << "x" << bench[0].getCols() << " matrix" << std::endl;
455
456 size_t size = bench.size();
457 std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
458 std::vector<vpColVector> sv(size);
459 int ret = EXIT_SUCCESS;
460
461 // test 0
462 unsigned int test = 0;
463 double t = vpTime::measureTimeMs();
464 for (unsigned int i = 0; i < bench.size(); i++) {
465 PI[i] = bench[i].pseudoInverseOpenCV();
466 }
467 time[test] = vpTime::measureTimeMs() - t;
468 for (unsigned int i = 0; i < time.size(); i++) {
469 ret += test_pseudo_inverse(bench, PI);
470 }
471
472 // test 1
473 test++;
475 for (unsigned int i = 0; i < bench.size(); i++) {
476 bench[i].pseudoInverseOpenCV(PI[i]);
477 }
478 time[test] = vpTime::measureTimeMs() - t;
479 for (unsigned int i = 0; i < time.size(); i++) {
480 ret += test_pseudo_inverse(bench, PI);
481 }
482
483 // test 2
484 test++;
486 for (unsigned int i = 0; i < bench.size(); i++) {
487 bench[i].pseudoInverseOpenCV(PI[i], sv[i]);
488 }
489 time[test] = vpTime::measureTimeMs() - t;
490 for (unsigned int i = 0; i < time.size(); i++) {
491 ret += test_pseudo_inverse(bench, PI);
492 }
493
494 // test 3
495 test++;
497 for (unsigned int i = 0; i < bench.size(); i++) {
498 bench[i].pseudoInverseOpenCV(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
499 }
500 time[test] = vpTime::measureTimeMs() - t;
501
502 for (unsigned int i = 0; i < time.size(); i++) {
503 ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
504 }
505
506 return ret;
507}
508#endif
509
510void save_time(const std::string &method, unsigned int nrows, unsigned int ncols, bool verbose, bool use_plot_file,
511 std::ofstream &of, const std::vector<double> &time)
512{
513 for (size_t i = 0; i < time.size(); i++) {
514 if (use_plot_file)
515 of << time[i] << "\t";
516 if (verbose) {
517 std::cout << " " << method << " svd(" << nrows << "x" << ncols << ")"
518 << " test " << i << ": " << time[i] << std::endl;
519 }
520 }
521}
522
523int main(int argc, const char *argv[])
524{
525 try {
526#if defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_OPENCV)
527 unsigned int nb_matrices = 10;
528 unsigned int nb_iterations = 10;
529 unsigned int nb_rows = 12;
530 unsigned int nb_cols = 6;
531 bool verbose = false;
532 std::string plotfile("plot-pseudo-inv.csv");
533 bool use_plot_file = false;
534 std::ofstream of;
535
536 unsigned int nb_svd_functions = 4; // 4 tests for each existing vpMatrix::pseudoInverse(...) functions
537 unsigned int nb_test_matrix_size = 3; // 3 tests: m > n, m = n, m < n
538 std::vector<double> time(nb_svd_functions);
539 std::vector<unsigned int> nrows(nb_test_matrix_size), ncols(nb_test_matrix_size);
540
541 // Read the command line options
542 if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) ==
543 false) {
544 return EXIT_FAILURE;
545 }
546
547 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
548 // consider m > n, m = n, m < n
549 if (s == 0) {
550 nrows[s] = nb_rows;
551 ncols[s] = nb_cols;
552 } else if (s == 1) {
553 nrows[s] = nb_cols;
554 ncols[s] = nb_cols;
555 } else {
556 nrows[s] = nb_cols;
557 ncols[s] = nb_rows;
558 }
559 }
560
561 if (use_plot_file) {
562 of.open(plotfile.c_str());
563 of << "iter"
564 << "\t";
565
566 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
567 for (unsigned int i = 0; i < nb_svd_functions; i++)
568 of << "\"default " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
569 << "\t";
570
571#if defined(VISP_HAVE_LAPACK)
572 for (unsigned int i = 0; i < nb_svd_functions; i++)
573 of << "\"Lapack " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
574 << "\t";
575#endif
576#if defined(VISP_HAVE_EIGEN3)
577 for (unsigned int i = 0; i < nb_svd_functions; i++)
578 of << "\"Eigen3 " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
579 << "\t";
580#endif
581#if defined(VISP_HAVE_OPENCV)
582 for (unsigned int i = 0; i < nb_svd_functions; i++)
583 of << "\"OpenCV " << nrows[s] << "x" << ncols[s] << " test " << i << "\""
584 << "\t";
585#endif
586 }
587 of << std::endl;
588 }
589
590 int ret = EXIT_SUCCESS;
591 for (unsigned int iter = 0; iter < nb_iterations; iter++) {
592
593 if (use_plot_file)
594 of << iter << "\t";
595
596 for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
597 std::vector<vpMatrix> bench_random_matrices;
598 create_bench_random_matrix(nb_matrices, nrows[s], ncols[s], verbose, bench_random_matrices);
599
600 ret += test_pseudo_inverse_default(verbose, bench_random_matrices, time);
601 save_time("default -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
602
603#if defined(VISP_HAVE_LAPACK)
604 ret += test_pseudo_inverse_lapack(verbose, bench_random_matrices, time);
605 save_time("Lapack -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
606#endif
607
608#if defined(VISP_HAVE_EIGEN3)
609 ret += test_pseudo_inverse_eigen3(verbose, bench_random_matrices, time);
610 save_time("Eigen3 -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
611#endif
612
613#if defined(VISP_HAVE_OPENCV)
614 ret += test_pseudo_inverse_opencv(verbose, bench_random_matrices, time);
615 save_time("OpenCV -", nrows[s], ncols[s], verbose, use_plot_file, of, time);
616#endif
617 }
618 if (use_plot_file)
619 of << std::endl;
620 }
621 if (use_plot_file) {
622 of.close();
623 std::cout << "Result saved in " << plotfile << std::endl;
624 }
625
626 if (ret == EXIT_SUCCESS) {
627 std::cout << "Test succeed" << std::endl;
628 } else {
629 std::cout << "Test failed" << std::endl;
630 }
631
632 return ret;
633#else
634 (void)argc;
635 (void)argv;
636 std::cout << "Test does nothing since you dont't have Lapack, Eigen3 or OpenCV 3rd party" << std::endl;
637 return EXIT_SUCCESS;
638#endif
639 } catch (const vpException &e) {
640 std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
641 return EXIT_FAILURE;
642 }
643}
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
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
vpMatrix t() const
Definition vpMatrix.cpp:461
double frobeniusNorm() const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
VISP_EXPORT double measureTimeMs()